Robotics StackExchange | Archived questions

problem moving robot setting /gazebo/set_link_state

Hi, I am building a simple wheeled robot simulator. I build up a node that @10Hz set wheels speeds using the /gazebo/setlinkstate service. The cpp code is the following:

int main(int argc, char** argv){
    ros::init(argc, argv, "my_node");
    ros::NodeHandle n;
    ros::Rate loop_rate(10);
    ros::ServiceClient client;

    std::string leftWheelName   = (std::string)"robot::left_middle_wheel";

    geometry_msgs::Twist twist;
    twist.linear.x  = 0.0;
    twist.linear.y  = 0.0;
    twist.linear.z  = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 6.28/10;
    twist.angular.z = 0.0;

    gazebo::LinkState localLinkState;
    gazebo::GetLinkState getLinkState;
    gazebo::SetLinkState setLinkState;

    while (ros::ok()) {

        client = n.serviceClient<gazebo::GetLinkState>("/gazebo/get_link_state");
        getLinkState.request.link_name = leftWheelName;
        getLinkState.request.reference_frame = leftWheelName.c_str();
        client.call(getLinkState);

        localLinkState = getLinkState.response.link_state;
        localLinkState.twist = twist;
        localLinkState.reference_frame = leftWheelName.c_str();

        client = n.serviceClient<gazebo::SetLinkState>("/gazebo/set_link_state");
        setLinkState.request.link_state = localLinkState;
        client.call(setLinkState);

        ros::spinOnce();
        loop_rate.sleep();
   }
    return 0;
}

This seems to work but the real speed of the wheel is halved. I saw that only during half of the steps the wheel spins, during the other steps is stopped. Can someone help me?

Thank you!

Asked by forsand on 2012-04-05 01:27:45 UTC

Comments

Answers