feat: split configs

src/me5413_world/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 generate_dynamic_reconfigure_options(
+  cfg/path_publisher.cfg
   cfg/path_tracker.cfg
 )
 
@@ -46,7 +47,7 @@ include_directories(
 # Add Nodes
 add_executable(path_publisher_node src/path_publisher_node.cpp)
 target_link_libraries(path_publisher_node ${catkin_LIBRARIES})
-add_dependencies(path_publisher_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(path_publisher_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 
 add_executable(path_tracker_node src/path_tracker_node.cpp)
 target_link_libraries(path_tracker_node ${catkin_LIBRARIES})

src/me5413_world/cfg/path_publisher.cfg

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "me5413_world"
+
+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("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)
+
+exit(gen.generate(PACKAGE, "path_publisher_node", "path_publisher"))

src/me5413_world/cfg/path_tracker.cfg

@@ -5,10 +5,7 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("speed_target", double_t, 1, "Default: 1.0 [m/s]", 0.5, 0.1, 1.0)
-gen.add("track_A_axis", double_t, 1, "Default: 10.0", 10.0, 1.0, 20.0)
-gen.add("track_B_axis", double_t, 1, "Default: 10.0", 10.0, 1.0, 20.0)
-gen.add("track_wp_num", int_t, 1, "Default: 500", 500, 100, 2000)
+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)

src/me5413_world/include/me5413_world/path_publisher_node.hpp

@@ -33,7 +33,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <dynamic_reconfigure/server.h>
-#include <me5413_world/path_trackerConfig.h>
+#include <me5413_world/path_publisherConfig.h>
 
 namespace me5413_world
 {
@@ -63,8 +63,8 @@ class PathPublisherNode
   tf2_ros::Buffer tf2_buffer_;
   tf2_ros::TransformListener tf2_listener_;
   tf2_ros::TransformBroadcaster tf2_bcaster_;
-  dynamic_reconfigure::Server<me5413_world::path_trackerConfig> server;
-  dynamic_reconfigure::Server<me5413_world::path_trackerConfig>::CallbackType f;
+  dynamic_reconfigure::Server<me5413_world::path_publisherConfig> server;
+  dynamic_reconfigure::Server<me5413_world::path_publisherConfig>::CallbackType f;
 
   ros::Publisher pub_global_path_;
   ros::Publisher pub_local_path_;

src/me5413_world/include/me5413_world/pid.hpp

@@ -21,6 +21,7 @@ class PID
   PID(double dt, double max, double min, double Kp, double Kd, double Ki);
   ~PID() {};
 
+  void updateSettings(const double Kp, const double Kd, const double Ki);
   // Returns the manipulated variable given a setpoint and current process value
   double calculate(const double setpoint, const double pv);
 
@@ -46,6 +47,13 @@ PID::PID(double dt, double max, double min, double Kp, double Kd, double Ki) :
   integral_(0) 
 {};
 
+void PID::updateSettings(const double Kp, const double Kd, const double Ki)
+{
+  this->Kp_ = Kp;
+  this->Kd_ = Kd;
+  this->Ki_ = Ki;
+};
+
 double PID::calculate(const double setpoint, const double pv)
 {
   // Calculate error

src/me5413_world/src/path_publisher_node.cpp

@@ -19,7 +19,7 @@ double TRACK_B_AXIS;
 double TRACK_WP_NUM;
 bool PARAMS_UPDATED;
 
-void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level)
+void dynamicParamCallback(me5413_world::path_publisherConfig& config, uint32_t level)
 {
   // Common Params
   SPEED_TARGET = config.speed_target;
@@ -35,7 +35,7 @@ PathPublisherNode::PathPublisherNode() : tf2_listener_(tf2_buffer_)
   f = boost::bind(&dynamicParamCallback, _1, _2);
   server.setCallback(f);
 
-  this->timer_ = nh_.createTimer(ros::Duration(0.2), &PathPublisherNode::timerCallback, this);
+  this->timer_ = nh_.createTimer(ros::Duration(0.1), &PathPublisherNode::timerCallback, this);
   this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathPublisherNode::robotOdomCallback, this);
   this->pub_global_path_ = nh_.advertise<nav_msgs::Path>("/me5413_world/planning/global_path", 1);
   this->pub_local_path_ = nh_.advertise<nav_msgs::Path>("/me5413_world/planning/local_path", 1);

src/me5413_world/src/path_tracker_node.cpp

@@ -16,6 +16,7 @@ namespace me5413_world
 double SPEED_TARGET;
 double PID_Kp, PID_Ki, PID_Kd;
 double STANLEY_K;
+bool PARAMS_UPDATED;
 
 void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level)
 {
@@ -27,6 +28,8 @@ void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t lev
   PID_Kd = config.PID_Kd;
   // Stanley
   STANLEY_K = config.stanley_K;
+
+  PARAMS_UPDATED = true;
 };
 
 PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_)
@@ -100,11 +103,16 @@ geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odom
   const double velocity = robot_vel.length();
 
   geometry_msgs::Twist cmd_vel;
-  cmd_vel.linear.x = pid_.calculate(SPEED_TARGET, velocity);
+  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);
 
-  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;
+  // 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;
 }