From afbd5643549ac48ccf33304b51bd1685045e28be Mon Sep 17 00:00:00 2001 From: Yash Agarwal Date: Thu, 28 Mar 2024 12:41:15 +0800 Subject: [PATCH 1/2] update --- .catkin_tools/README | 13 ++ .catkin_tools/VERSION | 1 + .catkin_tools/profiles/default/build.yaml | 22 ++ .../profiles/default/devel_collisions.txt | 1 + .../catkin_tools_prebuild/devel_manifest.txt | 13 ++ .../catkin_tools_prebuild/package.xml | 10 + .../jackal_description/devel_manifest.txt | 5 + .../packages/jackal_description/package.xml | 26 +++ .../packages/me5413_world/devel_manifest.txt | 19 ++ .../default/packages/me5413_world/package.xml | 75 ++++++ src/me5413_world/CMakeLists.txt | 13 ++ src/me5413_world/cfg/path_tracker.cfg | 11 +- .../me5413_world/path_tracker_node.hpp | 26 +-- src/me5413_world/launch/path_tracking.launch | 2 +- src/me5413_world/src/path_publisher_node.cpp | 2 +- src/me5413_world/src/path_tracker_node.cpp | 152 ++++++++----- .../pure_pursuit_closest_point_approach.cpp | 201 ++++++++++++++++ .../src/pure_pursuit_linear_approach.cpp | 215 ++++++++++++++++++ .../pure_pursuit_target_point_approach.cpp | 144 ++++++++++++ 19 files changed, 868 insertions(+), 83 deletions(-) create mode 100644 .catkin_tools/README create mode 100644 .catkin_tools/VERSION create mode 100644 .catkin_tools/profiles/default/build.yaml create mode 100644 .catkin_tools/profiles/default/devel_collisions.txt create mode 100644 .catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt create mode 100644 .catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml create mode 100644 .catkin_tools/profiles/default/packages/jackal_description/devel_manifest.txt create mode 100644 .catkin_tools/profiles/default/packages/jackal_description/package.xml create mode 100644 .catkin_tools/profiles/default/packages/me5413_world/devel_manifest.txt create mode 100644 .catkin_tools/profiles/default/packages/me5413_world/package.xml create mode 100644 src/me5413_world/src/pure_pursuit_closest_point_approach.cpp create mode 100644 src/me5413_world/src/pure_pursuit_linear_approach.cpp create mode 100644 src/me5413_world/src/pure_pursuit_target_point_approach.cpp diff --git a/.catkin_tools/README b/.catkin_tools/README new file mode 100644 index 0000000..4706f47 --- /dev/null +++ b/.catkin_tools/README @@ -0,0 +1,13 @@ +# Catkin Tools Metadata + +This directory was generated by catkin_tools and it contains persistent +configuration information used by the `catkin` command and its sub-commands. + +Each subdirectory of the `profiles` directory contains a set of persistent +configuration options for separate profiles. The default profile is called +`default`. If another profile is desired, it can be described in the +`profiles.yaml` file in this directory. + +Please see the catkin_tools documentation before editing any files in this +directory. Most actions can be performed with the `catkin` command-line +program. diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION new file mode 100644 index 0000000..f76f913 --- /dev/null +++ b/.catkin_tools/VERSION @@ -0,0 +1 @@ +0.9.2 \ No newline at end of file diff --git a/.catkin_tools/profiles/default/build.yaml b/.catkin_tools/profiles/default/build.yaml new file mode 100644 index 0000000..910ef42 --- /dev/null +++ b/.catkin_tools/profiles/default/build.yaml @@ -0,0 +1,22 @@ +authors: [] +blacklist: [] +build_space: build +catkin_make_args: [] +cmake_args: [] +devel_layout: linked +devel_space: devel +extend_path: null +extends: null +install: false +install_space: install +isolate_install: false +jobs_args: [] +licenses: +- TODO +log_space: logs +maintainers: [] +make_args: [] +source_space: src +use_env_cache: false +use_internal_make_jobserver: true +whitelist: [] diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt new file mode 100644 index 0000000..f8b7411 --- /dev/null +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -0,0 +1 @@ +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock 6 diff --git a/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt b/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt new file mode 100644 index 0000000..3f15456 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt @@ -0,0 +1,13 @@ +/home/yash/ME5413/HW3/ME5413_Planning_Project/build/catkin_tools_prebuild +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/local_setup.bash /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./local_setup.bash +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/env.sh /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./env.sh +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/setup.bash /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./setup.bash +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/setup.sh /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./setup.sh +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./local_setup.zsh +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/_setup_util.py /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./_setup_util.py +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/local_setup.sh /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./local_setup.sh +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/setup.zsh /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./setup.zsh +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/cmake.lock /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/pkgconfig/catkin_tools_prebuild.pc +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake diff --git a/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml b/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml new file mode 100644 index 0000000..134c59a --- /dev/null +++ b/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml @@ -0,0 +1,10 @@ + + catkin_tools_prebuild + + This package is used to generate catkin setup files. + + 0.0.0 + BSD + jbohren + catkin + diff --git a/.catkin_tools/profiles/default/packages/jackal_description/devel_manifest.txt b/.catkin_tools/profiles/default/packages/jackal_description/devel_manifest.txt new file mode 100644 index 0000000..51fbf00 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/jackal_description/devel_manifest.txt @@ -0,0 +1,5 @@ +jackal_description +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/jackal_description/cmake.lock /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/jackal_description/lib/pkgconfig/jackal_description.pc /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/pkgconfig/jackal_description.pc +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/jackal_description/share/jackal_description/cmake/jackal_descriptionConfig.cmake /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/jackal_description/cmake/jackal_descriptionConfig.cmake +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/jackal_description/share/jackal_description/cmake/jackal_descriptionConfig-version.cmake /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/jackal_description/cmake/jackal_descriptionConfig-version.cmake diff --git a/.catkin_tools/profiles/default/packages/jackal_description/package.xml b/.catkin_tools/profiles/default/packages/jackal_description/package.xml new file mode 100644 index 0000000..f840763 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/jackal_description/package.xml @@ -0,0 +1,26 @@ + + + jackal_description + 0.8.6 + URDF robot description for Jackal + + Mike Purvis + + BSD + + Mike Purvis + + catkin + roslaunch + flir_camera_description + robot_state_publisher + urdf + xacro + lms1xx + pointgrey_camera_description + sick_tim + velodyne_description + + + + diff --git a/.catkin_tools/profiles/default/packages/me5413_world/devel_manifest.txt b/.catkin_tools/profiles/default/packages/me5413_world/devel_manifest.txt new file mode 100644 index 0000000..2cfc55e --- /dev/null +++ b/.catkin_tools/profiles/default/packages/me5413_world/devel_manifest.txt @@ -0,0 +1,19 @@ +me5413_world +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/cmake.lock /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/me5413_world/path_publisher_node /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/me5413_world/path_publisher_node +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/me5413_world/path_tracker_node /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/me5413_world/path_tracker_node +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/python3/dist-packages/me5413_world/__init__.py /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/python3/dist-packages/me5413_world/__init__.py +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/python3/dist-packages/me5413_world/cfg/__init__.py /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/python3/dist-packages/me5413_world/cfg/__init__.py +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/python3/dist-packages/me5413_world/cfg/path_trackerConfig.py /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/python3/dist-packages/me5413_world/cfg/path_trackerConfig.py +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/python3/dist-packages/me5413_world/cfg/path_publisherConfig.py /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/python3/dist-packages/me5413_world/cfg/path_publisherConfig.py +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/lib/pkgconfig/me5413_world.pc /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/lib/pkgconfig/me5413_world.pc +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/include/me5413_world/path_trackerConfig.h /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/include/me5413_world/path_trackerConfig.h +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/include/me5413_world/path_publisherConfig.h /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/include/me5413_world/path_publisherConfig.h +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/cmake/me5413_worldConfig-version.cmake /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/cmake/me5413_worldConfig-version.cmake +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/cmake/me5413_worldConfig.cmake /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/cmake/me5413_worldConfig.cmake +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/docs/path_publisherConfig.dox /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/docs/path_publisherConfig.dox +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/docs/path_trackerConfig-usage.dox /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/docs/path_trackerConfig-usage.dox +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/docs/path_trackerConfig.wikidoc /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/docs/path_trackerConfig.wikidoc +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/docs/path_publisherConfig-usage.dox /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/docs/path_publisherConfig-usage.dox +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/docs/path_publisherConfig.wikidoc /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/docs/path_publisherConfig.wikidoc +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/.private/me5413_world/share/me5413_world/docs/path_trackerConfig.dox /home/yash/ME5413/HW3/ME5413_Planning_Project/devel/share/me5413_world/docs/path_trackerConfig.dox diff --git a/.catkin_tools/profiles/default/packages/me5413_world/package.xml b/.catkin_tools/profiles/default/packages/me5413_world/package.xml new file mode 100644 index 0000000..49f4927 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/me5413_world/package.xml @@ -0,0 +1,75 @@ + + + me5413_world + 0.0.0 + The me5413_world package + + + + + shuo + + + + + + MIT + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + rospy + roscpp + rviz + std_msgs + nav_msgs + geometry_msgs + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs + gazebo_ros + jsk_rviz_plugins + jackal_gazebo + jackal_navigation + velodyne_simulator + dynamic_reconfigure + + + + + + + diff --git a/src/me5413_world/CMakeLists.txt b/src/me5413_world/CMakeLists.txt index 37361e4..7bea21f 100644 --- a/src/me5413_world/CMakeLists.txt +++ b/src/me5413_world/CMakeLists.txt @@ -51,3 +51,16 @@ add_dependencies(path_publisher_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME add_executable(path_tracker_node src/path_tracker_node.cpp) target_link_libraries(path_tracker_node ${catkin_LIBRARIES}) add_dependencies(path_tracker_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) + + +# add_executable(pure_pursuit_closest_point_node src/pure_pursuit_closest_point_approach.cpp) +# target_link_libraries(pure_pursuit_closest_point_node ${catkin_LIBRARIES}) +# add_dependencies(pure_pursuit_closest_point_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) + +# add_executable(pure_pursuit_linear_node src/pure_pursuit_linear_approach.cpp) +# target_link_libraries(pure_pursuit_linear_node ${catkin_LIBRARIES}) +# add_dependencies(pure_pursuit_linear_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) + +add_executable(pure_pursuit_target_point_node src/pure_pursuit_target_point_approach.cpp) +target_link_libraries(pure_pursuit_target_point_node ${catkin_LIBRARIES}) +add_dependencies(pure_pursuit_target_point_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) \ No newline at end of file diff --git a/src/me5413_world/cfg/path_tracker.cfg b/src/me5413_world/cfg/path_tracker.cfg index 3845318..f8a9b20 100644 --- a/src/me5413_world/cfg/path_tracker.cfg +++ b/src/me5413_world/cfg/path_tracker.cfg @@ -5,12 +5,9 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("speed_target", double_t, 1, "Default: 0.5[m/s]", 0.5, 0.1, 1.0) - -gen.add("PID_Kp", double_t, 1, "Default: 0.15", 0.5, 0, 10.0) -gen.add("PID_Ki", double_t, 1, "Default: 0.01", 0.2, 0, 10.0) -gen.add("PID_Kd", double_t, 1, "Default: 0.0", 0.2, 0, 10.0) - -gen.add("stanley_K", double_t, 1, "Default: 0.5", 0.5, 0, 10.0) +gen.add("max_throttle", double_t, 1, "Maximum throttle value. Default: 0.5", 0.5, 0.1, 10.0) +gen.add("throttle_gain", double_t, 1, "Throttle gain. Default: 0.1", 0.1, 0.1, 1.0) +gen.add("robot_length", double_t, 1, "Length of the robot (for pure pursuit algorithm). Default: 0.5", 0.5, 0.1, 10.0) +gen.add("lookahead_distance", double_t, 1, "Default lookahead distance. Default: 0.5", 0.5, 0.1, 10.0) exit(gen.generate(PACKAGE, "path_tracker_node", "path_tracker")) diff --git a/src/me5413_world/include/me5413_world/path_tracker_node.hpp b/src/me5413_world/include/me5413_world/path_tracker_node.hpp index 50c2a6e..f270f44 100644 --- a/src/me5413_world/include/me5413_world/path_tracker_node.hpp +++ b/src/me5413_world/include/me5413_world/path_tracker_node.hpp @@ -1,9 +1,9 @@ /** path_tracker_node.hpp - * + * * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * + * * MIT License - * + * * Declarations for PathTrackerNode class */ @@ -25,6 +25,8 @@ #include #include +#include "angles/angles.h" +#include #include #include #include @@ -35,9 +37,7 @@ #include #include -#include "me5413_world/pid.hpp" - -namespace me5413_world +namespace me5413_world { class PathTrackerNode @@ -48,16 +48,17 @@ class PathTrackerNode private: void robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom); - void goalPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& goal_pose); void localPathCallback(const nav_msgs::Path::ConstPtr& path); tf2::Transform convertPoseToTransform(const geometry_msgs::Pose& pose); - double computeStanelyControl(const double heading_error, const double cross_track_error, const double velocity); geometry_msgs::Twist computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); - + double computeDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2); + double computeThrottle(double distance_to_goal); + double computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); + double computeLookaheadDistance(const nav_msgs::Odometry& odom_robot); + // ROS declaration ros::NodeHandle nh_; - ros::Timer timer_; ros::Subscriber sub_robot_odom_; ros::Subscriber sub_local_path_; ros::Publisher pub_cmd_vel_; @@ -73,11 +74,8 @@ class PathTrackerNode std::string robot_frame_; nav_msgs::Odometry odom_world_robot_; geometry_msgs::Pose pose_world_goal_; - - // Controllers - control::PID pid_; }; } // namespace me5413_world -#endif // PATH_TRACKER_NODE_H_ +#endif // PATH_TRACKER_NODE_H_ \ No newline at end of file diff --git a/src/me5413_world/launch/path_tracking.launch b/src/me5413_world/launch/path_tracking.launch index 132f2c5..6e4d926 100644 --- a/src/me5413_world/launch/path_tracking.launch +++ b/src/me5413_world/launch/path_tracking.launch @@ -1,7 +1,7 @@ - + diff --git a/src/me5413_world/src/path_publisher_node.cpp b/src/me5413_world/src/path_publisher_node.cpp index 290ed7c..91a353e 100644 --- a/src/me5413_world/src/path_publisher_node.cpp +++ b/src/me5413_world/src/path_publisher_node.cpp @@ -181,7 +181,7 @@ void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, else if (id_next >= this->global_path_msg_.poses.size() - 1) { ROS_WARN("Robot has reached the end of the track, please restart"); - } + } else { this->current_id_ = std::max(this->current_id_, id_next - 1); diff --git a/src/me5413_world/src/path_tracker_node.cpp b/src/me5413_world/src/path_tracker_node.cpp index 19ab019..4520f18 100644 --- a/src/me5413_world/src/path_tracker_node.cpp +++ b/src/me5413_world/src/path_tracker_node.cpp @@ -1,37 +1,31 @@ /** path_tracker_node.cpp - * + * * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * + * * MIT License - * + * * ROS Node for robot to track a given path */ -#include "me5413_world/math_utils.hpp" #include "me5413_world/path_tracker_node.hpp" -namespace me5413_world +namespace me5413_world { // Dynamic Parameters -double SPEED_TARGET; -double PID_Kp, PID_Ki, PID_Kd; -double STANLEY_K; -bool PARAMS_UPDATED; +double MAX_THROTTLE; +double THROTTLE_GAIN; +double ROBOT_LENGTH; +bool DEFAULT_LOOKAHEAD_DISTANCE; -void dynamicParamCallback(const me5413_world::path_trackerConfig& config, uint32_t level) +void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) { - // Common Params - SPEED_TARGET = config.speed_target; - // PID - PID_Kp = config.PID_Kp; - PID_Ki = config.PID_Ki; - PID_Kd = config.PID_Kd; - // Stanley - STANLEY_K = config.stanley_K; - - PARAMS_UPDATED = true; -} + + MAX_THROTTLE = config.max_throttle; + THROTTLE_GAIN = config.throttle_gain; + ROBOT_LENGTH = config.robot_length; + DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; +}; PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) { @@ -45,9 +39,7 @@ PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) // Initialization this->robot_frame_ = "base_link"; this->world_frame_ = "world"; - - this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); -} +}; void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) { @@ -56,7 +48,7 @@ void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); return; -} +}; void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) { @@ -65,57 +57,97 @@ void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom this->odom_world_robot_ = *odom.get(); return; -} +}; -double PathTrackerNode::computeStanelyControl(const double heading_error, const double cross_track_error, const double velocity) +geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) { - const double stanley_output = -1.0*(heading_error + std::atan2(STANLEY_K*cross_track_error, std::max(velocity, 0.3))); + //Implement Pure Pursuit Controller for Throttle + double distance_to_goal = computeDistance(odom_robot.pose.pose.position, pose_goal.position); + ROS_INFO("distance to goal: %f", distance_to_goal); + double throttle = computeThrottle(distance_to_goal); - return std::min(std::max(stanley_output, -2.2), 2.2); + //Implement Pure Pursuit Controller for Steering + double steering = computeSteering(odom_robot, pose_goal); + + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = throttle; + cmd_vel.angular.z = steering; + + // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; + // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; + + return cmd_vel; } -geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +double PathTrackerNode::computeDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { - // Heading Error - tf2::Quaternion q_robot, q_goal; - tf2::fromMsg(odom_robot.pose.pose.orientation, q_robot); - tf2::fromMsg(pose_goal.orientation, q_goal); - const tf2::Matrix3x3 m_robot = tf2::Matrix3x3(q_robot); - const tf2::Matrix3x3 m_goal = tf2::Matrix3x3(q_goal); + double dx = p2.x - p1.x; + double dy = p2.y - p1.y; + return std::sqrt(dx * dx + dy * dy); +} - double roll, pitch, yaw_robot, yaw_goal; - m_robot.getRPY(roll, pitch, yaw_robot); - m_goal.getRPY(roll, pitch, yaw_goal); +double PathTrackerNode::computeThrottle(double distance_to_goal) +{ + //adjust throttle based on distance to goal + return std::min(MAX_THROTTLE, distance_to_goal * THROTTLE_GAIN); +} - const double heading_error = unifyAngleRange(yaw_robot - yaw_goal); +double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Compute heading error + double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); + double yaw_goal = tf2::getYaw(pose_goal.orientation); + double heading_error = yaw_goal - yaw_robot; - // Lateral Error + // Compute lateral error tf2::Vector3 point_robot, point_goal; tf2::fromMsg(odom_robot.pose.pose.position, point_robot); tf2::fromMsg(pose_goal.position, point_goal); - const tf2::Vector3 V_goal_robot = point_robot - point_goal; - const double angle_goal_robot = std::atan2(V_goal_robot.getY(), V_goal_robot.getX()); - const double angle_diff = angle_goal_robot - yaw_goal; - const double lat_error = V_goal_robot.length()*std::sin(angle_diff); + double dx = point_goal.getX() - point_robot.getX(); + double dy = point_goal.getY() - point_robot.getY(); + double cross_track_error = std::sqrt(dx * dx + dy * dy); + + // Compute alpha (angle between the goal point and the path point) + double alpha = std::atan2(dy, dx) - yaw_robot; + + // Ensure alpha is within the range [-pi/2, pi/2] + if (alpha > M_PI / 2) { + alpha -= M_PI; + } else if (alpha < -M_PI / 2) { + alpha += M_PI; + } - // Velocity - tf2::Vector3 robot_vel; - tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); - const double velocity = robot_vel.length(); + // Compute lookahead distance + double lookahead_distance = computeLookaheadDistance(odom_robot); - geometry_msgs::Twist cmd_vel; - if (PARAMS_UPDATED) - { - this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); - PARAMS_UPDATED = false; + // Compute steering angle using the pure pursuit formula + double steering = std::atan((2.0 * ROBOT_LENGTH * cross_track_error) / lookahead_distance) + alpha; + + // Limit the steering angle to a maximum value + const double MAX_STEERING_ANGLE = 0.5; + if (steering > MAX_STEERING_ANGLE) { + steering = MAX_STEERING_ANGLE; + } else if (steering < -MAX_STEERING_ANGLE) { + steering = -MAX_STEERING_ANGLE; } - cmd_vel.linear.x = this->pid_.calculate(SPEED_TARGET, velocity); - cmd_vel.angular.z = computeStanelyControl(heading_error, lat_error, velocity); - // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; - // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; + // Debug output + ROS_INFO("Yaw Robot: %f", yaw_robot); + ROS_INFO("Yaw Goal: %f", yaw_goal); + ROS_INFO("Heading Error: %f", heading_error); + ROS_INFO("Cross-track Error: %f", cross_track_error); + ROS_INFO("Alpha: %f", alpha); + ROS_INFO("Lookahead Distance: %f", lookahead_distance); + ROS_INFO("Computed Steering Angle: %f", steering); + ROS_INFO("Throttle: %f", computeThrottle(computeDistance(odom_robot.pose.pose.position, pose_goal.position))); + + return steering; +} - return cmd_vel; +double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) +{ + double velocity = odom_robot.twist.twist.linear.x; + return std::max(1.0, velocity * DEFAULT_LOOKAHEAD_DISTANCE); } } // namespace me5413_world @@ -126,4 +158,4 @@ int main(int argc, char** argv) me5413_world::PathTrackerNode path_tracker_node; ros::spin(); // spin the ros node. return 0; -} +} \ No newline at end of file diff --git a/src/me5413_world/src/pure_pursuit_closest_point_approach.cpp b/src/me5413_world/src/pure_pursuit_closest_point_approach.cpp new file mode 100644 index 0000000..f97dd5f --- /dev/null +++ b/src/me5413_world/src/pure_pursuit_closest_point_approach.cpp @@ -0,0 +1,201 @@ +/** path_tracker_node.cpp + * + * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore + * + * MIT License + * + * ROS Node for robot to track a given path + */ + +#include "me5413_world/path_tracker_node.hpp" +#include "me5413_world/math_utils.hpp" + +namespace me5413_world +{ + +// Dynamic Parameters +double SPEED_TARGET; +double PID_Kp, PID_Ki, PID_Kd; +double ROBOT_LENGTH; +bool DEFAULT_LOOKAHEAD_DISTANCE; +bool PARAMS_UPDATED; + +void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) +{ + SPEED_TARGET = config.speed_target; + PID_Kp = config.PID_Kp; + PID_Ki = config.PID_Ki; + PID_Kd = config.PID_Kd; + ROBOT_LENGTH = config.robot_length; + DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; + + PARAMS_UPDATED = true; +}; + +PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_), path_points_() +{ + f = boost::bind(&dynamicParamCallback, _1, _2); + server.setCallback(f); + + this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); + this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); + this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); + + // Initialization + this->robot_frame_ = "base_link"; + this->world_frame_ = "world"; + + this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); +}; + +void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) +{ + // Extract waypoints from the received path message + std::vector waypoints = path->poses; + + //std::cout << "number of waypoints: " << waypoints.size() << std::endl; + + // Ensure that the received path message contains at least one waypoint + if (waypoints.empty()) { + ROS_WARN("Received empty path message."); + return; + } + + // Use the last waypoint as the goal pose + this->pose_world_goal_ = path->poses[15].pose; + + //std::cout << "goal pose :" << pose_world_goal_ << std::endl; + + // Store the waypoints for later use in computing the closest point on the path + this->path_points_.clear(); + for (const auto& waypoint : waypoints) { + tf2::Vector3 point; + tf2::fromMsg(waypoint.pose.position, point); + this->path_points_.push_back(point); + //std::cout << "path points :" << point* << std::endl; + } + + // Publish control outputs + this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); +}; + +void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) +{ + this->world_frame_ = odom->header.frame_id; + this->robot_frame_ = odom->child_frame_id; + this->odom_world_robot_ = *odom.get(); + + return; +}; + +geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Velocity + tf2::Vector3 robot_vel; + tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); + const double velocity = robot_vel.length(); + + // Update PID controller parameters if they are updated dynamically + if (PARAMS_UPDATED) + { + this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); + PARAMS_UPDATED = false; + } + + // Compute linear speed using PID controller + double target_speed = SPEED_TARGET; + double linear_speed = this->pid_.calculate(target_speed, velocity); + + //Implement Pure Pursuit Controller for Steering + double steering = computeSteering(odom_robot, pose_goal); + + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = linear_speed; + cmd_vel.angular.z = steering; + + // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; + // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; + + return cmd_vel; +} + +double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Compute heading error + double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); + double yaw_goal = tf2::getYaw(pose_goal.orientation); + double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] + + // Compute lateral error + tf2::Vector3 point_robot, point_goal; + tf2::fromMsg(odom_robot.pose.pose.position, point_robot); + tf2::fromMsg(pose_goal.position, point_goal); + + // Compute lookahead distance + double lookahead_distance = computeLookaheadDistance(odom_robot); + + // Find the closest point on the path to the computed target point + tf2::Vector3 target_point = findClosestPointOnPath(point_robot, path_points_, lookahead_distance); + + // Compute alpha (angle between the goal point and the path point) + double dx = target_point.getX() - point_robot.getX(); + double dy = target_point.getY() - point_robot.getY(); + double alpha = unifyAngleRange(std::atan2(dy, dx) - yaw_robot); + + // Compute desired steering angle using the pure pursuit formula + double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); + + // Incorporate heading error + steering += heading_error; + + // Ensure steering angle is within [-pi, pi] + steering = unifyAngleRange(steering); + +return steering; +} + +double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) +{ + double velocity = odom_robot.twist.twist.linear.x; + return std::max(1.0, velocity * DEFAULT_LOOKAHEAD_DISTANCE); +} + +tf2::Vector3 PathTrackerNode::findClosestPointOnPath(const tf2::Vector3& point_robot, const std::vector& path_points, double lookahead_distance) +{ + // Check if the path is empty + if (path_points.empty()) { + ROS_WARN("Path is empty."); + return tf2::Vector3(); // Return a default vector + } + + // Initialize variables to store the closest point and its distance + tf2::Vector3 closest_point = path_points.front(); // Start with the first point in the path + double min_distance = (point_robot - closest_point).length(); // Calculate initial distance + + // Iterate over path points + for (const auto& path_point : path_points) { + // Calculate distance between the robot's position and the path point + double distance = (point_robot - path_point).length(); + + // Check if the distance is within the lookahead distance + if (distance <= lookahead_distance) { + // Update closest point if this point is closer than the current closest point + if (distance < min_distance) { + min_distance = distance; + closest_point = path_point; + } + } + } + //std::cout << "closest_point: " << closest_point << std::endl; + return closest_point; +} + +} // namespace me5413_world + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "path_tracker_node"); + me5413_world::PathTrackerNode path_tracker_node; + ros::spin(); // spin the ros node. + return 0; +} diff --git a/src/me5413_world/src/pure_pursuit_linear_approach.cpp b/src/me5413_world/src/pure_pursuit_linear_approach.cpp new file mode 100644 index 0000000..dcfdea7 --- /dev/null +++ b/src/me5413_world/src/pure_pursuit_linear_approach.cpp @@ -0,0 +1,215 @@ +/** path_tracker_node.cpp + * + * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore + * + * MIT License + * + * ROS Node for robot to track a given path + */ + +#include "me5413_world/path_tracker_node.hpp" +#include "me5413_world/math_utils.hpp" + +namespace me5413_world +{ + +// Dynamic Parameters +double SPEED_TARGET; +double PID_Kp, PID_Ki, PID_Kd; +double ROBOT_LENGTH; +bool DEFAULT_LOOKAHEAD_DISTANCE; +bool PARAMS_UPDATED; + +void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) +{ + SPEED_TARGET = config.speed_target; + PID_Kp = config.PID_Kp; + PID_Ki = config.PID_Ki; + PID_Kd = config.PID_Kd; + ROBOT_LENGTH = config.robot_length; + DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; + + PARAMS_UPDATED = true; +}; + +PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_), path_points_() +{ + f = boost::bind(&dynamicParamCallback, _1, _2); + server.setCallback(f); + + this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); + this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); + this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); + + // Initialization + this->robot_frame_ = "base_link"; + this->world_frame_ = "world"; + + this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); +}; + +void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) +{ + // Extract waypoints from the received path message + std::vector waypoints = path->poses; + + //std::cout << "number of waypoints: " << waypoints.size() << std::endl; + + // Ensure that the received path message contains at least one waypoint + if (waypoints.empty()) { + ROS_WARN("Received empty path message."); + return; + } + + // Use the last waypoint as the goal pose + this->pose_world_goal_ = path->poses[30].pose; + + //std::cout << "goal pose :" << pose_world_goal_ << std::endl; + + // Store the waypoints for later use in computing the closest point on the path + this->path_points_.clear(); + for (const auto& waypoint : waypoints) { + tf2::Vector3 point; + tf2::fromMsg(waypoint.pose.position, point); + this->path_points_.push_back(point); + //std::cout << "path points :" << point* << std::endl; + } + + // Publish control outputs + this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); +}; + +void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) +{ + this->world_frame_ = odom->header.frame_id; + this->robot_frame_ = odom->child_frame_id; + this->odom_world_robot_ = *odom.get(); + + return; +}; + +geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Velocity + tf2::Vector3 robot_vel; + tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); + const double velocity = robot_vel.length(); + + // Update PID controller parameters if they are updated dynamically + if (PARAMS_UPDATED) + { + this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); + PARAMS_UPDATED = false; + } + + // Compute linear speed using PID controller + double target_speed = SPEED_TARGET; + double linear_speed = this->pid_.calculate(target_speed, velocity); + + //Implement Pure Pursuit Controller for Steering + double steering = computeSteering(odom_robot, pose_goal); + + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = linear_speed; + cmd_vel.angular.z = steering; + + // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; + // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; + + return cmd_vel; +} + +double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Compute heading error + double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); + double yaw_goal = tf2::getYaw(pose_goal.orientation); + double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] + + // Compute lateral error + tf2::Vector3 point_robot, point_goal; + tf2::fromMsg(odom_robot.pose.pose.position, point_robot); + tf2::fromMsg(pose_goal.position, point_goal); + + // Compute lookahead distance + double lookahead_distance = computeLookaheadDistance(odom_robot); + + // Find the closest point on the path to the computed target point + tf2::Vector3 target_point = findClosestPointOnPath(point_robot, path_points_, lookahead_distance); + + // Compute alpha (angle between the goal point and the path point) + double dx = target_point.getX() - point_robot.getX(); + double dy = target_point.getY() - point_robot.getY(); + double alpha = unifyAngleRange(std::atan2(dy, dx) - yaw_robot); + + // Compute desired steering angle using the pure pursuit formula + double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); + + // Incorporate heading error + steering += heading_error; + + // Ensure steering angle is within [-pi, pi] + steering = unifyAngleRange(steering); + +return steering; +} + +double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) +{ + double velocity = odom_robot.twist.twist.linear.x; + return std::max(0.1, velocity * DEFAULT_LOOKAHEAD_DISTANCE); +} + +tf2::Vector3 PathTrackerNode::findClosestPointOnPath(const tf2::Vector3& point_robot, const std::vector& path_points, double lookahead_distance) +{ + // Check if the path is empty + if (path_points.empty()) { + ROS_WARN("Path is empty."); + return tf2::Vector3(); + } + + // Target point initialization + tf2::Vector3 target_point; + + // Iterate over path segments + for (int i = 0; i < path_points.size() - 1; ++i) { + // Calculate distance along the current segment from the robot + tf2::Vector3 segment_direction = path_points[i + 1] - path_points[i]; + double segment_length = segment_direction.length(); + segment_direction /= segment_length; // Normalize direction vector + + tf2::Vector3 relative_robot_position = point_robot - path_points[i]; + double projection = relative_robot_position.dot(segment_direction); + + // Check if projection is within segment bounds (0 to segment_length) + if (projection >= 0 && projection <= segment_length) { + // Calculate distance traveled along the segment + double distance_traveled = projection; + + // Check if distance traveled is within lookahead distance + if (distance_traveled <= lookahead_distance) { + // Calculate target point on the segment + target_point = path_points[i] + distance_traveled * segment_direction; + std::cout << "get target point: (" << target_point.getX() << ", " << target_point.getY() << ")" << std::endl; + break; // Exit loop as we found a point within lookahead distance + } + } + } + + // Handle no intersection case + if (target_point.getX() == 0 && target_point.getY() == 0 && target_point.getZ() == 0) { + ROS_WARN("No point found within lookahead distance on the path."); + } + + return target_point; +} + +} // namespace me5413_world + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "path_tracker_node"); + me5413_world::PathTrackerNode path_tracker_node; + ros::spin(); // spin the ros node. + return 0; +} diff --git a/src/me5413_world/src/pure_pursuit_target_point_approach.cpp b/src/me5413_world/src/pure_pursuit_target_point_approach.cpp new file mode 100644 index 0000000..6bce38b --- /dev/null +++ b/src/me5413_world/src/pure_pursuit_target_point_approach.cpp @@ -0,0 +1,144 @@ +/** path_tracker_node.cpp + * + * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore + * + * MIT License + * + * ROS Node for robot to track a given path + */ + +#include "me5413_world/path_tracker_node.hpp" +#include "me5413_world/math_utils.hpp" + +namespace me5413_world +{ + +// Dynamic Parameters +double SPEED_TARGET; +double PID_Kp, PID_Ki, PID_Kd; +double ROBOT_LENGTH; +bool DEFAULT_LOOKAHEAD_DISTANCE; +bool PARAMS_UPDATED; + +void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) +{ + SPEED_TARGET = config.speed_target; + PID_Kp = config.PID_Kp; + PID_Ki = config.PID_Ki; + PID_Kd = config.PID_Kd; + ROBOT_LENGTH = config.robot_length; + DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; + + PARAMS_UPDATED = true; +}; + +PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) +{ + f = boost::bind(&dynamicParamCallback, _1, _2); + server.setCallback(f); + + this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); + this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); + this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); + + // Initialization + this->robot_frame_ = "base_link"; + this->world_frame_ = "world"; + + this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); +}; + +void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) +{ + // Calculate absolute errors (wrt to world frame) + this->pose_world_goal_ = path->poses[11].pose; + this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); + + return; +}; + +void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) +{ + this->world_frame_ = odom->header.frame_id; + this->robot_frame_ = odom->child_frame_id; + this->odom_world_robot_ = *odom.get(); + + return; +}; + +geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Velocity + tf2::Vector3 robot_vel; + tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); + const double velocity = robot_vel.length(); + + // Update PID controller parameters if they are updated dynamically + if (PARAMS_UPDATED) + { + this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); + PARAMS_UPDATED = false; + } + + // Compute linear speed using PID controller + double target_speed = SPEED_TARGET; + double linear_speed = this->pid_.calculate(target_speed, velocity); + + //Implement Pure Pursuit Controller for Steering + double steering = computeSteering(odom_robot, pose_goal); + + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = linear_speed; + cmd_vel.angular.z = steering; + + // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; + // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; + + return cmd_vel; +} + +double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) +{ + // Compute heading error + double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); + double yaw_goal = tf2::getYaw(pose_goal.orientation); + double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] + + // Compute lateral error + tf2::Vector3 point_robot, point_goal; + tf2::fromMsg(odom_robot.pose.pose.position, point_robot); + tf2::fromMsg(pose_goal.position, point_goal); + double dx = point_goal.getX() - point_robot.getX(); + double dy = point_goal.getY() - point_robot.getY(); + double alpha = std::atan2(dy, dx) - yaw_robot; + + // Compute lookahead distance + double lookahead_distance = computeLookaheadDistance(odom_robot); + + // Compute desired steering angle using the pure pursuit formula + double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); + + // Incorporate heading error + steering += heading_error; + + // Ensure steering angle is within [-pi, pi] + steering = unifyAngleRange(steering); + +return steering; +} + +double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) +{ + double velocity = odom_robot.twist.twist.linear.x; + return std::max(1.0, velocity * DEFAULT_LOOKAHEAD_DISTANCE); +} + +} // namespace me5413_world + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "path_tracker_node"); + me5413_world::PathTrackerNode path_tracker_node; + ros::spin(); // spin the ros node. + return 0; +} From e6f9516737a1287af782f5787ddb99effdcae8a9 Mon Sep 17 00:00:00 2001 From: Yash Agarwal Date: Thu, 28 Mar 2024 12:51:34 +0800 Subject: [PATCH 2/2] code init --- .../profiles/default/devel_collisions.txt | 2 +- src/me5413_world/CMakeLists.txt | 13 -- src/me5413_world/cfg/path_publisher.cfg | 2 +- src/me5413_world/cfg/path_tracker.cfg | 6 +- .../me5413_world/path_publisher_node.hpp | 8 +- .../me5413_world/path_tracker_node.hpp | 10 +- src/me5413_world/include/me5413_world/pid.hpp | 10 +- src/me5413_world/src/path_publisher_node.cpp | 45 ++-- src/me5413_world/src/path_tracker_node.cpp | 91 +++----- .../pure_pursuit_closest_point_approach.cpp | 201 ---------------- .../src/pure_pursuit_linear_approach.cpp | 215 ------------------ .../pure_pursuit_target_point_approach.cpp | 144 ------------ 12 files changed, 83 insertions(+), 664 deletions(-) delete mode 100644 src/me5413_world/src/pure_pursuit_closest_point_approach.cpp delete mode 100644 src/me5413_world/src/pure_pursuit_linear_approach.cpp delete mode 100644 src/me5413_world/src/pure_pursuit_target_point_approach.cpp diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index f8b7411..f8f3125 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1 +1 @@ -/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock 6 +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock 8 diff --git a/src/me5413_world/CMakeLists.txt b/src/me5413_world/CMakeLists.txt index 7bea21f..37361e4 100644 --- a/src/me5413_world/CMakeLists.txt +++ b/src/me5413_world/CMakeLists.txt @@ -51,16 +51,3 @@ add_dependencies(path_publisher_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME add_executable(path_tracker_node src/path_tracker_node.cpp) target_link_libraries(path_tracker_node ${catkin_LIBRARIES}) add_dependencies(path_tracker_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) - - -# add_executable(pure_pursuit_closest_point_node src/pure_pursuit_closest_point_approach.cpp) -# target_link_libraries(pure_pursuit_closest_point_node ${catkin_LIBRARIES}) -# add_dependencies(pure_pursuit_closest_point_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) - -# add_executable(pure_pursuit_linear_node src/pure_pursuit_linear_approach.cpp) -# target_link_libraries(pure_pursuit_linear_node ${catkin_LIBRARIES}) -# add_dependencies(pure_pursuit_linear_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) - -add_executable(pure_pursuit_target_point_node src/pure_pursuit_target_point_approach.cpp) -target_link_libraries(pure_pursuit_target_point_node ${catkin_LIBRARIES}) -add_dependencies(pure_pursuit_target_point_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) \ No newline at end of file diff --git a/src/me5413_world/cfg/path_publisher.cfg b/src/me5413_world/cfg/path_publisher.cfg index e7d1fd9..b4d3a2b 100644 --- a/src/me5413_world/cfg/path_publisher.cfg +++ b/src/me5413_world/cfg/path_publisher.cfg @@ -13,4 +13,4 @@ gen.add("track_wp_num", int_t, 1, "Default: 500", 500, 100, 2000) gen.add("local_prev_wp_num", int_t, 1, "Default: 10", 10, 1, 20) gen.add("local_next_wp_num", int_t, 1, "Default: 50", 50, 5, 200) -exit(gen.generate(PACKAGE, "path_publisher_node", "path_publisher")) +exit(gen.generate(PACKAGE, "path_publisher_node", "path_publisher")) \ No newline at end of file diff --git a/src/me5413_world/cfg/path_tracker.cfg b/src/me5413_world/cfg/path_tracker.cfg index f8a9b20..a1e7dea 100644 --- a/src/me5413_world/cfg/path_tracker.cfg +++ b/src/me5413_world/cfg/path_tracker.cfg @@ -5,8 +5,10 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("max_throttle", double_t, 1, "Maximum throttle value. Default: 0.5", 0.5, 0.1, 10.0) -gen.add("throttle_gain", double_t, 1, "Throttle gain. Default: 0.1", 0.1, 0.1, 1.0) +gen.add("speed_target", double_t, 1, "Default: 0.5[m/s]", 0.5, 0.1, 1.0) +gen.add("PID_Kp", double_t, 1, "Default: 0.15", 0.5, 0, 10.0) +gen.add("PID_Ki", double_t, 1, "Default: 0.01", 0.2, 0, 10.0) +gen.add("PID_Kd", double_t, 1, "Default: 0.0", 0.2, 0, 10.0) gen.add("robot_length", double_t, 1, "Length of the robot (for pure pursuit algorithm). Default: 0.5", 0.5, 0.1, 10.0) gen.add("lookahead_distance", double_t, 1, "Default lookahead distance. Default: 0.5", 0.5, 0.1, 10.0) diff --git a/src/me5413_world/include/me5413_world/path_publisher_node.hpp b/src/me5413_world/include/me5413_world/path_publisher_node.hpp index dea31dd..fdb45d4 100644 --- a/src/me5413_world/include/me5413_world/path_publisher_node.hpp +++ b/src/me5413_world/include/me5413_world/path_publisher_node.hpp @@ -1,9 +1,9 @@ /** path_publisher_node.hpp - * + * * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * + * * MIT License - * + * * Declarations for PathPublisherNode class */ @@ -103,4 +103,4 @@ class PathPublisherNode } // namespace me5413_world -#endif // PATH_PUBLISHER_NODE_H_ +#endif // PATH_PUBLISHER_NODE_H_ \ No newline at end of file diff --git a/src/me5413_world/include/me5413_world/path_tracker_node.hpp b/src/me5413_world/include/me5413_world/path_tracker_node.hpp index f270f44..93b6905 100644 --- a/src/me5413_world/include/me5413_world/path_tracker_node.hpp +++ b/src/me5413_world/include/me5413_world/path_tracker_node.hpp @@ -37,6 +37,8 @@ #include #include +#include "me5413_world/pid.hpp" + namespace me5413_world { @@ -52,10 +54,9 @@ class PathTrackerNode tf2::Transform convertPoseToTransform(const geometry_msgs::Pose& pose); geometry_msgs::Twist computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); - double computeDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2); - double computeThrottle(double distance_to_goal); double computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); double computeLookaheadDistance(const nav_msgs::Odometry& odom_robot); + //tf2::Vector3 findClosestPointOnPath(const tf2::Vector3& point_robot, const std::vector& path_points, double lookahead_distance); // ROS declaration ros::NodeHandle nh_; @@ -74,6 +75,11 @@ class PathTrackerNode std::string robot_frame_; nav_msgs::Odometry odom_world_robot_; geometry_msgs::Pose pose_world_goal_; + + // Controllers + control::PID pid_; + + // std::vector path_points_; }; } // namespace me5413_world diff --git a/src/me5413_world/include/me5413_world/pid.hpp b/src/me5413_world/include/me5413_world/pid.hpp index 6263a50..db8f44f 100644 --- a/src/me5413_world/include/me5413_world/pid.hpp +++ b/src/me5413_world/include/me5413_world/pid.hpp @@ -1,9 +1,9 @@ /** pid.hpp - * + * * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * + * * MIT License - * + * * Implementation of PID controller */ @@ -44,7 +44,7 @@ PID::PID(double dt, double max, double min, double Kp, double Kd, double Ki) : Kd_(Kd), Ki_(Ki), pre_error_(0), - integral_(0) + integral_(0) {}; void PID::updateSettings(const double Kp, const double Kd, const double Ki) @@ -83,4 +83,4 @@ double PID::calculate(const double setpoint, const double pv) return output; }; -} // namespace control +} // namespace control \ No newline at end of file diff --git a/src/me5413_world/src/path_publisher_node.cpp b/src/me5413_world/src/path_publisher_node.cpp index 91a353e..1407ad5 100644 --- a/src/me5413_world/src/path_publisher_node.cpp +++ b/src/me5413_world/src/path_publisher_node.cpp @@ -1,13 +1,12 @@ /** path_publisher_node.cpp * * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * + * * MIT License - * + * * ROS Node for publishing short term paths */ -#include "me5413_world/math_utils.hpp" #include "me5413_world/path_publisher_node.hpp" namespace me5413_world @@ -22,7 +21,7 @@ double LOCAL_PREV_WP_NUM; double LOCAL_NEXT_WP_NUM; bool PARAMS_UPDATED = false; -void dynamicParamCallback(const me5413_world::path_publisherConfig& config, uint32_t level) +void dynamicParamCallback(me5413_world::path_publisherConfig& config, uint32_t level) { // Common Params SPEED_TARGET = config.speed_target; @@ -33,7 +32,7 @@ void dynamicParamCallback(const me5413_world::path_publisherConfig& config, uint LOCAL_PREV_WP_NUM = config.local_prev_wp_num; LOCAL_NEXT_WP_NUM = config.local_next_wp_num; PARAMS_UPDATED = true; -} +}; PathPublisherNode::PathPublisherNode() : tf2_listener_(tf2_buffer_) { @@ -68,7 +67,7 @@ PathPublisherNode::PathPublisherNode() : tf2_listener_(tf2_buffer_) this->num_time_steps_ = 1; this->sum_sqr_position_error_ = 0.0; this->sum_sqr_heading_error_ = 0.0; -} +}; void PathPublisherNode::timerCallback(const ros::TimerEvent &) { @@ -110,7 +109,7 @@ void PathPublisherNode::timerCallback(const ros::TimerEvent &) this->num_time_steps_++; return; -} +}; void PathPublisherNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr &odom) { @@ -131,7 +130,7 @@ void PathPublisherNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr &od this->tf2_bcaster_.sendTransform(transformStamped); return; -} +}; std::vector PathPublisherNode::createGlobalPath(const double A, const double B, const double t_res) { @@ -162,14 +161,14 @@ std::vector PathPublisherNode::createGlobalPath(cons poses.back().pose.orientation = tf2::toMsg(q); return poses; -} +}; void PathPublisherNode::publishGlobalPath() { // Update the message this->global_path_msg_.header.stamp = ros::Time::now(); this->pub_global_path_.publish(this->global_path_msg_); -} +}; void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, const int n_wp_prev, const int n_wp_post) { @@ -181,7 +180,7 @@ void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, else if (id_next >= this->global_path_msg_.poses.size() - 1) { ROS_WARN("Robot has reached the end of the track, please restart"); - } + } else { this->current_id_ = std::max(this->current_id_, id_next - 1); @@ -197,10 +196,12 @@ void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, this->pub_local_path_.publish(this->local_path_msg_); this->pose_world_goal_ = this->local_path_msg_.poses[n_wp_prev].pose; } -} +}; int PathPublisherNode::closestWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const int id_start = 0) { + const double yaw_robot = getYawFromOrientation(robot_pose.orientation); + double min_dist = DBL_MAX; int id_closest = id_start; for (int i = id_start; i < path.poses.size(); i++) @@ -219,7 +220,7 @@ int PathPublisherNode::closestWaypoint(const geometry_msgs::Pose &robot_pose, co } return id_closest; -} +}; int PathPublisherNode::nextWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const int id_start = 0) { @@ -237,7 +238,7 @@ int PathPublisherNode::nextWaypoint(const geometry_msgs::Pose &robot_pose, const } return id_closest; -} +}; double PathPublisherNode::getYawFromOrientation(const geometry_msgs::Quaternion &orientation) { @@ -248,7 +249,7 @@ double PathPublisherNode::getYawFromOrientation(const geometry_msgs::Quaternion m.getRPY(roll, pitch, yaw); return yaw; -} +}; tf2::Transform PathPublisherNode::convertPoseToTransform(const geometry_msgs::Pose &pose) { @@ -259,13 +260,13 @@ tf2::Transform PathPublisherNode::convertPoseToTransform(const geometry_msgs::Po T.setRotation(q); return T; -} +}; std::pair PathPublisherNode::calculatePoseError(const geometry_msgs::Pose &pose_robot, const geometry_msgs::Pose &pose_goal) { // Positional Error const double position_error = std::hypot( - pose_robot.position.x - pose_goal.position.x, + pose_robot.position.x - pose_goal.position.x, pose_robot.position.y - pose_goal.position.y ); @@ -280,13 +281,13 @@ std::pair PathPublisherNode::calculatePoseError(const geometry_m m_robot.getRPY(roll, pitch, yaw_robot); m_goal.getRPY(roll, pitch, yaw_wp); - const double heading_error = unifyAngleRange(yaw_robot - yaw_wp) / M_PI * 180.0; + const double heading_error = (yaw_robot - yaw_wp) / M_PI * 180.0; return std::pair( - position_error, - isLegal(heading_error)? heading_error : 0.0 + position_error, + isnan(heading_error)? 0.0 : heading_error ); -} +}; } // namespace me5413_world @@ -296,4 +297,4 @@ int main(int argc, char **argv) me5413_world::PathPublisherNode path_publisher_node; ros::spin(); // spin the ros node. return 0; -} +}; \ No newline at end of file diff --git a/src/me5413_world/src/path_tracker_node.cpp b/src/me5413_world/src/path_tracker_node.cpp index 4520f18..02bfebd 100644 --- a/src/me5413_world/src/path_tracker_node.cpp +++ b/src/me5413_world/src/path_tracker_node.cpp @@ -8,23 +8,28 @@ */ #include "me5413_world/path_tracker_node.hpp" +#include "me5413_world/math_utils.hpp" namespace me5413_world { // Dynamic Parameters -double MAX_THROTTLE; -double THROTTLE_GAIN; +double SPEED_TARGET; +double PID_Kp, PID_Ki, PID_Kd; double ROBOT_LENGTH; bool DEFAULT_LOOKAHEAD_DISTANCE; +bool PARAMS_UPDATED; void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) { - - MAX_THROTTLE = config.max_throttle; - THROTTLE_GAIN = config.throttle_gain; + SPEED_TARGET = config.speed_target; + PID_Kp = config.PID_Kp; + PID_Ki = config.PID_Ki; + PID_Kd = config.PID_Kd; ROBOT_LENGTH = config.robot_length; DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; + + PARAMS_UPDATED = true; }; PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) @@ -39,6 +44,8 @@ PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) // Initialization this->robot_frame_ = "base_link"; this->world_frame_ = "world"; + + this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); }; void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) @@ -61,16 +68,27 @@ void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) { - //Implement Pure Pursuit Controller for Throttle - double distance_to_goal = computeDistance(odom_robot.pose.pose.position, pose_goal.position); - ROS_INFO("distance to goal: %f", distance_to_goal); - double throttle = computeThrottle(distance_to_goal); + // Velocity + tf2::Vector3 robot_vel; + tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); + const double velocity = robot_vel.length(); + + // Update PID controller parameters if they are updated dynamically + if (PARAMS_UPDATED) + { + this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); + PARAMS_UPDATED = false; + } + + // Compute linear speed using PID controller + double target_speed = SPEED_TARGET; + double linear_speed = this->pid_.calculate(target_speed, velocity); //Implement Pure Pursuit Controller for Steering double steering = computeSteering(odom_robot, pose_goal); geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = throttle; + cmd_vel.linear.x = linear_speed; cmd_vel.angular.z = steering; // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; @@ -79,25 +97,12 @@ geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odom return cmd_vel; } -double PathTrackerNode::computeDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) -{ - double dx = p2.x - p1.x; - double dy = p2.y - p1.y; - return std::sqrt(dx * dx + dy * dy); -} - -double PathTrackerNode::computeThrottle(double distance_to_goal) -{ - //adjust throttle based on distance to goal - return std::min(MAX_THROTTLE, distance_to_goal * THROTTLE_GAIN); -} - double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) { // Compute heading error double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); double yaw_goal = tf2::getYaw(pose_goal.orientation); - double heading_error = yaw_goal - yaw_robot; + double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] // Compute lateral error tf2::Vector3 point_robot, point_goal; @@ -105,43 +110,21 @@ double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, co tf2::fromMsg(pose_goal.position, point_goal); double dx = point_goal.getX() - point_robot.getX(); double dy = point_goal.getY() - point_robot.getY(); - double cross_track_error = std::sqrt(dx * dx + dy * dy); - - // Compute alpha (angle between the goal point and the path point) double alpha = std::atan2(dy, dx) - yaw_robot; - // Ensure alpha is within the range [-pi/2, pi/2] - if (alpha > M_PI / 2) { - alpha -= M_PI; - } else if (alpha < -M_PI / 2) { - alpha += M_PI; - } - // Compute lookahead distance double lookahead_distance = computeLookaheadDistance(odom_robot); - // Compute steering angle using the pure pursuit formula - double steering = std::atan((2.0 * ROBOT_LENGTH * cross_track_error) / lookahead_distance) + alpha; + // Compute desired steering angle using the pure pursuit formula + double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); - // Limit the steering angle to a maximum value - const double MAX_STEERING_ANGLE = 0.5; - if (steering > MAX_STEERING_ANGLE) { - steering = MAX_STEERING_ANGLE; - } else if (steering < -MAX_STEERING_ANGLE) { - steering = -MAX_STEERING_ANGLE; - } + // Incorporate heading error + steering += heading_error; + + // Ensure steering angle is within [-pi, pi] + steering = unifyAngleRange(steering); - // Debug output - ROS_INFO("Yaw Robot: %f", yaw_robot); - ROS_INFO("Yaw Goal: %f", yaw_goal); - ROS_INFO("Heading Error: %f", heading_error); - ROS_INFO("Cross-track Error: %f", cross_track_error); - ROS_INFO("Alpha: %f", alpha); - ROS_INFO("Lookahead Distance: %f", lookahead_distance); - ROS_INFO("Computed Steering Angle: %f", steering); - ROS_INFO("Throttle: %f", computeThrottle(computeDistance(odom_robot.pose.pose.position, pose_goal.position))); - - return steering; +return steering; } double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) diff --git a/src/me5413_world/src/pure_pursuit_closest_point_approach.cpp b/src/me5413_world/src/pure_pursuit_closest_point_approach.cpp deleted file mode 100644 index f97dd5f..0000000 --- a/src/me5413_world/src/pure_pursuit_closest_point_approach.cpp +++ /dev/null @@ -1,201 +0,0 @@ -/** path_tracker_node.cpp - * - * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * - * MIT License - * - * ROS Node for robot to track a given path - */ - -#include "me5413_world/path_tracker_node.hpp" -#include "me5413_world/math_utils.hpp" - -namespace me5413_world -{ - -// Dynamic Parameters -double SPEED_TARGET; -double PID_Kp, PID_Ki, PID_Kd; -double ROBOT_LENGTH; -bool DEFAULT_LOOKAHEAD_DISTANCE; -bool PARAMS_UPDATED; - -void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) -{ - SPEED_TARGET = config.speed_target; - PID_Kp = config.PID_Kp; - PID_Ki = config.PID_Ki; - PID_Kd = config.PID_Kd; - ROBOT_LENGTH = config.robot_length; - DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; - - PARAMS_UPDATED = true; -}; - -PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_), path_points_() -{ - f = boost::bind(&dynamicParamCallback, _1, _2); - server.setCallback(f); - - this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); - this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); - this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); - - // Initialization - this->robot_frame_ = "base_link"; - this->world_frame_ = "world"; - - this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); -}; - -void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) -{ - // Extract waypoints from the received path message - std::vector waypoints = path->poses; - - //std::cout << "number of waypoints: " << waypoints.size() << std::endl; - - // Ensure that the received path message contains at least one waypoint - if (waypoints.empty()) { - ROS_WARN("Received empty path message."); - return; - } - - // Use the last waypoint as the goal pose - this->pose_world_goal_ = path->poses[15].pose; - - //std::cout << "goal pose :" << pose_world_goal_ << std::endl; - - // Store the waypoints for later use in computing the closest point on the path - this->path_points_.clear(); - for (const auto& waypoint : waypoints) { - tf2::Vector3 point; - tf2::fromMsg(waypoint.pose.position, point); - this->path_points_.push_back(point); - //std::cout << "path points :" << point* << std::endl; - } - - // Publish control outputs - this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); -}; - -void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) -{ - this->world_frame_ = odom->header.frame_id; - this->robot_frame_ = odom->child_frame_id; - this->odom_world_robot_ = *odom.get(); - - return; -}; - -geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) -{ - // Velocity - tf2::Vector3 robot_vel; - tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); - const double velocity = robot_vel.length(); - - // Update PID controller parameters if they are updated dynamically - if (PARAMS_UPDATED) - { - this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); - PARAMS_UPDATED = false; - } - - // Compute linear speed using PID controller - double target_speed = SPEED_TARGET; - double linear_speed = this->pid_.calculate(target_speed, velocity); - - //Implement Pure Pursuit Controller for Steering - double steering = computeSteering(odom_robot, pose_goal); - - geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = linear_speed; - cmd_vel.angular.z = steering; - - // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; - // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; - - return cmd_vel; -} - -double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) -{ - // Compute heading error - double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); - double yaw_goal = tf2::getYaw(pose_goal.orientation); - double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] - - // Compute lateral error - tf2::Vector3 point_robot, point_goal; - tf2::fromMsg(odom_robot.pose.pose.position, point_robot); - tf2::fromMsg(pose_goal.position, point_goal); - - // Compute lookahead distance - double lookahead_distance = computeLookaheadDistance(odom_robot); - - // Find the closest point on the path to the computed target point - tf2::Vector3 target_point = findClosestPointOnPath(point_robot, path_points_, lookahead_distance); - - // Compute alpha (angle between the goal point and the path point) - double dx = target_point.getX() - point_robot.getX(); - double dy = target_point.getY() - point_robot.getY(); - double alpha = unifyAngleRange(std::atan2(dy, dx) - yaw_robot); - - // Compute desired steering angle using the pure pursuit formula - double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); - - // Incorporate heading error - steering += heading_error; - - // Ensure steering angle is within [-pi, pi] - steering = unifyAngleRange(steering); - -return steering; -} - -double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) -{ - double velocity = odom_robot.twist.twist.linear.x; - return std::max(1.0, velocity * DEFAULT_LOOKAHEAD_DISTANCE); -} - -tf2::Vector3 PathTrackerNode::findClosestPointOnPath(const tf2::Vector3& point_robot, const std::vector& path_points, double lookahead_distance) -{ - // Check if the path is empty - if (path_points.empty()) { - ROS_WARN("Path is empty."); - return tf2::Vector3(); // Return a default vector - } - - // Initialize variables to store the closest point and its distance - tf2::Vector3 closest_point = path_points.front(); // Start with the first point in the path - double min_distance = (point_robot - closest_point).length(); // Calculate initial distance - - // Iterate over path points - for (const auto& path_point : path_points) { - // Calculate distance between the robot's position and the path point - double distance = (point_robot - path_point).length(); - - // Check if the distance is within the lookahead distance - if (distance <= lookahead_distance) { - // Update closest point if this point is closer than the current closest point - if (distance < min_distance) { - min_distance = distance; - closest_point = path_point; - } - } - } - //std::cout << "closest_point: " << closest_point << std::endl; - return closest_point; -} - -} // namespace me5413_world - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "path_tracker_node"); - me5413_world::PathTrackerNode path_tracker_node; - ros::spin(); // spin the ros node. - return 0; -} diff --git a/src/me5413_world/src/pure_pursuit_linear_approach.cpp b/src/me5413_world/src/pure_pursuit_linear_approach.cpp deleted file mode 100644 index dcfdea7..0000000 --- a/src/me5413_world/src/pure_pursuit_linear_approach.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/** path_tracker_node.cpp - * - * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * - * MIT License - * - * ROS Node for robot to track a given path - */ - -#include "me5413_world/path_tracker_node.hpp" -#include "me5413_world/math_utils.hpp" - -namespace me5413_world -{ - -// Dynamic Parameters -double SPEED_TARGET; -double PID_Kp, PID_Ki, PID_Kd; -double ROBOT_LENGTH; -bool DEFAULT_LOOKAHEAD_DISTANCE; -bool PARAMS_UPDATED; - -void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) -{ - SPEED_TARGET = config.speed_target; - PID_Kp = config.PID_Kp; - PID_Ki = config.PID_Ki; - PID_Kd = config.PID_Kd; - ROBOT_LENGTH = config.robot_length; - DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; - - PARAMS_UPDATED = true; -}; - -PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_), path_points_() -{ - f = boost::bind(&dynamicParamCallback, _1, _2); - server.setCallback(f); - - this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); - this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); - this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); - - // Initialization - this->robot_frame_ = "base_link"; - this->world_frame_ = "world"; - - this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); -}; - -void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) -{ - // Extract waypoints from the received path message - std::vector waypoints = path->poses; - - //std::cout << "number of waypoints: " << waypoints.size() << std::endl; - - // Ensure that the received path message contains at least one waypoint - if (waypoints.empty()) { - ROS_WARN("Received empty path message."); - return; - } - - // Use the last waypoint as the goal pose - this->pose_world_goal_ = path->poses[30].pose; - - //std::cout << "goal pose :" << pose_world_goal_ << std::endl; - - // Store the waypoints for later use in computing the closest point on the path - this->path_points_.clear(); - for (const auto& waypoint : waypoints) { - tf2::Vector3 point; - tf2::fromMsg(waypoint.pose.position, point); - this->path_points_.push_back(point); - //std::cout << "path points :" << point* << std::endl; - } - - // Publish control outputs - this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); -}; - -void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) -{ - this->world_frame_ = odom->header.frame_id; - this->robot_frame_ = odom->child_frame_id; - this->odom_world_robot_ = *odom.get(); - - return; -}; - -geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) -{ - // Velocity - tf2::Vector3 robot_vel; - tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); - const double velocity = robot_vel.length(); - - // Update PID controller parameters if they are updated dynamically - if (PARAMS_UPDATED) - { - this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); - PARAMS_UPDATED = false; - } - - // Compute linear speed using PID controller - double target_speed = SPEED_TARGET; - double linear_speed = this->pid_.calculate(target_speed, velocity); - - //Implement Pure Pursuit Controller for Steering - double steering = computeSteering(odom_robot, pose_goal); - - geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = linear_speed; - cmd_vel.angular.z = steering; - - // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; - // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; - - return cmd_vel; -} - -double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) -{ - // Compute heading error - double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); - double yaw_goal = tf2::getYaw(pose_goal.orientation); - double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] - - // Compute lateral error - tf2::Vector3 point_robot, point_goal; - tf2::fromMsg(odom_robot.pose.pose.position, point_robot); - tf2::fromMsg(pose_goal.position, point_goal); - - // Compute lookahead distance - double lookahead_distance = computeLookaheadDistance(odom_robot); - - // Find the closest point on the path to the computed target point - tf2::Vector3 target_point = findClosestPointOnPath(point_robot, path_points_, lookahead_distance); - - // Compute alpha (angle between the goal point and the path point) - double dx = target_point.getX() - point_robot.getX(); - double dy = target_point.getY() - point_robot.getY(); - double alpha = unifyAngleRange(std::atan2(dy, dx) - yaw_robot); - - // Compute desired steering angle using the pure pursuit formula - double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); - - // Incorporate heading error - steering += heading_error; - - // Ensure steering angle is within [-pi, pi] - steering = unifyAngleRange(steering); - -return steering; -} - -double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) -{ - double velocity = odom_robot.twist.twist.linear.x; - return std::max(0.1, velocity * DEFAULT_LOOKAHEAD_DISTANCE); -} - -tf2::Vector3 PathTrackerNode::findClosestPointOnPath(const tf2::Vector3& point_robot, const std::vector& path_points, double lookahead_distance) -{ - // Check if the path is empty - if (path_points.empty()) { - ROS_WARN("Path is empty."); - return tf2::Vector3(); - } - - // Target point initialization - tf2::Vector3 target_point; - - // Iterate over path segments - for (int i = 0; i < path_points.size() - 1; ++i) { - // Calculate distance along the current segment from the robot - tf2::Vector3 segment_direction = path_points[i + 1] - path_points[i]; - double segment_length = segment_direction.length(); - segment_direction /= segment_length; // Normalize direction vector - - tf2::Vector3 relative_robot_position = point_robot - path_points[i]; - double projection = relative_robot_position.dot(segment_direction); - - // Check if projection is within segment bounds (0 to segment_length) - if (projection >= 0 && projection <= segment_length) { - // Calculate distance traveled along the segment - double distance_traveled = projection; - - // Check if distance traveled is within lookahead distance - if (distance_traveled <= lookahead_distance) { - // Calculate target point on the segment - target_point = path_points[i] + distance_traveled * segment_direction; - std::cout << "get target point: (" << target_point.getX() << ", " << target_point.getY() << ")" << std::endl; - break; // Exit loop as we found a point within lookahead distance - } - } - } - - // Handle no intersection case - if (target_point.getX() == 0 && target_point.getY() == 0 && target_point.getZ() == 0) { - ROS_WARN("No point found within lookahead distance on the path."); - } - - return target_point; -} - -} // namespace me5413_world - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "path_tracker_node"); - me5413_world::PathTrackerNode path_tracker_node; - ros::spin(); // spin the ros node. - return 0; -} diff --git a/src/me5413_world/src/pure_pursuit_target_point_approach.cpp b/src/me5413_world/src/pure_pursuit_target_point_approach.cpp deleted file mode 100644 index 6bce38b..0000000 --- a/src/me5413_world/src/pure_pursuit_target_point_approach.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/** path_tracker_node.cpp - * - * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore - * - * MIT License - * - * ROS Node for robot to track a given path - */ - -#include "me5413_world/path_tracker_node.hpp" -#include "me5413_world/math_utils.hpp" - -namespace me5413_world -{ - -// Dynamic Parameters -double SPEED_TARGET; -double PID_Kp, PID_Ki, PID_Kd; -double ROBOT_LENGTH; -bool DEFAULT_LOOKAHEAD_DISTANCE; -bool PARAMS_UPDATED; - -void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) -{ - SPEED_TARGET = config.speed_target; - PID_Kp = config.PID_Kp; - PID_Ki = config.PID_Ki; - PID_Kd = config.PID_Kd; - ROBOT_LENGTH = config.robot_length; - DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; - - PARAMS_UPDATED = true; -}; - -PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) -{ - f = boost::bind(&dynamicParamCallback, _1, _2); - server.setCallback(f); - - this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); - this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); - this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); - - // Initialization - this->robot_frame_ = "base_link"; - this->world_frame_ = "world"; - - this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); -}; - -void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) -{ - // Calculate absolute errors (wrt to world frame) - this->pose_world_goal_ = path->poses[11].pose; - this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); - - return; -}; - -void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) -{ - this->world_frame_ = odom->header.frame_id; - this->robot_frame_ = odom->child_frame_id; - this->odom_world_robot_ = *odom.get(); - - return; -}; - -geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) -{ - // Velocity - tf2::Vector3 robot_vel; - tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); - const double velocity = robot_vel.length(); - - // Update PID controller parameters if they are updated dynamically - if (PARAMS_UPDATED) - { - this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); - PARAMS_UPDATED = false; - } - - // Compute linear speed using PID controller - double target_speed = SPEED_TARGET; - double linear_speed = this->pid_.calculate(target_speed, velocity); - - //Implement Pure Pursuit Controller for Steering - double steering = computeSteering(odom_robot, pose_goal); - - geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = linear_speed; - cmd_vel.angular.z = steering; - - // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; - // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; - - return cmd_vel; -} - -double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) -{ - // Compute heading error - double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); - double yaw_goal = tf2::getYaw(pose_goal.orientation); - double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] - - // Compute lateral error - tf2::Vector3 point_robot, point_goal; - tf2::fromMsg(odom_robot.pose.pose.position, point_robot); - tf2::fromMsg(pose_goal.position, point_goal); - double dx = point_goal.getX() - point_robot.getX(); - double dy = point_goal.getY() - point_robot.getY(); - double alpha = std::atan2(dy, dx) - yaw_robot; - - // Compute lookahead distance - double lookahead_distance = computeLookaheadDistance(odom_robot); - - // Compute desired steering angle using the pure pursuit formula - double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); - - // Incorporate heading error - steering += heading_error; - - // Ensure steering angle is within [-pi, pi] - steering = unifyAngleRange(steering); - -return steering; -} - -double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) -{ - double velocity = odom_robot.twist.twist.linear.x; - return std::max(1.0, velocity * DEFAULT_LOOKAHEAD_DISTANCE); -} - -} // namespace me5413_world - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "path_tracker_node"); - me5413_world::PathTrackerNode path_tracker_node; - ros::spin(); // spin the ros node. - return 0; -}