Right way to move robots in Gazebo with ROS
Setup:
- Pioneer2DX in Gazebo world
- Test python script, which rotates robot's right wheel for a second, using ROS service "gazebo/applybodywrench":
#!/usr/bin/env python
#ros
import rospy
#msgs
from gazebo_msgs.srv import ApplyBodyWrench
#position
from geometry_msgs.msg import *
apply_wrench = rospy.ServiceProxy("gazebo/apply_body_wrench", ApplyBodyWrench)
wrench = Wrench()
wrench.torque.z = 30.0
apply_wrench("pioneer2dx::right_wheel", "", None, wrench, rospy.Time.from_sec(0), rospy.Duration.from_sec(1.0))
In the result robot moves a little, but the wheel does not change it's relative orientation to the robot. So, the force is created, but the robot rotates not because of the wheel's motion. Nevertheless, when I apply the same torque to the same wheel through Gazebo GUI, it rotates.
So, the questions are:
- Why does it happen?
- How to make robot move properly? (Maybe I am using the wrong method)
Asked by Entrack on 2017-02-26 05:13:47 UTC
Answers
I have solved the problem of moving the robot another way. But anyway maybe it will be helpful to someone else.
I don't know the reason of the issue with wheels, but moving pioneer2dx can be performed using the pluguin. You just add this to your .world file:
<model name='pioneer2dx'>
<!-- Your model code -->
<plugin name='differential_drive_controller'
filename='libgazebo_ros_diff_drive.so'>
<alwaysOn>true</alwaysOn>
<updateRate>100</updateRate>
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
<wheelSeparation>0.39</wheelSeparation>
<wheelDiameter>0.15</wheelDiameter>
<torque>5</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>chassis</robotBaseFrame>
</plugin>
</model>
Once your world is running, you can use this to drive your Pioneer2DX:
publisher = rospy.Publisher('/%s/cmd_vel' % topic, Twist, queue_size=8)
msg = Twist()
msg.linear.x = your_linear_speed
msg.angular.z = your_angular_speed
publisher.publish(msg)
Asked by Entrack on 2017-02-27 04:01:30 UTC
Comments