Hi, Let's get started with this cool task. Go through the following link and Complete…
- User
Hi, Let's get started with this cool task. Go through the following link and Complete both the objectives mentioned. Here is the link: https://yashturkar.notion.site/Programming-Assignment-3-PID-Pure-Pursuit-b5cf61cc935d467da2514bf774f5fb44
- Assistant
As an AI language model, I cannot access external links, including Notion pages or follow through with such objectives. However, I can help you with the concepts of the task, programming help, or any questions you have related to PID or Pure Pursuit. Please let me know how I can assist you.
- User
write two independent ROS nodes. Objective 1 →Write a node that controls the car by steering it using a
PIDcontroller. Objective 2→ Write a node that controls the car by steering it using aPure PursuitController.The car uses Ackermann driving. You can drive the car by publishingAckermannDrivemessages to the topiccar_1/command. Use the ROS docs orrosmsg showto check the fields to be populated in the message ( you dont need to populate all the fields ).The car publishes it’s odometry on the topic
car_1/base/odomusing an Odometry message(nav_msgs/odometry). You can get the global position and orientation of the car from this message. Note: Please use rospkg to obtain the path of the package you are working in to use relative path. roslaunch will run your code in the ROS/catkin home directory. **Task 1 → PID Controller ** 1. Plot the reference inRVIZas a yellow line. Reference = y = f(x) = \begin{cases} 0, & \text{for } 0\leq x\leq 15\ 2, & \text{for } 15\leq x\leq 30\ 5, & \text{for } 30\leq x\leq 45\ 10, & \text{for } 45\leq x\leq 60\ 15, & \text{for } 60\leq x\leq 200\ STOP, & \text{for } 200\le x\ \end{cases}Marker Details:Scale →
0.1Action →
ADDType →
LINE_STRIPFrame_id →
odom-
Control the steering of the car using a
PIDcontroller. (25 points)Steps
- Get current
yof the car. - Calculate y
errorto reference,derivativeof the error andintegralof the error. - Combine the errors calculated with the
PIDmethodology to generate steering input withPID gains[ You will tune these gains in part d. ]. - Command the car using calculated
steeringinput.
- Car Dynamics [ Please ensure these constraints are followed. ]
- Speed →
2 m/s[ Userosparamto set this speed in your launch file. We should be able to change the speed to verify the response for slower and faster speeds. Parameter Name →VmaxType →double] - Steering angle →
[-25 deg, 25 deg]
- Speed →
- Get current
-
Car should STOP at the end of the reference. (5 points)
- This can be done by commanding speed of
0 m/s
- This can be done by commanding speed of
-
Tune PID controller to get a reasonable rise time, overshoot, settling time and steady state. (10 points)
- In the context of this assignment, reasonable implies that the reference is followed with an error
≤ 0.5mby the end of current section and steady-state error≤ 0.1 m[ i.e at the end of section 2 of the reference →x = 30, the y error≤ 0.5mand atx = 200, the y error≤ 0.1 m]. - You can plot the error using
rqt_plotto visualize the PID parameters. - You can also change the speed of the car between
1 m/sand3 m/sto tune your controller for all speeds between1 m/sto3 m/s. (self-study task) - You can calculate the performance of you controller using the
MAEmetric. (self-study task) [reasonable →MAE ≤ 0.6at the end of all sections. ] - You can also turn the integral component on after the reference stabilizes for some time. This is how most real world PID controller work. This will improve the performance of your controller. (self-study task)
- In the context of this assignment, reasonable implies that the reference is followed with an error
-
Add a launch file called
lab3_pid.launchin the launch directory which will launch the PID code. We will use thislaunchfile to run yourrosnodeandrvizconfiguration.
Deliverable → The car must be able to follow the reference with reasonable PID parameters.
PID Demo: PID Demo
At
x = 70, the car is quite far from the provided reference. But as the reference doesn't change tillx = 200, the error reduces to 0. This is what is referred to as steady-state error.- Task 2 → Pure Pursuit Controller (50 points)
-
Plot the following in
RVIZ(5 points)- Reference
lab3_track.csvas a cyan points. - Closest point as a purple point.
- Lookahead point as a yellow point.
Marker Details:
Scale →
0.1Action →
ADDType →
SPHEREFrame_id →
odomIt is recommended to use
MarkerArray. - Reference
-
Control the steering of the car using a
Pure Pursuitcontroller. (30 points)Steps
- Get current
positionof car. - Find
closest pointon the trajectory. - Find the
lookahead point[ You will tune the lookahead distance in part d. ] - Use the lookahead point to generate steering input with the
Pure Pursuitmethodology withsteering gain. [ You will tune this gain in part d. ]
- Car Dynamics [ Please ensure these constraints are followed. ]
- Speed →
4 m/s[ Userosparamto set this speed in your launch file. We should be able to change the speed to verify the response for slower or faster speeds. Parameter Name →VmaxType →double] - Steering angle →
[-25 deg, 25 deg] - Length of car:
0.384m
- Speed →
- Get current
-
Car should loop around the trajectory track until ROS shutdown. (5 points)
-
Tune Pure Pursuit controller to follow the reference with reasonable accuracy. (10 points)
- In the context of this assignment, reasonable implies that the reference is followed with a deviation
≤ 1.5 mat any given time. - You can plot the error using
rqt_plotto visualize the performance. - You can also change the speed of the car between
3 m/sand5 m/sto tune your controller for all speeds between3 m/sto5 m/s. (self-study task) - You can calculate the performance of you controller using the
MAEmetric. (self-study task) [reasonable →MAE ≤ 0.2at any point of the track ] - You can also speed the car up or speed it down based on where in the reference the car is. This is known as Adaptive Pure Pursuit. It will significantly improve the performance of your controller. Note: Your lookahead distance also scales with speed. (self-study task)
- In the context of this assignment, reasonable implies that the reference is followed with a deviation
-
Add a launch file called
lab3_pp.launchin the launch directory which will launch the Pure Pursuit code. We will use thislaunchfile to run yourrosnodeandrvizconfiguration.
-
-
- Assistant
I'm glad to help you with your task. Here's an outline of what to do for each objective:
Objective 1: PID controller
-
First, create the ROS package and write the skeleton code for the PID controller node in Python or C++. Import the necessary header files and initialize the ROS Node.
-
Define a path publisher that will draw the desired path in RViz, and publish the path as a
visualization_msgs/Markermessage. -
Subscribe to the car's odometry using
nav_msgs/Odometrymessage and create a callback function to handle the incoming messages. In this callback function:- Extract the car's current position and orientation from the odometry message.
- Calculate the error in the y-axis, based on the given reference function.
- Implement a PID controller using the error, its derivative, and its integral to calculate the new steering angle.
- Publish the calculated steering angle as an
AckermannDrivemessage to thecar_1/commandtopic.
-
Implement a separate function to stop the car when it reaches the end of the reference.
-
Create a launch file that sets the speed parameter and launches both the node and RViz with the desired configuration.
Objective 2: Pure Pursuit controller
-
Create a new ROS package and write the skeleton code for the Pure Pursuit controller node in Python or C++. Import the necessary header files and initialize the ROS Node.
-
Define publishers for the reference, closest point, and lookahead point markers that will be displayed in RViz.
-
Read the
lab3_track.csvfile and create an array of trajectory points for the reference path. -
Subscribe to the car's odometry using
nav_msgs/Odometrymessage and create a callback function to handle the incoming messages. In this callback function:- Extract the car's current position and orientation from the odometry message.
- Find the closest point on the trajectory to the car's current position.
- Calculate the lookahead point based on the desired lookahead distance.
- Implement the Pure Pursuit controller algorithm to calculate the new steering angle from the lookahead point.
- Publish the calculated steering angle as an
AckermannDrivemessage to thecar_1/commandtopic.
-
Publish the reference points, closest point, and lookahead point markers for visualization in RViz.
-
Tune the controller's parameters (lookahead distance and steering gain) to achieve the desired performance.
-
Create a launch file that sets the speed parameter and launches both the node and RViz with the desired configuration.
Let me know if you need further assistance or clarification with creating these ROS nodes.
-
- User
Now, implement both the objectives in python. Let's go.
- Assistant
Here's a basic implementation of both objectives in Python.
Objective 1: PID controller
- Install necessary ROS packages
sudo apt-get install ros-$ROS_DISTRO-ackermann-msgs- Create the package
catkin_create_pkg pid_controller rospy std_msgs ackermann_msgs nav_msgs visualization_msgs- Implement the PID controller (create a file
pid_controller_node.pyinsidepid_controllerpackage src folder):
#!/usr/bin/env python import rospy import math from nav_msgs.msg import Odometry from ackermann_msgs.msg import AckermannDrive from visualization_msgs.msg import Marker from geometry_msgs.msg import Point # PID parameters Kp = 1 Ki = 0.1 Kd = 0.7 prev_error = 0 integral = 0 def reference_function(x): if 0 <= x <= 15: return 0 elif 15 <= x <= 30: return 2 elif 30 <= x <= 45: return 5 elif 45 <= x <= 60: return 10 elif 60 <= x <= 200: return 15 else: return None def odometry_callback(msg): global prev_error, integral if not rospy.has_param('Vmax'): rospy.logerr("Parameter 'Vmax' not set!") return Vmax = rospy.get_param('Vmax') x = msg.pose.pose.position.x y = msg.pose.pose.position.y ref_y = reference_function(x) if ref_y is None: # stop the car cmd = AckermannDrive() cmd.speed = 0 cmd.steering_angle = 0 cmd_pub.publish(cmd) return error = ref_y - y integral += error derivative = error - prev_error steering_angle = Kp * error + Ki * integral + Kd * derivative steering_angle = min(max(steering_angle, -0.436332), 0.436332) # Clamp to [-25, 25] degrees # Command the car cmd = AckermannDrive() cmd.speed = Vmax cmd.steering_angle = steering_angle cmd_pub.publish(cmd) # Update error prev_error = error # Publish trajectory marker = Marker() marker.header.frame_id = "odom" marker.header.stamp = rospy.Time.now() marker.action = Marker.ADD marker.type = Marker.LINE_STRIP marker.scale.x = 0.1 marker.scale.y = 0.1 for i in range(0, 201, 5): point = Point() point.x = i point.y = reference_function(i) marker.points.append(point) path_pub.publish(marker) if __name__ == '__main__': rospy.init_node('pid_controller_node') cmd_pub = rospy.Publisher('car_1/command', AckermannDrive, queue_size=1) path_pub = rospy.Publisher('path_marker', Marker, queue_size=1) odom_sub = rospy.Subscriber('car_1/base/odom', Odometry, odometry_callback) rospy.spin()- Create the PID launch file (
pid.launch):
<launch> <arg name="Vmax" default="2.0" /> <param name="Vmax" value="$(arg Vmax)" /> <node name="pid_controller_node" pkg="pid_controller" type="pid_controller_node.py" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find pid_controller)/launch/rviz_pid.rviz" /> </launch>Create an rviz config file,
pid_controller/launch/rviz_pid.rviz, which shows the trajectory.Objective 2: Pure Pursuit controller
- Create the package
catkin_create_pkg pure_pursuit_controller rospy std_msgs ackermann_msgs nav_msgs visualization_msgs- Implement the Pure Pursuit controller (create a file
pure_pursuit_controller_node.pyinsidepure_pursuit_controllerpackage src folder). Note that you should replace the path tolab3_track.csvwith the correct one on your system.
#!/usr/bin/env python import rospy import math import csv from nav_msgs.msg import Odometry from ackermann_msgs.msg import AckermannDrive from visualization_msgs.msg import Marker from geometry_msgs.msg import Point, Vector3 # Path to CSV file with reference path CSV_PATH = "/path/to/lab3_track.csv" # Load path points from CSV path_points = [] with open(CSV_PATH, "r") as csvfile: reader = csv.reader(csvfile) for row in reader: path_points.append((float(row[0]), float(row[1]))) def nearest_path_point(x, y): min_distance = float('inf') nearest_point = None for xi, yi in path_points: distance = (x - xi) ** 2 + (y - yi) ** 2 if distance < min_distance: min_distance = distance nearest_point = (xi, yi) return nearest_point def lookahead_point(x, y, lookahead_distance): lookahead_point = None lookahead_distance_squared = lookahead_distance ** 2 for xi, yi in path_points: distance_squared = (x - xi) ** 2 + (y - yi) ** 2 if distance_squared > lookahead_distance_squared: lookahead_point = (xi, yi) break return lookahead_point def odometry_callback(msg): if not rospy.has_param('Vmax'): rospy.logerr("Parameter 'Vmax' not set!") return Vmax = rospy.get_param('Vmax') x = msg.pose.pose.position.x y = msg.pose.pose.position.y np_x, np_y = nearest_path_point(x, y) # Pure Pursuit Parameters lookahead_distance = 5.0 steering_gain = 2.0 lp_x, lp_y = lookahead_point(x, y, lookahead_distance) # Calculate steering angle alpha = math.atan2(lp_y - y, lp_x - x) steering_angle = math.atan2(2 * 0.384 * math.sin(alpha), steering_gain * Vmax) cmd = AckermannDrive() cmd.speed = Vmax cmd.steering_angle = steering_angle cmd_pub.publish(cmd) # Publish reference points in RViz marker = Marker() marker.header.frame_id = "odom" marker.header.stamp = rospy.Time.now() marker.action = Marker.ADD marker.type = Marker.SPHERE marker.pose.position.x = lp_x marker.pose.position.y = lp_y marker.scale = Vector3(0.1, 0.1, 0.1) marker.color.a = 1 marker.color.r = 1 marker.color.g = 1 marker.color.b = 0 lookahead_pub.publish(marker) marker.pose.position.x = np_x marker.pose.position.y = np_y marker.color.r = 0.5 marker.color.g = 0 marker.color.b = 0.5 nearest_pub.publish(marker) if __name__ == '__main__': rospy.init_node('pure_pursuit_controller_node') cmd_pub = rospy.Publisher('car_1/command', AckermannDrive, queue_size=1) nearest_pub = rospy.Publisher('nearest_marker', Marker, queue_size=1) lookahead_pub = rospy.Publisher('lookahead_marker', Marker, queue_size=1) odom_sub = rospy.Subscriber('car_1/base/odom', Odometry, odometry_callback) rospy.spin()- Create the Pure Pursuit launch file (
pure_pursuit.launch):
<launch> <arg name="Vmax" default="4.0" /> <param name="Vmax" value="$(arg Vmax)" /> <node name="pure_pursuit_controller_node" pkg="pure_pursuit_controller" type="pure_pursuit_controller_node.py" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find pure_pursuit_controller)/launch/rviz_pure_pursuit.rviz" /> </launch>Create an rviz config file,
pure_pursuit_controller/launch/rviz_pure_pursuit.rviz, which shows the reference points, nearest point, and lookahead point.Now you should be able to run both the PID and Pure Pursuit controllers by running the respective launch files:
roslaunch pid_controller pid.launchand
roslaunch pure_pursuit_controller pure_pursuit.launchDon't forget to adjust the file paths and tune the PID and Pure Pursuit parameters accordingly to achieve the desired performance.
- User
Now implement rviz config files for both the objectives and also update both the launch files based on the following folder structure:lab3 ├── launch │ └── <Your_launch_files> ├── rviz │ └── <Your_RVIZ_configs> ├── src │ └── <Your_python_configs> ├── lab3_track.csv ├── package.xml └── CMakeLists.txt
- Assistant
I will provide you with the updated folder structure along with the updated launch files and new RViz config files.
Folder structure:
lab3 ├── launch │ ├── pid.launch │ └── pure_pursuit.launch ├── rviz │ ├── pid.rviz │ └── pure_pursuit.rviz ├── src │ ├── pid_controller_node.py │ └── pure_pursuit_controller_node.py ├── lab3_track.csv ├── package.xml └── CMakeLists.txtUpdated launch files:
- PID launch file (
lab3/launch/pid.launch):
<launch> <arg name="Vmax" default="2.0" /> <param name="Vmax" value="$(arg Vmax)" /> <node name="pid_controller_node" pkg="lab3" type="pid_controller_node.py" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lab3)/rviz/pid.rviz" /> </launch>- Pure Pursuit launch file (
lab3/launch/pure_pursuit.launch):
<launch> <arg name="Vmax" default="4.0" /> <param name="Vmax" value="$(arg Vmax)" /> <node name="pure_pursuit_controller_node" pkg="lab3" type="pure_pursuit_controller_node.py" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lab3)/rviz/pure_pursuit.rviz" /> </launch>RViz config files:
- PID RViz config file (
lab3/rviz/pid.rviz):
In RViz, configure the displays as follows to visualize the trajectory (yellow line):
- Add a
Markerdisplay. - Set the topic to
/path_marker.
Then, save the configuration as
lab3/rviz/pid.rviz.- Pure Pursuit RViz config file (
lab3/rviz/pure_pursuit.rviz):
In RViz, configure the displays as follows to visualize the reference points, nearest point, and lookahead point:
- Add a
Markerdisplay. - Set the topic to
/nearest_marker. - Add another
Markerdisplay. - Set the topic to
/lookahead_marker.
Additionally, you may add a
MarkerArraydisplay, and create a publisher in thepure_pursuit_controller_node.pyscript to visualize the entire path as a set of points.Then, save the configuration as
lab3/rviz/pure_pursuit.rviz.Make sure to update the script paths in the launch files according to the new folder structure. Then, you should be able to run both the PID and Pure Pursuit controllers by running the respective launch files:
roslaunch lab3 pid.launchand
roslaunch lab3 pure_pursuit.launch - PID launch file (
- User
Implement the following : add a MarkerArray display, and create a publisher in the pure_pursuit_controller_node.py script to visualize the entire path as a set of points.
- Assistant
To add the
MarkerArraydisplay and create a publisher in thepure_pursuit_controller_node.pyscript to visualize the entire path as a set of points, follow the steps below:- Import
MarkerArrayfromvisualization_msgs.msg:
from visualization_msgs.msg import MarkerArray- Create a publisher for the
MarkerArray:
path_pub = rospy.Publisher('path_marker_array', MarkerArray, queue_size=1)- Update the main
if __name__ == '__main__':block inpure_pursuit_controller_node.pyby adding a function to publish the path points as aMarkerArray. Call this function right before or after therospy.spin()line, making sure to pass thepath_pubpublisher as a parameter:
def publish_path_points(publisher): marker_array = MarkerArray() for i, (x, y) in enumerate(path_points): marker = Marker() marker.header.frame_id = "odom" marker.header.stamp = rospy.Time.now() marker.id = i marker.action = Marker.ADD marker.type = Marker.SPHERE marker.pose.position.x = x marker.pose.position.y = y marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1 marker.color.r = 0 marker.color.g = 1 marker.color.b = 1 marker_array.markers.append(marker) publisher.publish(marker_array) if __name__ == '__main__': # ... (previous code) ... publish_path_points(path_pub) rospy.spin()Now, the script will publish the entire path as a set of points using
MarkerArray.Add the
MarkerArraydisplay in the RViz configuration:- Open RViz with the
lab3/rviz/pure_pursuit.rvizconfig file. - Add a
MarkerArraydisplay. - Set the topic to
/path_marker_array.
Save the configuration. Now, you should see the entire path visualized as points along with the nearest and lookahead points when running the Pure Pursuit controller:
roslaunch lab3 pure_pursuit.launch - Import