From 894b45123fc68149a5c0c87857b2a699a267dcbe Mon Sep 17 00:00:00 2001 From: SS47816 Date: Thu, 14 Mar 2024 13:07:39 +0800 Subject: [PATCH] feat: split configs --- src/me5413_world/CMakeLists.txt | 3 ++- src/me5413_world/cfg/path_publisher.cfg | 16 ++++++++++++++++ src/me5413_world/cfg/path_tracker.cfg | 5 +---- .../include/me5413_world/path_publisher_node.hpp | 6 +++--- src/me5413_world/include/me5413_world/pid.hpp | 8 ++++++++ src/me5413_world/src/path_publisher_node.cpp | 4 ++-- src/me5413_world/src/path_tracker_node.cpp | 14 +++++++++++--- 7 files changed, 43 insertions(+), 13 deletions(-) create mode 100644 src/me5413_world/cfg/path_publisher.cfg diff --git a/src/me5413_world/CMakeLists.txt b/src/me5413_world/CMakeLists.txt index a174a85..2042f87 100644 --- a/src/me5413_world/CMakeLists.txt +++ b/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}) diff --git a/src/me5413_world/cfg/path_publisher.cfg b/src/me5413_world/cfg/path_publisher.cfg new file mode 100644 index 0000000..0e374b3 --- /dev/null +++ b/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")) \ 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 51b5a28..5a7174f 100644 --- a/src/me5413_world/cfg/path_tracker.cfg +++ b/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) 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 58f5eb7..7d65a34 100644 --- a/src/me5413_world/include/me5413_world/path_publisher_node.hpp +++ b/src/me5413_world/include/me5413_world/path_publisher_node.hpp @@ -33,7 +33,7 @@ #include #include -#include +#include 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 server; - dynamic_reconfigure::Server::CallbackType f; + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; ros::Publisher pub_global_path_; ros::Publisher pub_local_path_; diff --git a/src/me5413_world/include/me5413_world/pid.hpp b/src/me5413_world/include/me5413_world/pid.hpp index 4426d92..db8f44f 100644 --- a/src/me5413_world/include/me5413_world/pid.hpp +++ b/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 diff --git a/src/me5413_world/src/path_publisher_node.cpp b/src/me5413_world/src/path_publisher_node.cpp index a4f4a21..4cb7b4f 100644 --- a/src/me5413_world/src/path_publisher_node.cpp +++ b/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("/me5413_world/planning/global_path", 1); this->pub_local_path_ = nh_.advertise("/me5413_world/planning/local_path", 1); diff --git a/src/me5413_world/src/path_tracker_node.cpp b/src/me5413_world/src/path_tracker_node.cpp index 1fa7a36..270d7a3 100644 --- a/src/me5413_world/src/path_tracker_node.cpp +++ b/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; }