NUS-Advanced-Robotics-Centre · yash27agarwal · Mar 28, 2024 · Mar 28, 2024
diff --git a/.catkin_tools/README 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
@@ -0,0 +1 @@
+0.9.2
diff --git a/.catkin_tools/profiles/default/build.yaml 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
@@ -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
@@ -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
@@ -0,0 +1,10 @@
+<package>
+  <name>catkin_tools_prebuild</name>
+  <description>
+    This package is used to generate catkin setup files.
+  </description>
+  <version>0.0.0</version>
+  <license>BSD</license>
+  <maintainer email="[email protected]">jbohren</maintainer>
+  <buildtool_depend>catkin</buildtool_depend>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/jackal_description/devel_manifest.txt 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
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package>
+  <name>jackal_description</name>
+  <version>0.8.6</version>
+  <description>URDF robot description for Jackal</description>
+
+  <maintainer email="[email protected]">Mike Purvis</maintainer>
+
+  <license>BSD</license>
+
+  <author email="[email protected]">Mike Purvis</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <run_depend>flir_camera_description</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>urdf</run_depend>
+  <run_depend>xacro</run_depend>
+  <run_depend>lms1xx</run_depend>
+  <run_depend>pointgrey_camera_description</run_depend>
+  <run_depend>sick_tim</run_depend>
+  <run_depend>velodyne_description</run_depend>
+
+  <export>
+  </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/me5413_world/devel_manifest.txt 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
@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>me5413_world</name>
+  <version>0.0.0</version>
+  <description>The me5413_world package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">shuo</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/me5413_world</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>rospy</depend>
+  <depend>roscpp</depend>
+  <depend>rviz</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>gazebo_ros</depend>
+  <depend>jsk_rviz_plugins</depend>
+  <depend>jackal_gazebo</depend>
+  <depend>jackal_navigation</depend>
+  <depend>velodyne_simulator</depend>
+  <depend>dynamic_reconfigure</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    <gazebo plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
+  </export>
+</package>
diff --git 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"))
diff --git 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
@@ -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_
diff --git 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 <geometry_msgs/TransformStamped.h>
 
 #include <tf2/convert.h>
+#include "angles/angles.h"
+#include <tf2/utils.h>
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/LinearMath/Quaternion.h>
@@ -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<tf2::Vector3>& 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<tf2::Vector3> path_points_;
 };
 
 } // namespace me5413_world
 
-#endif // PATH_TRACKER_NODE_H_
+#endif // PATH_TRACKER_NODE_H_
diff --git 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
diff --git a/src/me5413_world/launch/path_tracking.launch b/src/me5413_world/launch/path_tracking.launch
@@ -1,7 +1,7 @@
 <launch>
   <!-- Launch the ME5413 Path Publisher Node -->
   <node ns="me5413_world" pkg="me5413_world" type="path_publisher_node" name="path_publisher_node" output="screen" />
-  <!-- Launch the ME5413 Path Publisher Node -->
+  <!-- Launch the ME5413 Path Tracker Node -->
   <node ns="me5413_world" pkg="me5413_world" type="path_tracker_node" name="path_tracker_node" output="screen" />
 
   <!-- Launch Rviz with our settings -->
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		/home/yash/ME5413/HW3/ME5413_Planning_Project/devel/./cmake.lock 8