Publish to wheels
Contents
Publish to wheels#
What you will need
A Duckietown robot turned ON and visible on
dts fleet discover
What you will get
Learn how to control the Duckiebot’s wheels using a ROS Publisher
Topic and message type of interest#
The topic used by the Duckiebot to receive wheel commands
is /ROBOT_NAME/wheels_driver_node/wheels_cmd
, while the message type used over this topic is
duckietown_msgs/WheelsCmdStamped which contains the following fields.
std_msgs/Header header
float32 vel_left
float32 vel_right
where,
header
: is the standard ROS header object;vel_left
: is the angular velocity inrad/s
of the left wheel;vel_right
: is the angular velocity inrad/s
of the right wheel;
Create Publisher ROS Node#
We now use our favorite text editor to create the file
wheel_control_node.py
inside the src/
directory of our catkin package and add the following content,
#!/usr/bin/env python3
import os
import math
import rospy
from duckietown.dtros import DTROS, NodeType
from duckietown_msgs.msg import WheelsCmdStamped
# angular velocities for each wheel (quarter rotation a second)
W_LEFT = 1/4 * (2 * math.pi)
W_RIGHT = 1/4 * (2 * math.pi)
class WheelControlNode(DTROS):
def __init__(self, node_name):
# initialize the DTROS parent class
super(WheelControlNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC)
# static parameters
vehicle_name = os.environ['VEHICLE_NAME']
wheels_topic = f"/{vehicle_name}/wheels_driver_node/wheels_cmd"
wheel_radius_param = f"/{vehicle_name}/kinematics_node/radius"
# get duckiebot's wheel radius
wheel_radius = rospy.get_param(wheel_radius_param)
# compute linear speeds
self._vel_left = W_LEFT * wheel_radius
self._vel_right = W_RIGHT * wheel_radius
# construct publisher
self._publisher = rospy.Publisher(wheels_topic, WheelsCmdStamped, queue_size=1)
def run(self):
# publish 10 messages every second (10 Hz)
rate = rospy.Rate(0.1)
message = WheelsCmdStamped(vel_left=self._vel_left, vel_right=self._vel_right)
while not rospy.is_shutdown():
self._publisher.publish(message)
rate.sleep()
def on_shutdown(self):
stop = WheelsCmdStamped(vel_left=0, vel_right=0)
self._publisher.publish(stop)
if __name__ == '__main__':
# create the node
node = WheelControlNode(node_name='wheel_control_node')
# run node
node.run()
# keep the process from terminating
rospy.spin()
Again, we make our node executable,
chmod +x ./packages/my_package/src/wheel_control_node.py
Define launcher#
We create a new launcher file ./launchers/wheel-control.sh
with the content,
#!/bin/bash
source /environment.sh
# initialize launch file
dt-launchfile-init
# launch subscriber
rosrun my_package wheel_control_node.py
# wait for app to end
dt-launchfile-join
Let us now re-compile our project using the command
dts devel build -f
Launch the node#
Danger
The robot’s wheels will start spinning as soon as the node is launched. Please, make sure that your robot has enough space to drive around without the risk of harming somebody or himself (e.g., by falling off a desk).
We run the node,
dts devel run -R ROBOT_NAME -L wheel-control
And observe the wheels rotate as instructed.
If you want to stop it, just use Ctrl+C
, and the wheels should stop spinning as per the behavior
defined in the function on_shutdown()
above.
Congratulations 🎉
You just built and run your first ROS node capable of interacting with the Duckiebot and control one of its actuators.