Clean up pure pursuit

src/pure_pursuit/CMakeLists.txt

@@ -38,25 +38,24 @@ include_directories(
 )
 
 # Create Cpp executable
-add_executable(pure_pursuit_node src/pure_pursuit_node.cpp)
+add_executable(pure_pursuit src/pure_pursuit.cpp)
 
-ament_target_dependencies(pure_pursuit_node 
+ament_target_dependencies(pure_pursuit
   rclcpp geometry_msgs ackermann_msgs nav_msgs sensor_msgs std_msgs visualization_msgs tf2_ros tf2 eigen3_cmake_module Eigen3
 )
 
-add_executable(waypoint_visualiser_node src/waypoint_visualiser_node.cpp)
+add_executable(waypoint_visualizer src/waypoint_visualizer.cpp)
 
-ament_target_dependencies(waypoint_visualiser_node 
+ament_target_dependencies(waypoint_visualizer
   rclcpp geometry_msgs std_msgs visualization_msgs eigen3_cmake_module Eigen3
 )
 
 # Install Cpp executables
 install(TARGETS
-  pure_pursuit_node
-  waypoint_visualiser_node
+  pure_pursuit
+  waypoint_visualizer
   DESTINATION lib/${PROJECT_NAME})
 
-
 # Install launch files and config
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})

src/pure_pursuit/USAGE_GUIDE.md

@@ -1,20 +1,28 @@
 # Usage Guide
+
 To use this node on the physical car, run
 
 ```bash
 ros2 launch pure_pursuit pure_pursuit.launch.py
 ```
 
-This will use the parameters from the config file `config/config.yaml` 
+This will use the parameters from the config file `config/config.yaml`
 
-If you want to test it out in simulation, which uses different topic names, run 
+If you want to test it out in simulation, which uses different topic names, run
 
 ```bash
 ros2 launch pure_pursuit sim_pure_pursuit_launch.py
 ```
 
 ### Trying different parameters without rebuilding
 
+Setting the gain
 ```bash
-ros2 param set pure_pursuit_node K_p 0.1
+ros2 param set pure_pursuit K_p 0.1
 ```
+
+Setting the velocity profile
+
+```bash
+ros2 param set pure_pursuit velocity_percentage 0.7
+```

src/pure_pursuit/config/config.yaml

@@ -1,15 +1,15 @@
-pure_pursuit_node:
+pure_pursuit:
   ros__parameters:
-    waypoints_path: "/f1tenth_ws/src/pure_pursuit/racelines/e7_floor5_biggest.csv" 
+    waypoints_path: "/f1tenth_ws/src/pure_pursuit/racelines/e7_floor5_biggest.csv"
     odom_topic: "/pf/pose/odom"
     car_refFrame: "laser"
     global_refFrame: "map"
     drive_topic: "/drive"
     rviz_current_waypoint_topic: "/current_waypoint"
     rviz_lookahead_waypoint_topic: "/lookahead_waypoint"
-    
+
     # Pure Pursuit Parameters
-    # Lookahead is computed using the following: lookahead = min(max(min_lookahead, max_lookahead * curr_velocity / lookahead_ratio), max_lookahead); 
+    # Lookahead is computed using the following: lookahead = min(max(min_lookahead, max_lookahead * curr_velocity / lookahead_ratio), max_lookahead);
     # min_lookahead: 0.5 # for square track
     min_lookahead: 1.0 # for larger track
     max_lookahead: 3.0 # lookahead when the car is going around max velocity
@@ -19,7 +19,6 @@ pure_pursuit_node:
     steering_limit: 25.0
     velocity_percentage: 0.8 # the limit, can only do 1 lap with clean tires
 
-
 waypoint_visualiser_node:
   ros__parameters:
-    waypoints_path: "/f1tenth_ws/src/pure_pursuit/racelines/e7_floor5_biggest.csv" 
+    waypoints_path: "/f1tenth_ws/src/pure_pursuit/racelines/e7_floor5_biggest.csv"

