Note that the orientation of the robot (theta) ranges from -pi to pi on 2D plane. When the robot turns in CCW direction and passes the direction of negative x axis, the value of theta will jump from pi to -pi. This needs to be handled properly, otherwise the robot will keep turning in place and cannot move forward. It is recommended that you bring up a robot, turn in place using keyboard teleoperation, and see how the value changes for theta. (You can see it from ROS logging messages if using the provided closed_loop.py file).
In general, PID controller is used to track a certain target value (called setpoint), and make sure the system can converge to this target value. For example, to control the temperature in a boiler system. (Note that the setpoint is a scalar, set to a certain target value.)
In our case, we have three variables (x, y, theta) to describe the 2D pose of the robot. At one time only one of them can be set as the desired value to track in a PID controller. If you are willing to track x, y, and theta at the same time, you will need three PID controllers. (To comlpete the task in this lab, out of three controllers, the one to track theta is required, and the other two for x and y are optional. See below an example algorithm.)
The following is an example of how to apply feedback control algorithm to waypoint navigation problem. You may follow this algorithm to start your implementation.
Suppose the robot’s current orientation is θ, the desired orientation is θ∗, the current position on X-Y plane is (x,y) and the desired position on X-Y plane is (x∗,y∗).
Calculate the moving direction from the difference between (x,y) and (x∗,y∗); set it as the desired orientation θ∗.
Initialize a PID controller with the setpoint θ∗ and a set of parameters (Kp, Ki and Kd). Adjust the angle according to the angular velocity computed by the PID controller.
Once θ→θ∗, start moving forward at a constant speed (or using a linear velocity generated by another PID controller), and keep adjusting the angle and checking the remaining distance toward desired position (x∗,y∗).
Once (x,y)→(x∗,y∗), stop and repeat the process for the next waypoint.
A sample code is provided as the starting point for your implementation. Please read carefully the provided code, and understand its functionality.
roscd ee144w25/src
touch closed_loop.py
gedit closed_loop.py
#!/usr/bin/env python3
from math import pi, sqrt, atan2, cos, sin
import numpy as np
import rospy
import tf
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Pose2D
class Turtlebot():
def __init__(self):
rospy.init_node("turtlebot_move")
rospy.loginfo("Press Ctrl + C to terminate")
self.vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
self.rate = rospy.Rate(10)
# reset odometry to zero
self.reset_pub = rospy.Publisher("mobile_base/commands/reset_odometry", Empty, queue_size=10)
for i in range(10):
self.reset_pub.publish(Empty())
self.rate.sleep()
# subscribe to odometry
self.pose = Pose2D()
self.logging_counter = 0
self.trajectory = list()
self.odom_sub = rospy.Subscriber("odom", Odometry, self.odom_callback)
try:
self.run()
except rospy.ROSInterruptException:
rospy.loginfo("Action terminated.")
finally:
# save trajectory into csv file
np.savetxt('trajectory.csv', np.array(self.trajectory), fmt='%f', delimiter=',')
def run(self):
# add your code here to adjust your movement based on 2D pose feedback
# Use the Controller
pass
def odom_callback(self, msg):
# get pose = (x, y, theta) from odometry topic
quarternion = [msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,\
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(quarternion)
self.pose.theta = yaw
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
# logging once every 100 times (Gazebo runs at 1000Hz; we save it at 10Hz)
self.logging_counter += 1
if self.logging_counter == 100:
self.logging_counter = 0
self.trajectory.append([self.pose.x, self.pose.y]) # save trajectory
# display (x, y, theta) on the terminal
rospy.loginfo("odom: x=" + str(self.pose.x) +\
"; y=" + str(self.pose.y) + "; theta=" + str(yaw))
if __name__ == '__main__':
whatever = Turtlebot()
python3 closed_loop.py
chmod +x closed_loop.py
./closed_loop.py
Odometry works in the way that it counts how many rounds the wheel rotates. Therefore, if you lift and place a robot from one place to another, it will still “think” that it is at the original place.
In Gazebo simulator, using command Ctrl + R to reset the robot is similar to lifting the robot and placing it to the origin, where the odometry will still report the previous history record. Therefore, to obtain a correct odometry feedback for the new run, we need to reset the odometry to zero.
self.reset_pub = rospy.Publisher("mobile_base/commands/reset_odometry", Empty, queue_size=10)
for i in range(10):
self.reset_pub.publish(Empty())
self.rate.sleep()
As we mentioned in the tutorial provided in Lab 0, we can use ROS Publisher to send a message out. ROS Subscriber is the one on the other side to receive and process the messages. The required arguments are the topic name odom, the message type Odometry, and a pointer to the callback function. The callback function will be executed whenever a new message is received (asynchronously; in another thread). We leverage the shared variables in Turtlebot class to store the latest pose of the robot. Note that the msg argument in the callback function is of the type Odometry. The definition is specified in the ROS Wiki documentation.
self.odom_sub = rospy.Subscriber("odom", Odometry, self.odom_callback)
def odom_callback(self, msg):
pass
In the try-except structure, finally is the keyword to indicate that the following code block will be executed regardless if the try block raises an error or not. In this case, we want to save the trajectory even when the robot stops halfway.
finally:
# save trajectory into csv file
np.savetxt('trajectory.csv', np.array(self.trajectory), fmt='%f', delimiter=',')
We provide a separate Python script to help visualize the trajectory from the saved csv file. Please do not submit this file and do not include it in the script you plan to submit, as it will block the autograder until running timeout.
#!/usr/bin/env python3
import numpy as np
import matplotlib.pyplot as plt
def visualization():
# load csv file and plot trajectory
_, ax = plt.subplots(1)
ax.set_aspect('equal')
trajectory = np.loadtxt("trajectory.csv", delimiter=',')
plt.plot(trajectory[:, 0], trajectory[:, 1], linewidth=2)
plt.xlim(-1, 5)
plt.ylim(-1, 5)
plt.show()
if __name__ == '__main__':
visualization()