feat: adjustable wp num

NUS-Advanced-Robotics-Centre · Mar 14, 2024 · 1b6b3a3 · 1b6b3a3
1 parent e7acb94
commit 1b6b3a3
Show file tree

Hide file tree

Showing 6 changed files with 12 additions and 14 deletions.
diff --git a/README.md b/README.md
@@ -31,15 +31,12 @@ NUS ME5413 Autonomous Mobile Robotics Final Project
   * `tf2_ros`
   * `tf2_eigen`
   * `tf2_geometry_msgs`
-  * `pluginlib`
-  * `map_server`
   * `gazebo_ros`
   * `jsk_rviz_plugins`
   * `jackal_gazebo`
   * `jackal_navigation`
   * `velodyne_simulator`
-  * `teleop_twist_keyboard`
-  * dynamic_reconfigure
+  * `dynamic_reconfigure`
 
 ## Installation
 

diff --git a/src/me5413_world/CMakeLists.txt b/src/me5413_world/CMakeLists.txt
@@ -10,19 +10,18 @@ add_compile_options(-std=c++11)
 find_package(catkin REQUIRED COMPONENTS
   rospy
   roscpp
+  rviz
   std_msgs
   nav_msgs
   geometry_msgs
   tf2
   tf2_ros
   tf2_eigen
   tf2_geometry_msgs
-  rviz
   gazebo_ros
   jsk_rviz_plugins
   jackal_gazebo
   jackal_navigation
-  teleop_twist_keyboard
   dynamic_reconfigure
 )
 

diff --git a/src/me5413_world/cfg/path_publisher.cfg b/src/me5413_world/cfg/path_publisher.cfg
@@ -11,6 +11,6 @@ gen.add("track_A_axis", double_t, 1, "Default: 8.0", 8.0, 1.0, 15.0)
 gen.add("track_B_axis", double_t, 1, "Default: 8.0", 8.0, 1.0, 15.0)
 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: 90", 90, 5, 200)
+gen.add("local_next_wp_num", int_t, 1, "Default: 50", 50, 5, 200)
 
 exit(gen.generate(PACKAGE, "path_publisher_node", "path_publisher"))
diff --git a/src/me5413_world/include/me5413_world/path_publisher_node.hpp b/src/me5413_world/include/me5413_world/path_publisher_node.hpp
@@ -66,6 +66,8 @@ class PathPublisherNode
   dynamic_reconfigure::Server<me5413_world::path_publisherConfig> server;
   dynamic_reconfigure::Server<me5413_world::path_publisherConfig>::CallbackType f;
 
+  ros::Subscriber sub_robot_odom_;
+
   ros::Publisher pub_global_path_;
   ros::Publisher pub_local_path_;
   ros::Publisher pub_abs_position_error_;
@@ -75,11 +77,8 @@ class PathPublisherNode
   ros::Publisher pub_rms_heading_error_;
   ros::Publisher pub_rms_speed_error_;
 
-  ros::Subscriber sub_robot_odom_;
-
   // Robot pose
   std::string world_frame_;
-  std::string map_frame_;
   std::string robot_frame_;
 
   geometry_msgs::Pose pose_world_goal_;

diff --git a/src/me5413_world/package.xml b/src/me5413_world/package.xml
@@ -52,19 +52,18 @@
 
   <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>rviz</depend>
   <depend>gazebo_ros</depend>
   <depend>jsk_rviz_plugins</depend>
   <depend>jackal_gazebo</depend>
   <depend>jackal_navigation</depend>
-  <depend>teleop_twist_keyboard</depend>
   <depend>velodyne_simulator</depend>
   <depend>dynamic_reconfigure</depend>
 

diff --git a/src/me5413_world/src/path_publisher_node.cpp b/src/me5413_world/src/path_publisher_node.cpp
@@ -17,6 +17,8 @@ double SPEED_TARGET;
 double TRACK_A_AXIS;
 double TRACK_B_AXIS;
 double TRACK_WP_NUM;
+double LOCAL_PREV_WP_NUM;
+double LOCAL_NEXT_WP_NUM;
 bool PARAMS_UPDATED;
 
 void dynamicParamCallback(me5413_world::path_publisherConfig& config, uint32_t level)
@@ -27,6 +29,8 @@ void dynamicParamCallback(me5413_world::path_publisherConfig& config, uint32_t l
   TRACK_A_AXIS = config.track_A_axis;
   TRACK_B_AXIS = config.track_B_axis;
   TRACK_WP_NUM = config.track_wp_num;
+  LOCAL_PREV_WP_NUM = config.local_prev_wp_num;
+  LOCAL_NEXT_WP_NUM = config.local_next_wp_num;
   PARAMS_UPDATED = true;
 };
 
@@ -48,7 +52,6 @@ PathPublisherNode::PathPublisherNode() : tf2_listener_(tf2_buffer_)
 
   // Initialization
   this->robot_frame_ = "base_link";
-  this->map_frame_ = "map";
   this->world_frame_ = "world";
 
   this->global_path_msg_.header.frame_id = this->world_frame_;
@@ -72,10 +75,11 @@ void PathPublisherNode::timerCallback(const ros::TimerEvent &)
   if (PARAMS_UPDATED)
   {
     this->global_path_msg_.poses = createGlobalPath(TRACK_A_AXIS, TRACK_B_AXIS, 1.0/TRACK_WP_NUM);
+    this->current_id_ = 0;
     PARAMS_UPDATED = false;
   }
   publishGlobalPath();
-  publishLocalPath(this->odom_world_robot_.pose.pose, 10, 90);
+  publishLocalPath(this->odom_world_robot_.pose.pose, LOCAL_PREV_WP_NUM, LOCAL_NEXT_WP_NUM);
 
   // Calculate absolute errors (wrt to world frame)
   const std::pair<double, double> abs_errors = calculatePoseError(this->odom_world_robot_.pose.pose, this->pose_world_goal_);