After uploading the embedded code to LaunchPad, the next step is to handle the serial data from LaunchPad and convert it to ROS topics for further processing. The launchpad_node.py ROS Python driver node interfaces Tiva-C LaunchPad with ROS. The launchpad_node.py file is in the script folder, which is inside the ChefBot_bringup package. The following is an explanation of the important code sections of launchpad_node.py:
#ROS Python client import rospy import sys import time import math #This python module helps to receive values from serial port which execute in a thread from SerialDataGateway import SerialDataGateway #Importing required ROS data types for the code from std_msgs.msg import Int16,Int32, Int64, Float32,
String, Header, UInt64 #Importing ROS data type for IMU from sensor_msgs.msg import Imu
The launchpad_node.py file imports the preceding modules. The main module we can see is SerialDataGateway. This is a custom module written to receive serial data from the LaunchPad board in a thread. We also need some data types of ROS to handle the sensor data. The main function of the node is given in the following code snippet:
if __name__ =='__main__':
rospy.init_node('launchpad_ros',anonymous=True)
launchpad = Launchpad_Class()
try:
launchpad.Start()
rospy.spin()
except rospy.ROSInterruptException:
rospy.logwarn("Error in main function")
launchpad.Reset_Launchpad()
launchpad.Stop()
The main class of this node is called Launchpad_Class(). This class contains all the methods to start, stop, and convert serial data to ROS topics. In the main function, we will create an object of the Launchpad_Class(). After creating the object, we will call the Start() method, which will start the serial communication between Tiva-C LaunchPad and the PC. If we interrupt the driver node by typing Ctrl + C, it will reset LaunchPad and stop the serial communication between the PC and LaunchPad.
The following code snippet is from the constructor function of Launchpad_Class(). In the following snippet, we will retrieve the port and baud rate of the LaunchPad board from the ROS parameters and initialize the SerialDateGateway object using these parameters. The SerialDataGateway object calls the _HandleReceivedLine() function inside this class when any incoming serial data arrives at the serial port. This function will process each line of serial data and extract, convert, and insert it in the appropriate headers of each ROS topic data type:
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#################################################################
rospy.loginfo("Starting with serial port:
" + port + ", baud rate: " + str(baudRate))#Initializing SerialDataGateway object with serial port, baud
rate and callback function to handle incoming serial dataself._SerialDataGateway = SerialDataGateway(port,
baudRate, self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
###################################################################Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel',Int64,queue_size
= 10)self._Right_Encoder = rospy.Publisher('rwheel',Int64,queue_size
= 10)
#Publisher for Battery level(for upgrade purpose) self._Battery_Level =
rospy.Publisher('battery_level',Float32,queue_size = 10) #Publisher for Ultrasonic distance sensor self._Ultrasonic_Value =
rospy.Publisher('ultrasonic_distance',Float32,queue_size = 10) #Publisher for IMU rotation quaternion values self._qx_ = rospy.Publisher('qx',Float32,queue_size = 10) self._qy_ = rospy.Publisher('qy',Float32,queue_size = 10) self._qz_ = rospy.Publisher('qz',Float32,queue_size = 10) self._qw_ = rospy.Publisher('qw',Float32,queue_size = 10) #Publisher for entire serial data self._SerialPublisher = rospy.Publisher('serial',
String,queue_size=10)
We will create the ROS publisher object for sensors such as the encoder, IMU, and ultrasonic sensor, as well as for the entirety of the serial data for debugging purposes. We will also subscribe the speed commands to the left-hand side and right-hand side wheel of the robot. When a speed command arrives on the topic, it calls the respective callbacks to send speed commands to the robot's LaunchPad:
self._left_motor_speed = rospy.Subscriber('left_wheel_speed',Float32,self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',Float32,self._Update_Right_Speed)
After setting the ChefBot driver node, we need to interface the robot with a ROS navigation stack in order to perform autonomous navigation. The basic requirement for doing autonomous navigation is that the robot driver nodes receive velocity commands from the ROS navigational stack. The robot can be controlled using teleoperation. In addition to these features, the robot must be able to compute its positional or odometry data and generate the tf data to be sent into the navigational stack. There must be a PID controller to control the robot's motor velocity. The following ROS package helps us to perform these functions. The differential_drive package contains nodes to perform the preceding operation. We are reusing these nodes in our package to implement these functionalities. You can find the differential_drive package in ROS at http://wiki.ros.org/differential_drive.
The following diagram shows how these nodes communicate with each other:
The purpose of each node in the ChefBot_bringup package is as follows:
twist_to_motors.py: This node will convert a ROS Twist command or linear and angular velocity to an individual motor velocity target. The target velocities are published at a rate of the ~rate (measured in Hertz) and the publish timeout_ticks time's velocity after the Twist message stops. The following are the topics and parameters that will be published and subscribed to by this node:
Publishing topics:
lwheel_vtarget(std_msgs/Float32): This is the target velocity of the left wheel (measured in m/s).
rwheel_vtarget (std_msgs/Float32): This is the target velocity of the right wheel (measured in m/s).
Subscribing topics:
Twist (geometry_msgs/Twist): This is the target Twist command for the robot. The linear velocity in the x-direction and the angular velocity theta of the Twist messages are used in this robot.
Important ROS parameters:
~base_width (float, default: 0.1): This is the distance between the robot's two wheels in meters.
~rate (int, default: 50): This is the rate at which the velocity target is published (Hertz).
~timeout_ticks (int, default:2): This is the number of the velocity target message published after stopping the Twist messages.
pid_velocity.py: This is a simple PID controller to control the speed of each motor by taking feedback from the wheel encoders. In a differential drive system, we need one PID controller for each wheel. It will read the encoder data from each wheel and control the speed of each wheel.
Publishing topics:
motor_cmd (Float32): This is the final output of the PID controller that goes to the motor. We can change the range of the PID output using the out_min and out_max ROS parameter.
wheel_vel (Float32): This is the current velocity of the robot wheel in m/s.
Subscribing topics:
wheel (Int16): This topic is the output of a rotary encoder. There are individual topics for each encoder of the robot.
wheel_vtarget (Float32): This is the target velocity in m/s.
Important parameters:
~Kp (float,default: 10): This parameter is the proportional gain of the PID controller.
~Ki (float, default: 10): This parameter is the integral gain of the PID controller.
~Kd (float, default: 0.001): This parameter is the derivative gain of the PID controller.
~out_min (float, default: 255): This is the minimum limit of the velocity value to the motor. This parameter limits the velocity's value to the motor called the wheel_vel topic.
~out_max (float, default: 255): This is the maximum limit of the wheel_vel topic (measured in Hertz).
~rate (float, default: 20): This is the rate of publishing the wheel_vel topic.
ticks_meter (float, default: 20): This is the number of wheel encoder ticks per meter. This is a global parameter because it's used in other nodes too.
vel_threshold (float, default: 0.001): If the robot velocity drops below this parameter, we consider the wheel as stationary. If the velocity of the wheel is less than vel_threshold, we consider it as zero.
encoder_min (int, default: 32768): This is the minimum value of encoder reading.
encoder_max (int, default: 32768): This is the maximum value of encoder reading.
wheel_low_wrap (int, default: 0.3 * (encoder_max - encoder_min) + encoder_min): These values decide whether the odometry is in a negative or positive direction.
wheel_high_wrap (int, default: 0.7 * (encoder_max - encoder_min) + encoder_min): These values decide whether the odometry is in a negative or positive direction.
diff_tf.py: This node computes the transformation of odometry and broadcasts between the odometry frame and the robot's base frame.
Publishing topics:
odom (nav_msgs/odometry): This publishes the odometry (the current pose and twist of the robot).
tf: This provides the transformation between the odometry frame and the robot base link.
Subscribing topics:
lwheel (std_msgs/Int16), rwheel (std_msgs/Int16): These are the output values from the left and right encoders of the robot.
- ChefBot_keyboard_teleop.py: This node sends the Twist command using controls from the keyboard.
Publishing topics:
cmd_vel_mux/input/teleop (geometry_msgs/Twist): This publishes the Twist messages using keyboard commands.
Now that we have looked at the nodes in the ChefBot_bringup package, we will look at the functions of the launch files.