src/pure_pursuit/config/sim_config.yaml

@@ -1,23 +1,22 @@
-pure_pursuit_node:
+pure_pursuit:
   ros__parameters:
-    waypoints_path: "/sim_ws/src/pure_pursuit/racelines/e7_floor5_square.csv" 
+    waypoints_path: "/sim_ws/src/pure_pursuit/racelines/e7_floor5_square.csv"
     odom_topic: "/ego_racecar/odom"
     car_refFrame: "ego_racecar/base_link"
     global_refFrame: "map"
     drive_topic: "/drive"
     rviz_current_waypoint_topic: "/current_waypoint"
     rviz_lookahead_waypoint_topic: "/lookahead_waypoint"
-    
+
     # Pure Pursuit Parameters
-    # Lookahead is computed using the following: lookahead = min(max(min_lookahead, max_lookahead * curr_velocity / lookahead_ratio), max_lookahead); 
+    # Lookahead is computed using the following: lookahead = min(max(min_lookahead, max_lookahead * curr_velocity / lookahead_ratio), max_lookahead);
     min_lookahead: 0.5
     max_lookahead: 3.0 # lookahead when the car is going around max velocity
     lookahead_ratio: 8.0 # approximately the max velocity
     K_p: 0.5
     steering_limit: 25.0
     velocity_percentage: 1.0 # the limit, can only do 1 lap with clean tires
 
-
 waypoint_visualiser_node:
   ros__parameters:
-    waypoints_path: "/sim_ws/src/pure_pursuit/racelines/e7_floor5_square.csv" 
+    waypoints_path: "/sim_ws/src/pure_pursuit/racelines/e7_floor5_square.csv"

src/pure_pursuit/include/pure_pursuit.hpp

@@ -0,0 +1,122 @@
+/*
+Pure Pursuit Implementation in C++. Includes features such as dynamic lookahead. Does not have waypoint
+interpolation yet.
+*/
+#include <math.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <chrono>
+#include <cstdlib>
+#include <fstream>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#define _USE_MATH_DEFINES
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+class PurePursuit : public rclcpp::Node {
+   public:
+    PurePursuit();
+
+   private:
+    // global static (to be shared by all objects) and dynamic variables (each instance gets its own copy -> managed on the stack)
+    struct csvFileData {
+        std::vector<double> X;
+        std::vector<double> Y;
+        std::vector<double> V;
+
+        int index;
+        int velocity_index;
+
+        Eigen::Vector3d lookahead_point_world;  // from world reference frame (usually `map`)
+        Eigen::Vector3d lookahead_point_car;    // from car reference frame
+        Eigen::Vector3d current_point_world;    // Locks on to the closest waypoint, which gives a velocity profile
+    };
+
+    Eigen::Matrix3d rotation_m;
+
+    double x_car_world;
+    double y_car_world;
+
+    std::string odom_topic;
+    std::string car_refFrame;
+    std::string drive_topic;
+    std::string global_refFrame;
+    std::string rviz_current_waypoint_topic;
+    std::string rviz_lookahead_waypoint_topic;
+    std::string waypoints_path;
+    double K_p;
+    double min_lookahead;
+    double max_lookahead;
+    double lookahead_ratio;
+    double steering_limit;
+    double velocity_percentage;
+    double curr_velocity = 0.0;
+
+    bool emergency_breaking = false;
+    std::string lane_number = "left";  // left or right lane
+
+    // file object
+    std::fstream csvFile_waypoints;
+
+    // struct initialisation
+    csvFileData waypoints;
+    int num_waypoints;
+
+    // Timer initialisation
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // declare subscriber sharedpointer obj
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_odom;
+
+    // declare publisher sharedpointer obj
+    rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr publisher_drive;
+
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vis_current_point_pub;
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vis_lookahead_point_pub;
+
+    // declare tf shared pointers
+    std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+
+    // private functions
+    double to_radians(double degrees);
+    double to_degrees(double radians);
+    double p2pdist(double &x1, double &x2, double &y1, double &y2);
+
+    void load_waypoints();
+
+    void visualize_lookahead_point(Eigen::Vector3d &point);
+    void visualize_current_point(Eigen::Vector3d &point);
+
+    void get_waypoint();
+
+    void quat_to_rot(double q0, double q1, double q2, double q3);
+
+    void transformandinterp_waypoint();
+
+    double p_controller();
+
+    double get_velocity(double steering_angle);
+
+    void publish_message(double steering_angle);
+
+    void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr odom_submsgObj);
+
+    void timer_callback();
+};

