-
Notifications
You must be signed in to change notification settings - Fork 2
Part 2 Twist Example
First, you need to import the rospy
library, as well as the Twist
message type from the geometry_msgs
library:
import rospy
from geometry_msgs.msg import Twist
Then, we create an instance of a rospy.Publisher()
and assign it to an object called pub
. When we create the object we tell the Publisher()
method which topic we want to publish this message to (via the first input argument), and also that we will be publishing a message of the Twist
type (the second input argument):
pub = rospy.Publisher([topic name], Twist, queue_size=[a queue size])
We can then create a Twist()
message instance and assign it to an object called vel_cmd
:
vel_cmd = Twist()
Using the $ rosmsg info geometry_msgs/Twist
command in a terminal window, we can learn that the geometry_msgs/Twist
message is of the format:
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
We also know, based on the differential drive configuration of our robot, that only velocity commands issued to:
geometry_msgs/Vector3 linear
float64 x
... or:
geometry_msgs/Vector3 angular
float64 z
... will have any effect on the velocity of our robot (remember this figure).
As such, we can set velocity values to the instance of the Twist()
message (assigned to vel_cmd
) as required:
vel_cmd.linear.x = 0.0 # m/s
vel_cmd.angular.z = 0.0 # rad/s
We can then publish this to the relevant topic on our ROS system by supplying it to the rospy.Publisher()
method that we assigned to pub
earlier:
pub.publish(vel_cmd)
ROS Training
UK-RAS Manufacturing Robotics Challenge 2021
Tom Howard & Alex Lucas | The University of Sheffield