Node and Gazebo - publishing problem

asked 2016-06-14 04:34:19 -0500

almosca gravatar image

Hello guys, I created a controller velocity for moving the robotic base on gazebo. I create a simple node for moving the robot in gazebo sending command to the 4 wheels. I don't know why, but when I run rqt to check the links, i noticed that only one publisher is publishing on gazebo. this is the node graph:

image description

And this is the code:

    class cerob{

    public:
        float vel;

        cerob(){
            vel = 1.0; // rad/s
            velocity_fl = n.advertise<std_msgs::Float64>("/cerob/fl_wheel_velocity_controller/command", 10);
            velocity_fr = n.advertise<std_msgs::Float64>("/cerob/fr_wheel_velocity_controller/command", 10);
            velocity_rl = n.advertise<std_msgs::Float64>("/cerob/rl_wheel_velocity_controller/command", 10);
            velocity_rr = n.advertise<std_msgs::Float64>("/cerob/rr_wheel_velocity_controller/command", 10);
        }

        void keyboard_command(char command){
            std_msgs::Float64 velocity;
            if(command == 'w'){
                velocity.data = vel;
                velocity_fl.publish(velocity);
                velocity_fr.publish(velocity);
                velocity_rl.publish(velocity);
                velocity_rr.publish(velocity);
                ROS_INFO("FORWARD");
            }else if(command == 'a'){
                velocity.data = vel;
                velocity_fl.publish(velocity);
                velocity_fr.publish(velocity);
                velocity_rl.publish(velocity);
                velocity_rr.publish(velocity);
                ROS_INFO("LEFT");
            }else if(command == 's'){
                velocity.data = -vel;
                velocity_fl.publish(velocity);
                velocity_fr.publish(velocity);
                velocity_rl.publish(velocity);
                velocity_rr.publish(velocity);
                ROS_INFO("BACKWARD");
            }else if(command == 'd'){
                velocity.data = vel;
                velocity_fl.publish(velocity);
                velocity_fr.publish(velocity);
                velocity_rl.publish(velocity);
                velocity_rr.publish(velocity);
                ROS_INFO("RIGHT");
            }else if(command == 'x'){
                velocity.data = 0.0;
                velocity_fl.publish(velocity);
                velocity_fr.publish(velocity);
                velocity_rl.publish(velocity);
                velocity_rr.publish(velocity);
                ROS_INFO("stop");
            }
        }       

    private:
        ros::NodeHandle n;
        ros::Publisher velocity_fl, velocity_fr, velocity_rl, velocity_rr;

};


int main(int argc, char** argv){
    ros::init(argc, argv, "move_CeRob");
    char command;

    cerob base;
    while(ros::ok()){
        command = getch();
        base.keyboard_command(command);

        ros::spinOnce();    
    }

    return 0;
}

Thanks in advance.

edit retag flag offensive close merge delete