Skip to content
TheChatArchiveThe Chat Archive
gpt-4-0314· openaiHugging Face Suzhen/CodeChat

Hi, Let's get started with this cool task. Go through the following link and Complete…

  1. 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

  2. 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.

  3. User

    write two independent ROS nodes. Objective 1 →Write a node that controls the car by steering it using a PID controller. Objective 2→ Write a node that controls the car by steering it using a Pure Pursuit Controller.The car uses Ackermann driving. You can drive the car by publishing AckermannDrive messages to the topic car_1/command . Use the ROS docs or rosmsg show to 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/odom using 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 in RVIZ as 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.1

    Action → ADD

    Type → LINE_STRIP

    Frame_id → odom

    1. Control the steering of the car using a PID controller. (25 points)

      Steps

      1. Get current y of the car.
      2. Calculate y error to reference, derivative of the error and integral of the error.
      3. Combine the errors calculated with the PID methodology to generate steering input with PID gains [ You will tune these gains in part d. ].
      4. Command the car using calculated steering input.
      • Car Dynamics [ Please ensure these constraints are followed. ]
        • Speed → 2 m/s [ Use rosparam to 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 → Vmax Type → double ]
        • Steering angle → [-25 deg, 25 deg]
    2. Car should STOP at the end of the reference. (5 points)

      • This can be done by commanding speed of 0 m/s
    3. 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.5m by 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.5m and at x = 200, the y error ≤ 0.1 m ].
      • You can plot the error using rqt_plot to visualize the PID parameters.
      • You can also change the speed of the car between 1 m/s and 3 m/s to tune your controller for all speeds between 1 m/s to 3 m/s. (self-study task)
      • You can calculate the performance of you controller using the MAE metric. (self-study task) [reasonable → MAE ≤ 0.6 at 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)
    4. Add a launch file called lab3_pid.launch in the launch directory which will launch the PID code. We will use this launch file to run your rosnode and rviz configuration.


    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 till x = 200, the error reduces to 0. This is what is referred to as steady-state error.

    1. Task 2 → Pure Pursuit Controller (50 points)
      1. Plot the following in RVIZ (5 points)

        • Reference lab3_track.csv as a cyan points.
        • Closest point as a purple point.
        • Lookahead point as a yellow point.

        Marker Details:

        Scale → 0.1

        Action → ADD

        Type → SPHERE

        Frame_id → odom

        It is recommended to use MarkerArray.

      2. Control the steering of the car using a Pure Pursuit controller. (30 points)

        Steps

        1. Get current position of car.
        2. Find closest point on the trajectory.
        3. Find the lookahead point [ You will tune the lookahead distance in part d. ]
        4. Use the lookahead point to generate steering input with the Pure Pursuitmethodology with steering gain. [ You will tune this gain in part d. ]
        • Car Dynamics [ Please ensure these constraints are followed. ]
          • Speed → 4 m/s [ Use rosparam to 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 → Vmax Type → double ]
          • Steering angle → [-25 deg, 25 deg]
          • Length of car: 0.384m
      3. Car should loop around the trajectory track until ROS shutdown. (5 points)

      4. 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 m at any given time.
        • You can plot the error using rqt_plot to visualize the performance.
        • You can also change the speed of the car between 3 m/s and 5 m/s to tune your controller for all speeds between 3 m/s to 5 m/s. (self-study task)
        • You can calculate the performance of you controller using the MAEmetric. (self-study task) [reasonable → MAE ≤ 0.2 at 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)
      5. Add a launch file called lab3_pp.launch in the launch directory which will launch the Pure Pursuit code. We will use this launch file to run your rosnode and rviz configuration.

  4. 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

    1. 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.

    2. Define a path publisher that will draw the desired path in RViz, and publish the path as a visualization_msgs/Marker message.

    3. Subscribe to the car's odometry using nav_msgs/Odometry message 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 AckermannDrive message to the car_1/command topic.
    4. Implement a separate function to stop the car when it reaches the end of the reference.

    5. 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

    1. 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.

    2. Define publishers for the reference, closest point, and lookahead point markers that will be displayed in RViz.

    3. Read the lab3_track.csv file and create an array of trajectory points for the reference path.

    4. Subscribe to the car's odometry using nav_msgs/Odometry message 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 AckermannDrive message to the car_1/command topic.
    5. Publish the reference points, closest point, and lookahead point markers for visualization in RViz.

    6. Tune the controller's parameters (lookahead distance and steering gain) to achieve the desired performance.

    7. 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.

  5. User

    Now, implement both the objectives in python. Let's go.

  6. Assistant

    Here's a basic implementation of both objectives in Python.

    Objective 1: PID controller

    1. Install necessary ROS packages
    sudo apt-get install ros-$ROS_DISTRO-ackermann-msgs
    
    1. Create the package
    catkin_create_pkg pid_controller rospy std_msgs ackermann_msgs nav_msgs visualization_msgs
    
    1. Implement the PID controller (create a file pid_controller_node.py inside pid_controller package 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()
    
    1. 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

    1. Create the package
    catkin_create_pkg pure_pursuit_controller rospy std_msgs ackermann_msgs nav_msgs visualization_msgs
    
    1. Implement the Pure Pursuit controller (create a file pure_pursuit_controller_node.py inside pure_pursuit_controller package src folder). Note that you should replace the path to lab3_track.csv with 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()
    
    1. 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.launch
    

    and

    roslaunch pure_pursuit_controller pure_pursuit.launch
    

    Don't forget to adjust the file paths and tune the PID and Pure Pursuit parameters accordingly to achieve the desired performance.

  7. 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

  8. 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.txt
    

    Updated launch files:

    1. 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>
    
    1. 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:

    1. PID RViz config file (lab3/rviz/pid.rviz):

    In RViz, configure the displays as follows to visualize the trajectory (yellow line):

    • Add a Marker display.
    • Set the topic to /path_marker.

    Then, save the configuration as lab3/rviz/pid.rviz.

    1. 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 Marker display.
    • Set the topic to /nearest_marker.
    • Add another Marker display.
    • Set the topic to /lookahead_marker.

    Additionally, you may 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.

    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.launch
    

    and

    roslaunch lab3 pure_pursuit.launch
    
  9. 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.

  10. Assistant

    To add the MarkerArray display and create a publisher in the pure_pursuit_controller_node.py script to visualize the entire path as a set of points, follow the steps below:

    1. Import MarkerArray from visualization_msgs.msg:
    from visualization_msgs.msg import MarkerArray
    
    1. Create a publisher for the MarkerArray:
    path_pub = rospy.Publisher('path_marker_array', MarkerArray, queue_size=1)
    
    1. Update the main if __name__ == '__main__': block in pure_pursuit_controller_node.py by adding a function to publish the path points as a MarkerArray. Call this function right before or after the rospy.spin() line, making sure to pass the path_pub publisher 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 MarkerArray display in the RViz configuration:

    1. Open RViz with the lab3/rviz/pure_pursuit.rviz config file.
    2. Add a MarkerArray display.
    3. 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