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..f8f3125 --- /dev/null +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -0,0 +1 @@ +/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock 8 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/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 3845318..a1e7dea 100644 --- a/src/me5413_world/cfg/path_tracker.cfg +++ b/src/me5413_world/cfg/path_tracker.cfg @@ -6,11 +6,10 @@ 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("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_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 50c2a6e..93b6905 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 @@ -37,7 +39,7 @@ #include "me5413_world/pid.hpp" -namespace me5413_world +namespace me5413_world { class PathTrackerNode @@ -48,16 +50,16 @@ 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 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_; - ros::Timer timer_; ros::Subscriber sub_robot_odom_; ros::Subscriber sub_local_path_; ros::Publisher pub_cmd_vel_; @@ -76,8 +78,10 @@ class PathTrackerNode // Controllers control::PID pid_; + + // std::vector path_points_; }; } // 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/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/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..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) { @@ -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 19ab019..02bfebd 100644 --- a/src/me5413_world/src/path_tracker_node.cpp +++ b/src/me5413_world/src/path_tracker_node.cpp @@ -1,37 +1,36 @@ /** 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" +#include "me5413_world/math_utils.hpp" -namespace me5413_world +namespace me5413_world { // Dynamic Parameters double SPEED_TARGET; double PID_Kp, PID_Ki, PID_Kd; -double STANLEY_K; +double ROBOT_LENGTH; +bool DEFAULT_LOOKAHEAD_DISTANCE; bool PARAMS_UPDATED; -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; - + ROBOT_LENGTH = config.robot_length; + DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; + PARAMS_UPDATED = true; -} +}; PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) { @@ -47,7 +46,7 @@ PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) 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 +55,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,52 +64,32 @@ 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) -{ - const double stanley_output = -1.0*(heading_error + std::atan2(STANLEY_K*cross_track_error, std::max(velocity, 0.3))); - - return std::min(std::max(stanley_output, -2.2), 2.2); -} +}; geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) { - // 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 roll, pitch, yaw_robot, yaw_goal; - m_robot.getRPY(roll, pitch, yaw_robot); - m_goal.getRPY(roll, pitch, yaw_goal); - - const double heading_error = unifyAngleRange(yaw_robot - yaw_goal); - - // 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); - // Velocity tf2::Vector3 robot_vel; tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); const double velocity = robot_vel.length(); - geometry_msgs::Twist cmd_vel; + // Update PID controller parameters if they are updated dynamically if (PARAMS_UPDATED) { this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); PARAMS_UPDATED = false; } - cmd_vel.linear.x = this->pid_.calculate(SPEED_TARGET, velocity); - cmd_vel.angular.z = computeStanelyControl(heading_error, lat_error, velocity); + + // 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; @@ -118,6 +97,42 @@ geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odom 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) @@ -126,4 +141,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