src/pure_pursuit/include/waypoint_visualizer.hpp

@@ -0,0 +1,56 @@
+#include <math.h>
+
+#include <chrono>
+#include <cstdlib>
+#include <fstream>
+#include <functional>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+// other macros
+#define _USE_MATH_DEFINES
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+class WaypointVisualizer : public rclcpp::Node {
+   public:
+    WaypointVisualizer();
+
+   private:
+    struct csvFileData {
+        std::vector<double> X;
+        std::vector<double> Y;
+    };
+
+    // topic names
+    std::string waypoints_path;
+    std::string rviz_waypoints_topic;
+
+    // file object
+    std::fstream csvFile_waypoints;
+
+    // struct initialisation
+    csvFileData waypoints;
+
+    // Publisher initialisation
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr vis_path_pub;
+
+    // Timer initialisation
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // private functions
+
+    void download_waypoints();
+
+    void visualize_points();
+    void timer_callback();
+};

src/pure_pursuit/launch/pure_pursuit_launch.py

@@ -3,21 +3,22 @@
 from ament_index_python.packages import get_package_share_directory
 import os
 
+
 def generate_launch_description():
     ld = LaunchDescription()
     config = os.path.join(
         get_package_share_directory('pure_pursuit'),
         'config',
         'config.yaml'
-        )
+    )
 
-    pure_pursuit_node = Node(
+    pure_pursuit = Node(
         package='pure_pursuit',
-        executable='pure_pursuit_node',
-        name='pure_pursuit_node',
+        executable='pure_pursuit',
+        name='pure_pursuit',
         parameters=[config]
     )
-    
+
     waypoint_visualizer_node = Node(
         package='pure_pursuit',
         executable='waypoint_visualiser_node',
@@ -29,12 +30,13 @@ def generate_launch_description():
         package='rviz2',
         executable='rviz2',
         name='rviz',
-        arguments=['-d', os.path.join(get_package_share_directory('pure_pursuit'), 'launch', 'pure_pursuit.rviz')]
+        arguments=['-d', os.path.join(get_package_share_directory(
+            'pure_pursuit'), 'launch', 'pure_pursuit.rviz')]
     )
 
     # finalize
     ld.add_action(rviz_node)
-    ld.add_action(pure_pursuit_node)
+    ld.add_action(pure_pursuit)
     ld.add_action(waypoint_visualizer_node)
 
-    return ld
+    return ld

src/pure_pursuit/launch/sim_pure_pursuit_launch.py

@@ -3,21 +3,22 @@
 from ament_index_python.packages import get_package_share_directory
 import os
 
+
 def generate_launch_description():
     ld = LaunchDescription()
     config = os.path.join(
         get_package_share_directory('pure_pursuit'),
         'config',
         'sim_config.yaml'
-        )
+    )
 
-    pure_pursuit_node = Node(
+    pure_pursuit = Node(
         package='pure_pursuit',
-        executable='pure_pursuit_node',
-        name='pure_pursuit_node',
+        executable='pure_pursuit',
+        name='pure_pursuit',
         parameters=[config]
     )
-    
+
     waypoint_visualizer_node = Node(
         package='pure_pursuit',
         executable='waypoint_visualiser_node',
@@ -26,7 +27,7 @@ def generate_launch_description():
     )
 
     # finalize
-    ld.add_action(pure_pursuit_node)
+    ld.add_action(pure_pursuit)
     ld.add_action(waypoint_visualizer_node)
 
-    return ld
+    return ld