From 94bf51705cf597035fb9bd41df2b8e61907f4281 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 25 Jan 2021 16:46:46 -0500 Subject: [PATCH 01/24] adds dynamic reconfigure package --- rr_common/CMakeLists.txt | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rr_common/CMakeLists.txt b/rr_common/CMakeLists.txt index ac7d8cdf0..cd0dec417 100644 --- a/rr_common/CMakeLists.txt +++ b/rr_common/CMakeLists.txt @@ -16,10 +16,19 @@ find_package(catkin REQUIRED COMPONENTS tf tf2_geometry_msgs parameter_assertions + dynamic_reconfigure ) find_package(OpenCV REQUIRED) +########################## +## dynamic reconfigure ## +########################## + +generate_dynamic_reconfigure_options( + cfg/planner_configuration.cfg +) + ################################### ## catkin specific configuration ## ################################### From 776513546aea73f5b4adfa1bb79fff809aeb20a5 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 25 Jan 2021 16:47:42 -0500 Subject: [PATCH 02/24] adds starter cfg file --- rr_common/cfg/planner_configuration.cfg | 26 +++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 rr_common/cfg/planner_configuration.cfg diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg new file mode 100644 index 000000000..c5ce36ece --- /dev/null +++ b/rr_common/cfg/planner_configuration.cfg @@ -0,0 +1,26 @@ +#!/usr/bin/env python +PACKAGE = "rr_common/include/rr_common/planning" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("num_path_segments", int_t, 0, "Number of path segments for the path representation", 5, 1, 10) +gen.add("step_size", double_t, 0, "The discrete step size for forward path simulation", 0.5, 0.01, 1.0) +gen.add("num_steps", int_t, 0, "Numbers of discrete steps per path segments", 50, 1, 100) +gen.add("max_fow_acceleration", double_t, 0, "Max forward acceleration of vehicle model", 10.0, 0.5, 20.0) +gen.add("max_lat_acceleration", double_t, 0, "Max lateral acceleration of vehicle model", 5.0, 0.5, 10.0) +gen.add("steer_rate", double_t, 0, "Steering rate of vehicle model", 15.0, 0.1, 30.0) +gen.add("steer_angle", double_t, 0, "Max steering angle for vehicle model", 0.75, 0.01, 1.5) +gen.add("slip_compensation_gain", double_t, 0, "Steering compensation gain for vehicle model", 1.15, 0.8, 1.5) +gen.add("speed_cost", double_t, 0,"Speed coefficient for cost function", 5.0, 0.0, 10.0) +gen.add("obstacle_clearance_cost", double_t, 0, "Obstacle clearing coefficient for cost function", 5.0, 0.0, 10.0) +gen.add("heading_cost", double_t, 0, "Heading coefficient for cost function", 5.0, 0.0, 10.0) +gen.add("forward_distance_cost", double_t, 0, "Forward distance coefficient for cost function", 50.0, 0.0, 100.0) +gen.add("collision_cost", double_t, 0, "Collision coefficient for cost function", 250.0, 0.0, 500.0) +gen.add("start_temperature", double_t, 0, "Simulated starting temperature", 0.5, 0.0, 1.0) +gen.add("end_temperature", double_t, 0, "Simulated ending temperature", 0.5, 0.0, 1.0) +gen.add("iterations", int_t, 0, "Number of iterations", 2500, 10, 5000) +gen.add("cost_scale_factor", double_t, "Scale factor for the cost", 0.5, 0.0, 1.0) + +exit(gen.generate(PACKAGE, "planner_node", "PathPlanner")) \ No newline at end of file From 260a074374992814598db03b54a6116a679be21d Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 25 Jan 2021 16:58:35 -0500 Subject: [PATCH 03/24] adds callback boilerplate code --- rr_common/src/planner/planner_node.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 7815a4636..1ba61098f 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -14,8 +14,9 @@ #include #include #include - #include +#include +#include constexpr int ctrl_dim = 1; @@ -161,9 +162,21 @@ void processMap() { } } +void callback(beginner_tutorials::TutorialConfig &config, uint32_t level) { + // need a smart way of only displaying the param that was changed + // for now just display a generic message + ROS_INFO("Reconfigure Request Processed"); +} + int main(int argc, char** argv) { ros::init(argc, argv, "planner"); + dynamic_reconfigure::Server DynReconfigServer; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&callback, _1, _2); + DynReconfigServer.setCallback(f); + ros::NodeHandle nh; ros::NodeHandle nhp("~"); From 0efbf6c69afa73aac6bef9ac3bc26dceb3f9030a Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 31 Jan 2021 17:59:45 -0500 Subject: [PATCH 04/24] first plug-in setup (failed) --- rr_common/cfg/planner_configuration.cfg | 6 ++-- rr_common/dyn_reconf_plugins.xml | 7 ++++ .../include/rr_common/planner_dyn_reconf.h | 12 +++++++ rr_common/package.xml | 1 + rr_common/src/planner/planner_node.cpp | 32 ++++++++++++------- 5 files changed, 44 insertions(+), 14 deletions(-) create mode 100644 rr_common/dyn_reconf_plugins.xml create mode 100644 rr_common/include/rr_common/planner_dyn_reconf.h diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index c5ce36ece..1203c3a30 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python -PACKAGE = "rr_common/include/rr_common/planning" +PACKAGE = "rr_common" from dynamic_reconfigure.parameter_generator_catkin import * @@ -21,6 +21,6 @@ gen.add("collision_cost", double_t, 0, "Collision coefficient for cost function" gen.add("start_temperature", double_t, 0, "Simulated starting temperature", 0.5, 0.0, 1.0) gen.add("end_temperature", double_t, 0, "Simulated ending temperature", 0.5, 0.0, 1.0) gen.add("iterations", int_t, 0, "Number of iterations", 2500, 10, 5000) -gen.add("cost_scale_factor", double_t, "Scale factor for the cost", 0.5, 0.0, 1.0) +gen.add("cost_scale_factor", double_t, 0, "Scale factor for the cost", 0.5, 0.0, 1.0) -exit(gen.generate(PACKAGE, "planner_node", "PathPlanner")) \ No newline at end of file +exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "PathPlanner")) \ No newline at end of file diff --git a/rr_common/dyn_reconf_plugins.xml b/rr_common/dyn_reconf_plugins.xml new file mode 100644 index 000000000..064ca9091 --- /dev/null +++ b/rr_common/dyn_reconf_plugins.xml @@ -0,0 +1,7 @@ + + + + Enable Dynamic Reconfigure For Planner Node + + + \ No newline at end of file diff --git a/rr_common/include/rr_common/planner_dyn_reconf.h b/rr_common/include/rr_common/planner_dyn_reconf.h new file mode 100644 index 000000000..d89d1ce32 --- /dev/null +++ b/rr_common/include/rr_common/planner_dyn_reconf.h @@ -0,0 +1,12 @@ +#include +#include +#include + +namespace planner_dyn_reconf_ns { + + void dynamic_callback(rr_common::PathPlannerConfig&, uint32_t); + class planner_node : public nodelet::Nodelet { + private: + virtual void onInit(); + }; +} \ No newline at end of file diff --git a/rr_common/package.xml b/rr_common/package.xml index 96e9a0f2f..4a60068ee 100644 --- a/rr_common/package.xml +++ b/rr_common/package.xml @@ -32,5 +32,6 @@ + diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 1ba61098f..c0f939d06 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -15,8 +15,8 @@ #include #include #include -#include -#include + +#include constexpr int ctrl_dim = 1; @@ -162,20 +162,27 @@ void processMap() { } } -void callback(beginner_tutorials::TutorialConfig &config, uint32_t level) { - // need a smart way of only displaying the param that was changed - // for now just display a generic message - ROS_INFO("Reconfigure Request Processed"); +namespace planner_dyn_reconf_ns { + void dynamic_callback(rr_common::PathPlannerConfig &config, uint32_t level) { + // need a smart way of only displaying the param that was changed + // for now just display a generic message + ROS_INFO("Reconfigure Request Processed"); + } + + void planner_node::onInit() { + dynamic_reconfigure::Server DynReconfigServer; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&dynamic_callback, _1, _2); + DynReconfigServer.setCallback(f); + } } + int main(int argc, char** argv) { ros::init(argc, argv, "planner"); - dynamic_reconfigure::Server DynReconfigServer; - dynamic_reconfigure::Server::CallbackType f; - - f = boost::bind(&callback, _1, _2); - DynReconfigServer.setCallback(f); + planner_dyn_reconf_ns::planner_node::onInit(); ros::NodeHandle nh; ros::NodeHandle nhp("~"); @@ -275,3 +282,6 @@ int main(int argc, char** argv) { return 0; } + + +PLUGINLIB_EXPORT_CLASS(planner_dyn_reconf_ns::planner_node, nodelet::Nodelet) From 9fc957ce59057cc4f10b8aafa33ad625488f6e9e Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 31 Jan 2021 19:49:42 -0500 Subject: [PATCH 05/24] resolved build issues --- rr_common/dyn_reconf_plugins.xml | 7 ---- .../include/rr_common/planner_dyn_reconf.h | 12 ------- rr_common/package.xml | 1 - rr_common/src/planner/planner_node.cpp | 33 +++++++------------ 4 files changed, 12 insertions(+), 41 deletions(-) delete mode 100644 rr_common/dyn_reconf_plugins.xml delete mode 100644 rr_common/include/rr_common/planner_dyn_reconf.h diff --git a/rr_common/dyn_reconf_plugins.xml b/rr_common/dyn_reconf_plugins.xml deleted file mode 100644 index 064ca9091..000000000 --- a/rr_common/dyn_reconf_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Enable Dynamic Reconfigure For Planner Node - - - \ No newline at end of file diff --git a/rr_common/include/rr_common/planner_dyn_reconf.h b/rr_common/include/rr_common/planner_dyn_reconf.h deleted file mode 100644 index d89d1ce32..000000000 --- a/rr_common/include/rr_common/planner_dyn_reconf.h +++ /dev/null @@ -1,12 +0,0 @@ -#include -#include -#include - -namespace planner_dyn_reconf_ns { - - void dynamic_callback(rr_common::PathPlannerConfig&, uint32_t); - class planner_node : public nodelet::Nodelet { - private: - virtual void onInit(); - }; -} \ No newline at end of file diff --git a/rr_common/package.xml b/rr_common/package.xml index 4a60068ee..96e9a0f2f 100644 --- a/rr_common/package.xml +++ b/rr_common/package.xml @@ -32,6 +32,5 @@ - diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index c0f939d06..c86da0597 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include constexpr int ctrl_dim = 1; @@ -162,27 +163,20 @@ void processMap() { } } -namespace planner_dyn_reconf_ns { - void dynamic_callback(rr_common::PathPlannerConfig &config, uint32_t level) { - // need a smart way of only displaying the param that was changed - // for now just display a generic message - ROS_INFO("Reconfigure Request Processed"); - } - - void planner_node::onInit() { - dynamic_reconfigure::Server DynReconfigServer; - dynamic_reconfigure::Server::CallbackType f; - - f = boost::bind(&dynamic_callback, _1, _2); - DynReconfigServer.setCallback(f); - } +void dynamic_callback(rr_common::PathPlannerConfig &config, uint32_t level) { + // need a smart way of only displaying the param that was changed + // for now just display a generic message + ROS_INFO("Reconfigure Request Processed"); } - int main(int argc, char** argv) { ros::init(argc, argv, "planner"); - planner_dyn_reconf_ns::planner_node::onInit(); + dynamic_reconfigure::Server DynReconfigServer; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&dynamic_callback, _1, _2); + DynReconfigServer.setCallback(f); ros::NodeHandle nh; ros::NodeHandle nhp("~"); @@ -281,7 +275,4 @@ int main(int argc, char** argv) { } return 0; -} - - -PLUGINLIB_EXPORT_CLASS(planner_dyn_reconf_ns::planner_node, nodelet::Nodelet) +} \ No newline at end of file From e454f44720e394f086d09b7f2eb945d200f8cae4 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 31 Jan 2021 20:48:26 -0500 Subject: [PATCH 06/24] formatting --- rr_common/src/planner/planner_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index c86da0597..1826d75fe 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -1,9 +1,11 @@ +#include #include #include #include #include #include #include +#include #include #include #include @@ -14,10 +16,8 @@ #include #include #include -#include -#include -#include +#include constexpr int ctrl_dim = 1; @@ -163,7 +163,7 @@ void processMap() { } } -void dynamic_callback(rr_common::PathPlannerConfig &config, uint32_t level) { +void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { // need a smart way of only displaying the param that was changed // for now just display a generic message ROS_INFO("Reconfigure Request Processed"); From 9dc03501686814add6daf2b62838cfcc4d1e406a Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 14 Feb 2021 18:01:30 -0500 Subject: [PATCH 07/24] bicycle model pointers not working --- rr_common/cfg/planner_configuration.cfg | 24 ++++++------------- .../rr_common/planning/bicycle_model.h | 20 ++++++++++++++++ rr_common/src/planner/bicycle_model.cpp | 17 +++++++++++++ rr_common/src/planner/planner_node.cpp | 12 +++++++--- 4 files changed, 53 insertions(+), 20 deletions(-) diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index 1203c3a30..90b505854 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -5,22 +5,12 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("num_path_segments", int_t, 0, "Number of path segments for the path representation", 5, 1, 10) -gen.add("step_size", double_t, 0, "The discrete step size for forward path simulation", 0.5, 0.01, 1.0) -gen.add("num_steps", int_t, 0, "Numbers of discrete steps per path segments", 50, 1, 100) -gen.add("max_fow_acceleration", double_t, 0, "Max forward acceleration of vehicle model", 10.0, 0.5, 20.0) -gen.add("max_lat_acceleration", double_t, 0, "Max lateral acceleration of vehicle model", 5.0, 0.5, 10.0) -gen.add("steer_rate", double_t, 0, "Steering rate of vehicle model", 15.0, 0.1, 30.0) -gen.add("steer_angle", double_t, 0, "Max steering angle for vehicle model", 0.75, 0.01, 1.5) -gen.add("slip_compensation_gain", double_t, 0, "Steering compensation gain for vehicle model", 1.15, 0.8, 1.5) -gen.add("speed_cost", double_t, 0,"Speed coefficient for cost function", 5.0, 0.0, 10.0) -gen.add("obstacle_clearance_cost", double_t, 0, "Obstacle clearing coefficient for cost function", 5.0, 0.0, 10.0) -gen.add("heading_cost", double_t, 0, "Heading coefficient for cost function", 5.0, 0.0, 10.0) -gen.add("forward_distance_cost", double_t, 0, "Forward distance coefficient for cost function", 50.0, 0.0, 100.0) -gen.add("collision_cost", double_t, 0, "Collision coefficient for cost function", 250.0, 0.0, 500.0) -gen.add("start_temperature", double_t, 0, "Simulated starting temperature", 0.5, 0.0, 1.0) -gen.add("end_temperature", double_t, 0, "Simulated ending temperature", 0.5, 0.0, 1.0) -gen.add("iterations", int_t, 0, "Number of iterations", 2500, 10, 5000) -gen.add("cost_scale_factor", double_t, 0, "Scale factor for the cost", 0.5, 0.0, 1.0) +# Planner Params +gen.add("n_segments", int_t, 0, "planner: segment size", 7, 1, 14) + +# Bicycle Model Params (arbitrary values) +gen.add("max_lateral_accel", double_t, 0, "bicycle model: max lateral acceleration", 7.0, 0.5, 14.0) +gen.add("segment_size", int_t, 0, "bicycle model: segment size", 25, 1, 50) +gen.add("dt", double_t, 0, "bicycle model: dt", 0.02, 0.001, 2.0) exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "PathPlanner")) \ No newline at end of file diff --git a/rr_common/include/rr_common/planning/bicycle_model.h b/rr_common/include/rr_common/planning/bicycle_model.h index 7b0a40ab3..813962919 100644 --- a/rr_common/include/rr_common/planning/bicycle_model.h +++ b/rr_common/include/rr_common/planning/bicycle_model.h @@ -32,6 +32,26 @@ class BicycleModel { // void RollOutPath(const Controls<2>& controls, std::vector& path_points) const; + /** + * Setter for the lateral acceleration. + * @param imax_lateral_accel input lateral acceleration + */ + void set_max_lateral_accel(double imax_lateral_accel); + + /** + * Setter for segment size. + * @param isegment_size input segment size + */ + void set_segment_size(int isegment_size); + + /** + * Setter for the dt + * @param idt input dt + */ + void set_dt(double idt); + + double get_dt(); + private: /** * Calculate a desired speed from a steering angle based on diff --git a/rr_common/src/planner/bicycle_model.cpp b/rr_common/src/planner/bicycle_model.cpp index 06cb837d3..4a9cf143c 100644 --- a/rr_common/src/planner/bicycle_model.cpp +++ b/rr_common/src/planner/bicycle_model.cpp @@ -63,6 +63,23 @@ void BicycleModel::RollOutPath(const Controls<1>& controls, TrajectoryRollout& r rollout.apply_speed = speed_model_temp.GetValue(); } + +void BicycleModel::set_max_lateral_accel(double imax_lateral_accel) { + max_lateral_accel_ = imax_lateral_accel; +} + +void BicycleModel::set_segment_size(int isegment_size) { + segment_size_ = isegment_size; +} + +void BicycleModel::set_dt(double idt) { + dt_ = idt; +} + +double BicycleModel::get_dt(){ + return dt_; +} + void BicycleModel::StepKinematics(const PathPoint& prev, Pose& next) const { double deltaX, deltaY, deltaTheta; double distance_increment = prev.speed * dt_; diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 1826d75fe..e3d42d858 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -164,9 +164,15 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { - // need a smart way of only displaying the param that was changed - // for now just display a generic message - ROS_INFO("Reconfigure Request Processed"); + // g_vehicle_model -> set_max_lateral_accel(config.max_lateral_accel); + // g_vehicle_model -> set_segment_size(config.segment_size); + // g_vehicle_model -> set_dt(config.dt); + + int n_segments = config.n_segments; + g_last_controls = rr::Controls(ctrl_dim, n_segments); + g_last_controls.setZero(); + + ROS_INFO("\n\nReconfigure Request Occurred %d\n\n", n_segments); } int main(int argc, char** argv) { From 4eebfc81759dcb6e5df31a31bcca65888e527404 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 15 Feb 2021 19:46:51 -0500 Subject: [PATCH 08/24] try mutex --- rr_common/src/planner/planner_node.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index e3d42d858..2039ac3ea 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -17,6 +17,9 @@ #include #include +/* temp */ +#include + #include constexpr int ctrl_dim = 1; @@ -28,6 +31,7 @@ std::unique_ptr g_effector_tracker; std::shared_ptr g_speed_model; std::shared_ptr g_steer_model; +std::mutex mtx; double k_map_cost_, k_speed_, k_steering_, k_angle_, collision_penalty_; rr::Controls g_last_controls; @@ -164,7 +168,8 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { - // g_vehicle_model -> set_max_lateral_accel(config.max_lateral_accel); + mtx.lock(); + g_vehicle_model -> set_max_lateral_accel(config.max_lateral_accel); // g_vehicle_model -> set_segment_size(config.segment_size); // g_vehicle_model -> set_dt(config.dt); @@ -172,7 +177,9 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { g_last_controls = rr::Controls(ctrl_dim, n_segments); g_last_controls.setZero(); + mtx.unlock(); ROS_INFO("\n\nReconfigure Request Occurred %d\n\n", n_segments); + ROS_INFO(typeid(config.n_segments).name()); } int main(int argc, char** argv) { @@ -267,10 +274,13 @@ int main(int argc, char** argv) { if (g_map_cost_interface->IsMapUpdated()) { auto start = ros::WallTime::now(); + mtx.lock(); g_map_cost_interface->StopUpdates(); processMap(); g_map_cost_interface->SetMapStale(); g_map_cost_interface->StartUpdates(); + mtx.unlock(); + double seconds = (ros::WallTime::now() - start).toSec(); total_planning_time += seconds; From 98704cfff76689dab7a92286f76d06dfa276db44 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 15 Feb 2021 20:22:53 -0500 Subject: [PATCH 09/24] adds dyn reconf to bicycle model --- .../rr_common/planning/bicycle_model.h | 22 ++++----------- rr_common/src/planner/bicycle_model.cpp | 19 +++---------- rr_common/src/planner/planner_node.cpp | 28 ++++++------------- 3 files changed, 18 insertions(+), 51 deletions(-) diff --git a/rr_common/include/rr_common/planning/bicycle_model.h b/rr_common/include/rr_common/planning/bicycle_model.h index 813962919..9d81e62b9 100644 --- a/rr_common/include/rr_common/planning/bicycle_model.h +++ b/rr_common/include/rr_common/planning/bicycle_model.h @@ -33,24 +33,12 @@ class BicycleModel { // void RollOutPath(const Controls<2>& controls, std::vector& path_points) const; /** - * Setter for the lateral acceleration. - * @param imax_lateral_accel input lateral acceleration + * Setter for the dynamic reconfigure variables in the bicycle model. + * @param max_lateral_accel input lateral acceleration + * @param segment_size input segment size + * @param dt input dt */ - void set_max_lateral_accel(double imax_lateral_accel); - - /** - * Setter for segment size. - * @param isegment_size input segment size - */ - void set_segment_size(int isegment_size); - - /** - * Setter for the dt - * @param idt input dt - */ - void set_dt(double idt); - - double get_dt(); + void set_dyn_param(double max_lateral_accel, int segment_size, double dt); private: /** diff --git a/rr_common/src/planner/bicycle_model.cpp b/rr_common/src/planner/bicycle_model.cpp index 4a9cf143c..f4d57ce30 100644 --- a/rr_common/src/planner/bicycle_model.cpp +++ b/rr_common/src/planner/bicycle_model.cpp @@ -63,21 +63,10 @@ void BicycleModel::RollOutPath(const Controls<1>& controls, TrajectoryRollout& r rollout.apply_speed = speed_model_temp.GetValue(); } - -void BicycleModel::set_max_lateral_accel(double imax_lateral_accel) { - max_lateral_accel_ = imax_lateral_accel; -} - -void BicycleModel::set_segment_size(int isegment_size) { - segment_size_ = isegment_size; -} - -void BicycleModel::set_dt(double idt) { - dt_ = idt; -} - -double BicycleModel::get_dt(){ - return dt_; +void BicycleModel::set_dyn_param(double max_lateral_accel, int segment_size, double dt) { + max_lateral_accel_ = max_lateral_accel; + segment_size_ = segment_size; + dt_ = dt; } void BicycleModel::StepKinematics(const PathPoint& prev, Pose& next) const { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 2039ac3ea..e56e1a2b6 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -17,9 +17,6 @@ #include #include -/* temp */ -#include - #include constexpr int ctrl_dim = 1; @@ -168,29 +165,19 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { - mtx.lock(); - g_vehicle_model -> set_max_lateral_accel(config.max_lateral_accel); - // g_vehicle_model -> set_segment_size(config.segment_size); - // g_vehicle_model -> set_dt(config.dt); + g_vehicle_model->set_dyn_param(config.max_lateral_accel, config.segment_size, config.dt); int n_segments = config.n_segments; g_last_controls = rr::Controls(ctrl_dim, n_segments); g_last_controls.setZero(); - mtx.unlock(); - ROS_INFO("\n\nReconfigure Request Occurred %d\n\n", n_segments); - ROS_INFO(typeid(config.n_segments).name()); + ROS_INFO("\n\nReconfigure Request Occurred\nBicycle Model: %f %d %f \n", config.max_lateral_accel, + config.segment_size, config.dt); } int main(int argc, char** argv) { ros::init(argc, argv, "planner"); - dynamic_reconfigure::Server DynReconfigServer; - dynamic_reconfigure::Server::CallbackType f; - - f = boost::bind(&dynamic_callback, _1, _2); - DynReconfigServer.setCallback(f); - ros::NodeHandle nh; ros::NodeHandle nhp("~"); @@ -247,6 +234,12 @@ int main(int argc, char** argv) { steer_pub = nh.advertise("plan/steering", 1); viz_pub = nh.advertise("plan/path", 1); + // init dynamic reconfigure + dynamic_reconfigure::Server DynReconfigServer; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&dynamic_callback, _1, _2); + DynReconfigServer.setCallback(f); + speed_message.reset(new rr_msgs::speed); steer_message.reset(new rr_msgs::steering); update_messages(0, 0); @@ -274,13 +267,10 @@ int main(int argc, char** argv) { if (g_map_cost_interface->IsMapUpdated()) { auto start = ros::WallTime::now(); - mtx.lock(); g_map_cost_interface->StopUpdates(); processMap(); g_map_cost_interface->SetMapStale(); g_map_cost_interface->StartUpdates(); - mtx.unlock(); - double seconds = (ros::WallTime::now() - start).toSec(); total_planning_time += seconds; From 1a539a8fe52284756fb9a5ffc0fce4065f3aa587 Mon Sep 17 00:00:00 2001 From: Udit Subramnya Date: Mon, 15 Feb 2021 20:22:53 -0500 Subject: [PATCH 10/24] adds dyn reconf to bicycle model --- .../rr_common/planning/bicycle_model.h | 22 ++++----------- rr_common/src/planner/bicycle_model.cpp | 19 +++---------- rr_common/src/planner/planner_node.cpp | 28 ++++++------------- 3 files changed, 18 insertions(+), 51 deletions(-) diff --git a/rr_common/include/rr_common/planning/bicycle_model.h b/rr_common/include/rr_common/planning/bicycle_model.h index 813962919..9d81e62b9 100644 --- a/rr_common/include/rr_common/planning/bicycle_model.h +++ b/rr_common/include/rr_common/planning/bicycle_model.h @@ -33,24 +33,12 @@ class BicycleModel { // void RollOutPath(const Controls<2>& controls, std::vector& path_points) const; /** - * Setter for the lateral acceleration. - * @param imax_lateral_accel input lateral acceleration + * Setter for the dynamic reconfigure variables in the bicycle model. + * @param max_lateral_accel input lateral acceleration + * @param segment_size input segment size + * @param dt input dt */ - void set_max_lateral_accel(double imax_lateral_accel); - - /** - * Setter for segment size. - * @param isegment_size input segment size - */ - void set_segment_size(int isegment_size); - - /** - * Setter for the dt - * @param idt input dt - */ - void set_dt(double idt); - - double get_dt(); + void set_dyn_param(double max_lateral_accel, int segment_size, double dt); private: /** diff --git a/rr_common/src/planner/bicycle_model.cpp b/rr_common/src/planner/bicycle_model.cpp index 4a9cf143c..f4d57ce30 100644 --- a/rr_common/src/planner/bicycle_model.cpp +++ b/rr_common/src/planner/bicycle_model.cpp @@ -63,21 +63,10 @@ void BicycleModel::RollOutPath(const Controls<1>& controls, TrajectoryRollout& r rollout.apply_speed = speed_model_temp.GetValue(); } - -void BicycleModel::set_max_lateral_accel(double imax_lateral_accel) { - max_lateral_accel_ = imax_lateral_accel; -} - -void BicycleModel::set_segment_size(int isegment_size) { - segment_size_ = isegment_size; -} - -void BicycleModel::set_dt(double idt) { - dt_ = idt; -} - -double BicycleModel::get_dt(){ - return dt_; +void BicycleModel::set_dyn_param(double max_lateral_accel, int segment_size, double dt) { + max_lateral_accel_ = max_lateral_accel; + segment_size_ = segment_size; + dt_ = dt; } void BicycleModel::StepKinematics(const PathPoint& prev, Pose& next) const { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 2039ac3ea..e56e1a2b6 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -17,9 +17,6 @@ #include #include -/* temp */ -#include - #include constexpr int ctrl_dim = 1; @@ -168,29 +165,19 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { - mtx.lock(); - g_vehicle_model -> set_max_lateral_accel(config.max_lateral_accel); - // g_vehicle_model -> set_segment_size(config.segment_size); - // g_vehicle_model -> set_dt(config.dt); + g_vehicle_model->set_dyn_param(config.max_lateral_accel, config.segment_size, config.dt); int n_segments = config.n_segments; g_last_controls = rr::Controls(ctrl_dim, n_segments); g_last_controls.setZero(); - mtx.unlock(); - ROS_INFO("\n\nReconfigure Request Occurred %d\n\n", n_segments); - ROS_INFO(typeid(config.n_segments).name()); + ROS_INFO("\n\nReconfigure Request Occurred\nBicycle Model: %f %d %f \n", config.max_lateral_accel, + config.segment_size, config.dt); } int main(int argc, char** argv) { ros::init(argc, argv, "planner"); - dynamic_reconfigure::Server DynReconfigServer; - dynamic_reconfigure::Server::CallbackType f; - - f = boost::bind(&dynamic_callback, _1, _2); - DynReconfigServer.setCallback(f); - ros::NodeHandle nh; ros::NodeHandle nhp("~"); @@ -247,6 +234,12 @@ int main(int argc, char** argv) { steer_pub = nh.advertise("plan/steering", 1); viz_pub = nh.advertise("plan/path", 1); + // init dynamic reconfigure + dynamic_reconfigure::Server DynReconfigServer; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&dynamic_callback, _1, _2); + DynReconfigServer.setCallback(f); + speed_message.reset(new rr_msgs::speed); steer_message.reset(new rr_msgs::steering); update_messages(0, 0); @@ -274,13 +267,10 @@ int main(int argc, char** argv) { if (g_map_cost_interface->IsMapUpdated()) { auto start = ros::WallTime::now(); - mtx.lock(); g_map_cost_interface->StopUpdates(); processMap(); g_map_cost_interface->SetMapStale(); g_map_cost_interface->StartUpdates(); - mtx.unlock(); - double seconds = (ros::WallTime::now() - start).toSec(); total_planning_time += seconds; From aa2f9e635fba0df504f542e96ead542651a861fd Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 21 Feb 2021 17:30:53 -0500 Subject: [PATCH 11/24] adds steadds speed and steering filter dyn conf --- rr_common/cfg/planner_configuration.cfg | 14 +++++++++++++- .../include/rr_common/linear_tracking_filter.hpp | 14 ++++++++++++++ .../include/rr_common/planning/bicycle_model.h | 2 +- rr_common/src/planner/bicycle_model.cpp | 2 +- rr_common/src/planner/planner_node.cpp | 12 +++++++----- 5 files changed, 36 insertions(+), 8 deletions(-) diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index 90b505854..74c9b39e4 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -8,9 +8,21 @@ gen = ParameterGenerator() # Planner Params gen.add("n_segments", int_t, 0, "planner: segment size", 7, 1, 14) -# Bicycle Model Params (arbitrary values) +# Bicycle Model Params gen.add("max_lateral_accel", double_t, 0, "bicycle model: max lateral acceleration", 7.0, 0.5, 14.0) gen.add("segment_size", int_t, 0, "bicycle model: segment size", 25, 1, 50) gen.add("dt", double_t, 0, "bicycle model: dt", 0.02, 0.001, 2.0) +# Linear Tracking Filter - Speed +gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.1, 26.0); +gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, 0.1, 2.0); +gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.1, 40.0); +gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, 0.1, 50.0); + +# Linear Tracking Filter - Steering +gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.01, 0.50); +gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, 0.01, 0.50); +gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.01, 4.0); +gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, 0.01, 4.0); + exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "PathPlanner")) \ No newline at end of file diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index 0c1183e45..137d2b1f3 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -51,6 +51,20 @@ class LinearTrackingFilter { target_ = x; } + /** + * Setter for the dynamic reconfigure variables in the linear tracking filter. + * @param val_max input max val + * @param val_min input min val + * @param rate_max input max rate + * @param rate_min input min rate + */ + inline void SetDynParam(double val_max, double val_min, double rate_max, double rate_min) { + val_max_ = val_max; + val_min_ = val_min; + rate_max_ = rate_max; + rate_min_ = rate_min; + } + inline void Update(double t) { if (last_update_ > 0) { double dt = t - last_update_; diff --git a/rr_common/include/rr_common/planning/bicycle_model.h b/rr_common/include/rr_common/planning/bicycle_model.h index 9d81e62b9..0de8b7f2b 100644 --- a/rr_common/include/rr_common/planning/bicycle_model.h +++ b/rr_common/include/rr_common/planning/bicycle_model.h @@ -38,7 +38,7 @@ class BicycleModel { * @param segment_size input segment size * @param dt input dt */ - void set_dyn_param(double max_lateral_accel, int segment_size, double dt); + void SetDynParam(double max_lateral_accel, int segment_size, double dt); private: /** diff --git a/rr_common/src/planner/bicycle_model.cpp b/rr_common/src/planner/bicycle_model.cpp index f4d57ce30..9f9c31ca4 100644 --- a/rr_common/src/planner/bicycle_model.cpp +++ b/rr_common/src/planner/bicycle_model.cpp @@ -63,7 +63,7 @@ void BicycleModel::RollOutPath(const Controls<1>& controls, TrajectoryRollout& r rollout.apply_speed = speed_model_temp.GetValue(); } -void BicycleModel::set_dyn_param(double max_lateral_accel, int segment_size, double dt) { +void BicycleModel::SetDynParam(double max_lateral_accel, int segment_size, double dt) { max_lateral_accel_ = max_lateral_accel; segment_size_ = segment_size; dt_ = dt; diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index e56e1a2b6..8b3656dd3 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -165,14 +165,16 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { - g_vehicle_model->set_dyn_param(config.max_lateral_accel, config.segment_size, config.dt); + g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt); - int n_segments = config.n_segments; - g_last_controls = rr::Controls(ctrl_dim, n_segments); + g_last_controls = rr::Controls(ctrl_dim, config.n_segments); g_last_controls.setZero(); - ROS_INFO("\n\nReconfigure Request Occurred\nBicycle Model: %f %d %f \n", config.max_lateral_accel, - config.segment_size, config.dt); + g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); + g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); + + ROS_INFO("\n\nSPF: %f %f %f %f\nSTF: %f %f %f %f \n ", config.spf_val_max, config.spf_val_min, config.spf_rate_max, + config.spf_rate_min, config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); } int main(int argc, char** argv) { From e83755f3a78c01ebdd61dd81ba0d8ad655ebb437 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 21 Feb 2021 17:54:55 -0500 Subject: [PATCH 12/24] adds constants to dyn config --- rr_common/cfg/planner_configuration.cfg | 24 ++++++++++++++++-------- rr_common/src/planner/planner_node.cpp | 16 +++++++++++----- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index 74c9b39e4..b7fd27723 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -14,15 +14,23 @@ gen.add("segment_size", int_t, 0, "bicycle model: segment size", 25, 1, 50) gen.add("dt", double_t, 0, "bicycle model: dt", 0.02, 0.001, 2.0) # Linear Tracking Filter - Speed -gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.1, 26.0); -gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, 0.1, 2.0); -gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.1, 40.0); -gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, 0.1, 50.0); +gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.1, 26.0) +gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, 0.1, 2.0) +gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.1, 40.0) +gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, 0.1, 50.0) # Linear Tracking Filter - Steering -gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.01, 0.50); -gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, 0.01, 0.50); -gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.01, 4.0); -gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, 0.01, 4.0); +gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.01, 0.50) +gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, 0.01, 0.50) +gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.01, 4.0) +gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, 0.01, 4.0) + +# Control Constants +gen.add("steering_gain", double_t, 0, "steering gain", 1.4, 0.01, 3.0) +gen.add("k_map_cost", double_t, 0, "cost: map constant", 0.1, 0.01, 1.0) +gen.add("k_speed", double_t, 0, "cost: speed constant", 0.05, 0.01, 1.0) +gen.add("k_steering", double_t, 0, "cost: steering constant", 0.01, 0.01, 1.0) +gen.add("k_angle", double_t, 0, "cost: angle constant", 0.00, 0.01, 1.0) +gen.add("collision_penalty", double_t, 0, "cost: collision penalty", 10000, 1000, 20000) exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "PathPlanner")) \ No newline at end of file diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 8b3656dd3..16d06a874 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -48,7 +48,7 @@ ros::Time caution_start_time; ros::Time reverse_start_time; reverse_state_t reverse_state; -double steering_gain; +double steering_gain_; double total_planning_time; size_t total_plans; @@ -143,7 +143,7 @@ void processMap() { ROS_WARN_STREAM("Planner: no path found but not reversing; reusing previous message"); } else { g_speed_model->Update(plan.rollout.apply_speed, now.toSec()); - update_messages(g_speed_model->GetValue(), plan.rollout.apply_steering * steering_gain); + update_messages(g_speed_model->GetValue(), plan.rollout.apply_steering * steering_gain_); } speed_pub.publish(speed_message); @@ -173,8 +173,14 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); - ROS_INFO("\n\nSPF: %f %f %f %f\nSTF: %f %f %f %f \n ", config.spf_val_max, config.spf_val_min, config.spf_rate_max, - config.spf_rate_min, config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); + k_map_cost_ = config.k_map_cost; + k_speed_ = config.k_speed; + k_steering_ = config.k_steering; + k_angle_ = config.k_angle; + collision_penalty_ = config.collision_penalty; + steering_gain_ = config.steering_gain; + + ROS_INFO("Dyn Reconf Updated"); } int main(int argc, char** argv) { @@ -230,7 +236,7 @@ int main(int argc, char** argv) { reverse_start_time = ros::Time(0); reverse_state = OK; - steering_gain = assertions::param(nhp, "steering_gain", 1.0); + steering_gain_ = assertions::param(nhp, "steering_gain_", 1.0); speed_pub = nh.advertise("plan/speed", 1); steer_pub = nh.advertise("plan/steering", 1); From e770aa542a37d97fb3fd2f360627e5536680e248 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 21 Feb 2021 17:58:46 -0500 Subject: [PATCH 13/24] removes mutex variable --- rr_common/src/planner/planner_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 16d06a874..b3633f7c0 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -28,7 +28,6 @@ std::unique_ptr g_effector_tracker; std::shared_ptr g_speed_model; std::shared_ptr g_steer_model; -std::mutex mtx; double k_map_cost_, k_speed_, k_steering_, k_angle_, collision_penalty_; rr::Controls g_last_controls; From 04a228fc99598056452c5ed3ee5df20a5ad5a4a0 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 22 Feb 2021 18:53:32 -0500 Subject: [PATCH 14/24] fixes minimum bounds --- rr_common/cfg/planner_configuration.cfg | 30 ++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index b7fd27723..cc6fe71a0 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -9,28 +9,28 @@ gen = ParameterGenerator() gen.add("n_segments", int_t, 0, "planner: segment size", 7, 1, 14) # Bicycle Model Params -gen.add("max_lateral_accel", double_t, 0, "bicycle model: max lateral acceleration", 7.0, 0.5, 14.0) +gen.add("max_lateral_accel", double_t, 0, "bicycle model: max lateral acceleration", 7.0, 0.0, 14.0) gen.add("segment_size", int_t, 0, "bicycle model: segment size", 25, 1, 50) gen.add("dt", double_t, 0, "bicycle model: dt", 0.02, 0.001, 2.0) # Linear Tracking Filter - Speed -gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.1, 26.0) -gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, 0.1, 2.0) -gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.1, 40.0) -gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, 0.1, 50.0) +gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.0, 26.0) +gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, -2.0, 0.0) +gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.0, 40.0) +gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, -50.0, 0.0) # Linear Tracking Filter - Steering -gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.01, 0.50) -gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, 0.01, 0.50) -gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.01, 4.0) -gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, 0.01, 4.0) +gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.00, 0.50) +gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, -0.50, 0.0) +gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.0, 4.0) +gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, -4.0, 0.0) # Control Constants -gen.add("steering_gain", double_t, 0, "steering gain", 1.4, 0.01, 3.0) -gen.add("k_map_cost", double_t, 0, "cost: map constant", 0.1, 0.01, 1.0) -gen.add("k_speed", double_t, 0, "cost: speed constant", 0.05, 0.01, 1.0) -gen.add("k_steering", double_t, 0, "cost: steering constant", 0.01, 0.01, 1.0) -gen.add("k_angle", double_t, 0, "cost: angle constant", 0.00, 0.01, 1.0) -gen.add("collision_penalty", double_t, 0, "cost: collision penalty", 10000, 1000, 20000) +gen.add("steering_gain", double_t, 0, "steering gain", 1.4, 0.00, 3.0) +gen.add("k_map_cost", double_t, 0, "cost: map constant", 0.1, 0.00, 1.0) +gen.add("k_speed", double_t, 0, "cost: speed constant", 0.05, 0.00, 1.0) +gen.add("k_steering", double_t, 0, "cost: steering constant", 0.01, 0.00, 1.0) +gen.add("k_angle", double_t, 0, "cost: angle constant", 0.00, 0.00, 1.0) +gen.add("collision_penalty", double_t, 0, "cost: collision penalty", 10000, 100, 20000) exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "PathPlanner")) \ No newline at end of file From 5e75b814a65107f63d675bf1717d8dc6cb1aed86 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 1 Mar 2021 19:25:23 -0500 Subject: [PATCH 15/24] adds first loop default value getters --- rr_common/cfg/planner_configuration.cfg | 3 +- .../rr_common/linear_tracking_filter.hpp | 7 +++ .../rr_common/planning/bicycle_model.h | 1 + rr_common/src/planner/bicycle_model.cpp | 7 +++ rr_common/src/planner/planner_node.cpp | 54 ++++++++++++++----- 5 files changed, 57 insertions(+), 15 deletions(-) diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index cc6fe71a0..61c77c51c 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -6,7 +6,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() # Planner Params -gen.add("n_segments", int_t, 0, "planner: segment size", 7, 1, 14) +# Must be 10 or less provided the matrix size +gen.add("n_segments", int_t, 0, "planner: segment size", 7, 1, 10) # Bicycle Model Params gen.add("max_lateral_accel", double_t, 0, "bicycle model: max lateral acceleration", 7.0, 0.0, 14.0) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index 137d2b1f3..ef8f44293 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -65,6 +65,13 @@ class LinearTrackingFilter { rate_min_ = rate_min; } + inline void GetDynParamDefaults(double val_max, double val_min, double rate_max, double rate_min) { + val_max = val_max_; + val_min = val_min_; + rate_max = rate_max_; + rate_min = rate_min_; + } + inline void Update(double t) { if (last_update_ > 0) { double dt = t - last_update_; diff --git a/rr_common/include/rr_common/planning/bicycle_model.h b/rr_common/include/rr_common/planning/bicycle_model.h index 0de8b7f2b..b5ff4de90 100644 --- a/rr_common/include/rr_common/planning/bicycle_model.h +++ b/rr_common/include/rr_common/planning/bicycle_model.h @@ -39,6 +39,7 @@ class BicycleModel { * @param dt input dt */ void SetDynParam(double max_lateral_accel, int segment_size, double dt); + void GetDynParamDefaults(double max_lateral_accel, int segment_size, double dt); private: /** diff --git a/rr_common/src/planner/bicycle_model.cpp b/rr_common/src/planner/bicycle_model.cpp index 9f9c31ca4..0174094ae 100644 --- a/rr_common/src/planner/bicycle_model.cpp +++ b/rr_common/src/planner/bicycle_model.cpp @@ -69,6 +69,13 @@ void BicycleModel::SetDynParam(double max_lateral_accel, int segment_size, doubl dt_ = dt; } +void BicycleModel::GetDynParamDefaults(double max_lateral_accel, int segment_size, double dt) { + max_lateral_accel = max_lateral_accel_; + segment_size = segment_size_; + dt = dt_; + ROS_INFO("\n\n Bicycle Model Values: %f, %d, %f \n\n", max_lateral_accel, segment_size, dt); +} + void BicycleModel::StepKinematics(const PathPoint& prev, Pose& next) const { double deltaX, deltaY, deltaTheta; double distance_increment = prev.speed * dt_; diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index b3633f7c0..4e0ea6e06 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -52,6 +52,10 @@ double steering_gain_; double total_planning_time; size_t total_plans; +int n_control_points_ = 0; + +static bool firstLoop = true; + void update_messages(double speed, double angle) { auto now = ros::Time::now(); @@ -164,21 +168,43 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { - g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt); + if (firstLoop) { + g_vehicle_model->GetDynParamDefaults(config.max_lateral_accel, config.segment_size, config.dt); + + g_speed_model->GetDynParamDefaults(config.spf_val_max, config.spf_val_min, config.spf_rate_max, + config.spf_rate_min); + g_steer_model->GetDynParamDefaults(config.stf_val_max, config.stf_val_min, config.stf_rate_max, + config.stf_rate_min); + + config.n_segments = n_control_points_; + config.k_map_cost = k_map_cost_; + config.k_speed = k_speed_; + config.k_steering = k_steering_; + config.k_angle = k_angle_; + config.collision_penalty = collision_penalty_; + config.steering_gain = steering_gain_; + ROS_INFO("\n\nCallback First Loop: %d \n\n", config.n_segments); - g_last_controls = rr::Controls(ctrl_dim, config.n_segments); - g_last_controls.setZero(); + } else { + g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt); + + g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); + g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); - g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); - g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); + g_last_controls = rr::Controls(ctrl_dim, config.n_segments); + g_last_controls.setZero(); - k_map_cost_ = config.k_map_cost; - k_speed_ = config.k_speed; - k_steering_ = config.k_steering; - k_angle_ = config.k_angle; - collision_penalty_ = config.collision_penalty; - steering_gain_ = config.steering_gain; + k_map_cost_ = config.k_map_cost; + k_speed_ = config.k_speed; + k_steering_ = config.k_steering; + k_angle_ = config.k_angle; + collision_penalty_ = config.collision_penalty; + steering_gain_ = config.steering_gain; + ROS_INFO("\n\nCallback First Loop: %d \n\n", config.n_segments); + } + // ROS_INFO("\n\n Bicycle Model Values Callback: %f, %d, %f \n\n", config.max_lateral_accel,config.segment_size, + // config.dt); ROS_INFO("Dyn Reconf Updated"); } @@ -224,9 +250,8 @@ int main(int argc, char** argv) { ros::shutdown(); } - int n_control_points = 0; - assertions::getParam(nhp, "n_segments", n_control_points); - g_last_controls = rr::Controls(ctrl_dim, n_control_points); + assertions::getParam(nhp, "n_segments", n_control_points_); + g_last_controls = rr::Controls(ctrl_dim, n_control_points_); g_last_controls.setZero(); caution_duration = ros::Duration(assertions::param(nhp, "impasse_caution_duration", 0.0)); @@ -285,6 +310,7 @@ int main(int argc, char** argv) { double sec_avg = total_planning_time / total_plans; ROS_INFO("PlanningOptimizer took %0.1fms, average %0.2fms", seconds * 1000, sec_avg * 1000); } + firstLoop = false; } return 0; From 5fab25b387c1aec84ffc724325f6821868ee3444 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 1 Mar 2021 20:27:18 -0500 Subject: [PATCH 16/24] fixes static bool, pass dyn reconf by reference --- .../rr_common/linear_tracking_filter.hpp | 18 +++++++++++++----- .../include/rr_common/planning/bicycle_model.h | 4 +++- rr_common/src/planner/bicycle_model.cpp | 9 ++++----- rr_common/src/planner/planner_node.cpp | 18 +++++------------- 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index ef8f44293..fc787383c 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -2,6 +2,7 @@ #include #include +#include namespace rr { @@ -65,11 +66,18 @@ class LinearTrackingFilter { rate_min_ = rate_min; } - inline void GetDynParamDefaults(double val_max, double val_min, double rate_max, double rate_min) { - val_max = val_max_; - val_min = val_min_; - rate_max = rate_max_; - rate_min = rate_min_; + inline void GetDynParamDefaultsSpeed(rr_common::PathPlannerConfig& config) { + config.spf_val_max = val_max_; + config.spf_val_min = val_min_; + config.spf_rate_max = rate_max_; + config.spf_rate_min = rate_min_; + } + + inline void GetDynParamDefaultsTurn(rr_common::PathPlannerConfig& config) { + config.stf_val_max = val_max_; + config.stf_val_min = val_min_; + config.stf_rate_max = rate_max_; + config.stf_rate_min = rate_min_; } inline void Update(double t) { diff --git a/rr_common/include/rr_common/planning/bicycle_model.h b/rr_common/include/rr_common/planning/bicycle_model.h index b5ff4de90..e8d5f1074 100644 --- a/rr_common/include/rr_common/planning/bicycle_model.h +++ b/rr_common/include/rr_common/planning/bicycle_model.h @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -39,7 +41,7 @@ class BicycleModel { * @param dt input dt */ void SetDynParam(double max_lateral_accel, int segment_size, double dt); - void GetDynParamDefaults(double max_lateral_accel, int segment_size, double dt); + void GetDynParamDefaults(rr_common::PathPlannerConfig& config); private: /** diff --git a/rr_common/src/planner/bicycle_model.cpp b/rr_common/src/planner/bicycle_model.cpp index 0174094ae..b6ba607d8 100644 --- a/rr_common/src/planner/bicycle_model.cpp +++ b/rr_common/src/planner/bicycle_model.cpp @@ -69,11 +69,10 @@ void BicycleModel::SetDynParam(double max_lateral_accel, int segment_size, doubl dt_ = dt; } -void BicycleModel::GetDynParamDefaults(double max_lateral_accel, int segment_size, double dt) { - max_lateral_accel = max_lateral_accel_; - segment_size = segment_size_; - dt = dt_; - ROS_INFO("\n\n Bicycle Model Values: %f, %d, %f \n\n", max_lateral_accel, segment_size, dt); +void BicycleModel::GetDynParamDefaults(rr_common::PathPlannerConfig& config) { + config.max_lateral_accel = max_lateral_accel_; + config.segment_size = segment_size_; + config.dt = dt_; } void BicycleModel::StepKinematics(const PathPoint& prev, Pose& next) const { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 4e0ea6e06..8029d0251 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -54,8 +54,6 @@ size_t total_plans; int n_control_points_ = 0; -static bool firstLoop = true; - void update_messages(double speed, double angle) { auto now = ros::Time::now(); @@ -168,13 +166,12 @@ void processMap() { } void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { + static bool firstLoop = true; if (firstLoop) { - g_vehicle_model->GetDynParamDefaults(config.max_lateral_accel, config.segment_size, config.dt); + g_vehicle_model->GetDynParamDefaults(config); - g_speed_model->GetDynParamDefaults(config.spf_val_max, config.spf_val_min, config.spf_rate_max, - config.spf_rate_min); - g_steer_model->GetDynParamDefaults(config.stf_val_max, config.stf_val_min, config.stf_rate_max, - config.stf_rate_min); + g_speed_model->GetDynParamDefaultsSpeed(config); + g_steer_model->GetDynParamDefaultsTurn(config); config.n_segments = n_control_points_; config.k_map_cost = k_map_cost_; @@ -183,7 +180,7 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { config.k_angle = k_angle_; config.collision_penalty = collision_penalty_; config.steering_gain = steering_gain_; - ROS_INFO("\n\nCallback First Loop: %d \n\n", config.n_segments); + firstLoop = false; } else { g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt); @@ -200,11 +197,7 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { k_angle_ = config.k_angle; collision_penalty_ = config.collision_penalty; steering_gain_ = config.steering_gain; - ROS_INFO("\n\nCallback First Loop: %d \n\n", config.n_segments); } - - // ROS_INFO("\n\n Bicycle Model Values Callback: %f, %d, %f \n\n", config.max_lateral_accel,config.segment_size, - // config.dt); ROS_INFO("Dyn Reconf Updated"); } @@ -310,7 +303,6 @@ int main(int argc, char** argv) { double sec_avg = total_planning_time / total_plans; ROS_INFO("PlanningOptimizer took %0.1fms, average %0.2fms", seconds * 1000, sec_avg * 1000); } - firstLoop = false; } return 0; From 6136a587ae12a37708e543133c2f1ccb973362d2 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 8 Mar 2021 20:04:08 -0500 Subject: [PATCH 17/24] corrects variable name --- rr_common/src/planner/planner_node.cpp | 10 +++++----- rr_iarrc/conf/planner_sim.yaml | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 8029d0251..61e1c55bc 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -47,7 +47,7 @@ ros::Time caution_start_time; ros::Time reverse_start_time; reverse_state_t reverse_state; -double steering_gain_; +double steering_gain; double total_planning_time; size_t total_plans; @@ -144,7 +144,7 @@ void processMap() { ROS_WARN_STREAM("Planner: no path found but not reversing; reusing previous message"); } else { g_speed_model->Update(plan.rollout.apply_speed, now.toSec()); - update_messages(g_speed_model->GetValue(), plan.rollout.apply_steering * steering_gain_); + update_messages(g_speed_model->GetValue(), plan.rollout.apply_steering * steering_gain); } speed_pub.publish(speed_message); @@ -179,7 +179,7 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { config.k_steering = k_steering_; config.k_angle = k_angle_; config.collision_penalty = collision_penalty_; - config.steering_gain = steering_gain_; + config.steering_gain = steering_gain; firstLoop = false; } else { @@ -196,7 +196,7 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { k_steering_ = config.k_steering; k_angle_ = config.k_angle; collision_penalty_ = config.collision_penalty; - steering_gain_ = config.steering_gain; + steering_gain = config.steering_gain; } ROS_INFO("Dyn Reconf Updated"); } @@ -253,7 +253,7 @@ int main(int argc, char** argv) { reverse_start_time = ros::Time(0); reverse_state = OK; - steering_gain_ = assertions::param(nhp, "steering_gain_", 1.0); + steering_gain = assertions::param(nhp, "steering_gain", 1.0); speed_pub = nh.advertise("plan/speed", 1); steer_pub = nh.advertise("plan/steering", 1); diff --git a/rr_iarrc/conf/planner_sim.yaml b/rr_iarrc/conf/planner_sim.yaml index eba95857c..22b49ef78 100644 --- a/rr_iarrc/conf/planner_sim.yaml +++ b/rr_iarrc/conf/planner_sim.yaml @@ -37,7 +37,7 @@ obstacle_points_map: max_y: 0.15 -steering_gain: 1.1 +steering_gain: 1.5 k_map_cost: 1.0 k_speed: 0.2 From 411c3359670ceda4fa0ff54688108569db43ea94 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 14 Mar 2021 17:09:45 -0400 Subject: [PATCH 18/24] start to split linear tracking dyn reconf --- rr_common/CMakeLists.txt | 1 + rr_common/cfg/lin-tracking-reconf.cfg | 20 +++++++++++ rr_common/cfg/planner_configuration.cfg | 12 ------- .../rr_common/linear_tracking_filter.hpp | 6 ++-- rr_common/src/planner/planner_node.cpp | 34 +++++++++++++------ rr_iarrc/conf/planner_sim.yaml | 2 +- 6 files changed, 49 insertions(+), 26 deletions(-) create mode 100644 rr_common/cfg/lin-tracking-reconf.cfg diff --git a/rr_common/CMakeLists.txt b/rr_common/CMakeLists.txt index cd0dec417..b1fdfcc9e 100644 --- a/rr_common/CMakeLists.txt +++ b/rr_common/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(OpenCV REQUIRED) generate_dynamic_reconfigure_options( cfg/planner_configuration.cfg + cfg/lin-tracking-reconf.cfg ) ################################### diff --git a/rr_common/cfg/lin-tracking-reconf.cfg b/rr_common/cfg/lin-tracking-reconf.cfg new file mode 100644 index 000000000..b20e49ac1 --- /dev/null +++ b/rr_common/cfg/lin-tracking-reconf.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python +PACKAGE = "rr_common" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# Linear Tracking Filter - Speed +gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.0, 26.0) +gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, -2.0, 0.0) +gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.0, 40.0) +gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, -50.0, 0.0) + +# Linear Tracking Filter - Steering +gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.00, 0.50) +gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, -0.50, 0.0) +gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.0, 4.0) +gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, -4.0, 0.0) + +exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "LinearTracking")) \ No newline at end of file diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index 61c77c51c..57462dc1c 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -14,18 +14,6 @@ gen.add("max_lateral_accel", double_t, 0, "bicycle model: max lateral accelerati gen.add("segment_size", int_t, 0, "bicycle model: segment size", 25, 1, 50) gen.add("dt", double_t, 0, "bicycle model: dt", 0.02, 0.001, 2.0) -# Linear Tracking Filter - Speed -gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.0, 26.0) -gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, -2.0, 0.0) -gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.0, 40.0) -gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, -50.0, 0.0) - -# Linear Tracking Filter - Steering -gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.00, 0.50) -gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, -0.50, 0.0) -gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.0, 4.0) -gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, -4.0, 0.0) - # Control Constants gen.add("steering_gain", double_t, 0, "steering gain", 1.4, 0.00, 3.0) gen.add("k_map_cost", double_t, 0, "cost: map constant", 0.1, 0.00, 1.0) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index fc787383c..1e9bde872 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include namespace rr { @@ -66,14 +66,14 @@ class LinearTrackingFilter { rate_min_ = rate_min; } - inline void GetDynParamDefaultsSpeed(rr_common::PathPlannerConfig& config) { + inline void GetDynParamDefaultsSpeed(rr_common::LinearTrackingConfig& config) { config.spf_val_max = val_max_; config.spf_val_min = val_min_; config.spf_rate_max = rate_max_; config.spf_rate_min = rate_min_; } - inline void GetDynParamDefaultsTurn(rr_common::PathPlannerConfig& config) { + inline void GetDynParamDefaultsTurn(rr_common::LinearTrackingConfig& config) { config.stf_val_max = val_max_; config.stf_val_min = val_min_; config.stf_rate_max = rate_max_; diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 61e1c55bc..e9f1defc6 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -165,14 +166,11 @@ void processMap() { } } -void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { +void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t level) { static bool firstLoop = true; if (firstLoop) { g_vehicle_model->GetDynParamDefaults(config); - g_speed_model->GetDynParamDefaultsSpeed(config); - g_steer_model->GetDynParamDefaultsTurn(config); - config.n_segments = n_control_points_; config.k_map_cost = k_map_cost_; config.k_speed = k_speed_; @@ -185,9 +183,6 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { } else { g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt); - g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); - g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); - g_last_controls = rr::Controls(ctrl_dim, config.n_segments); g_last_controls.setZero(); @@ -201,6 +196,20 @@ void dynamic_callback(rr_common::PathPlannerConfig& config, uint32_t level) { ROS_INFO("Dyn Reconf Updated"); } +void dynamic_callback_linear(rr_common::LinearTrackingConfig& config, uint32_t level) { + static bool firstLoop = true; + if (firstLoop) { + g_speed_model->GetDynParamDefaultsSpeed(config); + g_steer_model->GetDynParamDefaultsTurn(config); + firstLoop = false; + + } else { + g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); + g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); + } + ROS_INFO("Dyn Reconf Updated"); +} + int main(int argc, char** argv) { ros::init(argc, argv, "planner"); @@ -260,10 +269,15 @@ int main(int argc, char** argv) { viz_pub = nh.advertise("plan/path", 1); // init dynamic reconfigure - dynamic_reconfigure::Server DynReconfigServer; + dynamic_reconfigure::Server DynReconfigServerPlanner; dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&dynamic_callback, _1, _2); - DynReconfigServer.setCallback(f); + f = boost::bind(&dynamic_callback_planner, _1, _2); + DynReconfigServerPlanner.setCallback(f); + + dynamic_reconfigure::Server DynReconfigServerLinear; + dynamic_reconfigure::Server::CallbackType g; + g = boost::bind(&dynamic_callback_linear, _1, _2); + DynReconfigServerLinear.setCallback(g); speed_message.reset(new rr_msgs::speed); steer_message.reset(new rr_msgs::steering); diff --git a/rr_iarrc/conf/planner_sim.yaml b/rr_iarrc/conf/planner_sim.yaml index 22b49ef78..931aba5f9 100644 --- a/rr_iarrc/conf/planner_sim.yaml +++ b/rr_iarrc/conf/planner_sim.yaml @@ -42,7 +42,7 @@ steering_gain: 1.5 k_map_cost: 1.0 k_speed: 0.2 k_steering: 0 -k_angle: 0.5 +k_angle: 0.0 collision_penalty: 1000 impasse_caution_duration: 0.25 From cc452d2dcd45fcd85b1434333e0aa136ad1b2386 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 28 Mar 2021 16:07:15 -0400 Subject: [PATCH 19/24] dyn reconf for linear compiles; doesnt appear --- rr_common/cfg/lin-tracking-reconf.cfg | 6 ++-- rr_common/cfg/planner_configuration.cfg | 2 +- .../rr_common/linear_tracking_filter.hpp | 18 ++++++++++++ rr_common/src/planner/planner_node.cpp | 29 ++++--------------- 4 files changed, 28 insertions(+), 27 deletions(-) diff --git a/rr_common/cfg/lin-tracking-reconf.cfg b/rr_common/cfg/lin-tracking-reconf.cfg index b20e49ac1..961123c22 100644 --- a/rr_common/cfg/lin-tracking-reconf.cfg +++ b/rr_common/cfg/lin-tracking-reconf.cfg @@ -5,16 +5,16 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -# Linear Tracking Filter - Speed +# Linear Tracking Filter - Speed (TEMP NOW) gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.0, 26.0) gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, -2.0, 0.0) gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.0, 40.0) gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, -50.0, 0.0) -# Linear Tracking Filter - Steering +# Linear Tracking Filter - Steering (TEMP NOW) gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.00, 0.50) gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, -0.50, 0.0) gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.0, 4.0) gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, -4.0, 0.0) -exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "LinearTracking")) \ No newline at end of file +exit(gen.generate(PACKAGE, "rr_common", "LinearTracking")) \ No newline at end of file diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index 57462dc1c..d9a19e3ac 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -22,4 +22,4 @@ gen.add("k_steering", double_t, 0, "cost: steering constant", 0.01, 0.00, 1.0) gen.add("k_angle", double_t, 0, "cost: angle constant", 0.00, 0.00, 1.0) gen.add("collision_penalty", double_t, 0, "cost: collision penalty", 10000, 100, 20000) -exit(gen.generate(PACKAGE, "planner_dyn_reconf_ns", "PathPlanner")) \ No newline at end of file +exit(gen.generate(PACKAGE, "rr_common", "PathPlanner")) \ No newline at end of file diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index 1e9bde872..10221359f 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -3,6 +3,7 @@ #include #include #include +#include namespace rr { @@ -25,6 +26,11 @@ class LinearTrackingFilter { assertions::getParam(nh, "rate_min", rate_min_, { assertions::less(0) }); assertions::getParam(nh, "rate_max", rate_max_, { assertions::greater(0) }); last_update_ = 0; + + std::unique_ptr> dsrv_; + dsrv_ = std::make_unique>(nh); + dsrv_->setCallback(boost::bind(&LinearTrackingFilter::dynamic_callback_linear, this, _1, _2)); + ROS_INFO("\n\n\nCtor called for lin tracking \n\n\n"); } LinearTrackingFilter(const LinearTrackingFilter& t) = default; @@ -80,6 +86,18 @@ class LinearTrackingFilter { config.stf_rate_min = rate_min_; } + inline void dynamic_callback_linear(rr_common::LinearTrackingConfig& config, uint32_t level) { + static bool firstLoop = true; + if (firstLoop) { + this->GetDynParamDefaultsSpeed(config); + firstLoop = false; + + } else { + this->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); + } + ROS_INFO("Dyn Reconf Updated"); + } + inline void Update(double t) { if (last_update_ > 0) { double dt = t - last_update_; diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index e9f1defc6..9422903ad 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include @@ -196,19 +195,6 @@ void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t lev ROS_INFO("Dyn Reconf Updated"); } -void dynamic_callback_linear(rr_common::LinearTrackingConfig& config, uint32_t level) { - static bool firstLoop = true; - if (firstLoop) { - g_speed_model->GetDynParamDefaultsSpeed(config); - g_steer_model->GetDynParamDefaultsTurn(config); - firstLoop = false; - - } else { - g_speed_model->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); - g_steer_model->SetDynParam(config.stf_val_max, config.stf_val_min, config.stf_rate_max, config.stf_rate_min); - } - ROS_INFO("Dyn Reconf Updated"); -} int main(int argc, char** argv) { ros::init(argc, argv, "planner"); @@ -269,15 +255,12 @@ int main(int argc, char** argv) { viz_pub = nh.advertise("plan/path", 1); // init dynamic reconfigure - dynamic_reconfigure::Server DynReconfigServerPlanner; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&dynamic_callback_planner, _1, _2); - DynReconfigServerPlanner.setCallback(f); - - dynamic_reconfigure::Server DynReconfigServerLinear; - dynamic_reconfigure::Server::CallbackType g; - g = boost::bind(&dynamic_callback_linear, _1, _2); - DynReconfigServerLinear.setCallback(g); + // dynamic_reconfigure::Server DynReconfigServerPlanner; + // dynamic_reconfigure::Server::CallbackType f; + // f = boost::bind(&dynamic_callback_planner, _1, _2); + // DynReconfigServerPlanner.setCallback(f); + ROS_INFO("\n\n\ndyn reconf for planner (but empty??) \n\n\n"); + speed_message.reset(new rr_msgs::speed); steer_message.reset(new rr_msgs::steering); From dffb7df9fe7aae0563fdea565764efdc66350f51 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 28 Mar 2021 16:33:10 -0400 Subject: [PATCH 20/24] print debugging --- rr_common/include/rr_common/linear_tracking_filter.hpp | 2 +- rr_common/src/planner/planner_node.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index 10221359f..42c44ce35 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -95,7 +95,7 @@ class LinearTrackingFilter { } else { this->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); } - ROS_INFO("Dyn Reconf Updated"); + ROS_INFO("Dyn Reconf Linear inside of callback Updated"); } inline void Update(double t) { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 9422903ad..7faf615f8 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -192,7 +192,7 @@ void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t lev collision_penalty_ = config.collision_penalty; steering_gain = config.steering_gain; } - ROS_INFO("Dyn Reconf Updated"); + ROS_INFO("Dyn Reconf inside regular planner Updated"); } @@ -255,10 +255,10 @@ int main(int argc, char** argv) { viz_pub = nh.advertise("plan/path", 1); // init dynamic reconfigure - // dynamic_reconfigure::Server DynReconfigServerPlanner; - // dynamic_reconfigure::Server::CallbackType f; - // f = boost::bind(&dynamic_callback_planner, _1, _2); - // DynReconfigServerPlanner.setCallback(f); + dynamic_reconfigure::Server DynReconfigServerPlanner; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&dynamic_callback_planner, _1, _2); + DynReconfigServerPlanner.setCallback(f); ROS_INFO("\n\n\ndyn reconf for planner (but empty??) \n\n\n"); From 30cecc3b8a11afa01c34b687269998642f6ac9b8 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sun, 28 Mar 2021 16:45:54 -0400 Subject: [PATCH 21/24] formatting --- rr_common/include/rr_common/linear_tracking_filter.hpp | 2 +- rr_common/src/planner/planner_node.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index 42c44ce35..f7e102178 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -1,9 +1,9 @@ #pragma once +#include #include #include #include -#include namespace rr { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 38cf90f58..7c2a0c803 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -1,7 +1,7 @@ #include +#include #include #include -#include #include #include #include @@ -231,7 +231,6 @@ void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t lev ROS_INFO("Dyn Reconf inside regular planner Updated"); } - int main(int argc, char** argv) { ros::init(argc, argv, "planner"); @@ -302,7 +301,6 @@ int main(int argc, char** argv) { DynReconfigServerPlanner.setCallback(f); ROS_INFO("\n\n\ndyn reconf for planner (but empty??) \n\n\n"); - speed_message.reset(new rr_msgs::speed); steer_message.reset(new rr_msgs::steering); update_messages(0, 0); From e7f8a0d9a135665c444d4344c0f1bbb6249cd22a Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Mon, 29 Mar 2021 19:12:09 -0400 Subject: [PATCH 22/24] correctly adds dyn reconf instance for linear --- rr_common/cfg/lin-tracking-reconf.cfg | 15 +++----- rr_common/cfg/planner_configuration.cfg | 1 + .../rr_common/linear_tracking_filter.hpp | 35 ++++++------------- rr_common/src/planner/planner_node.cpp | 6 ++-- 4 files changed, 19 insertions(+), 38 deletions(-) diff --git a/rr_common/cfg/lin-tracking-reconf.cfg b/rr_common/cfg/lin-tracking-reconf.cfg index 961123c22..56775c260 100644 --- a/rr_common/cfg/lin-tracking-reconf.cfg +++ b/rr_common/cfg/lin-tracking-reconf.cfg @@ -5,16 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -# Linear Tracking Filter - Speed (TEMP NOW) -gen.add("spf_val_max", double_t, 0, "speed model: maximum value", 13.5, 0.0, 26.0) -gen.add("spf_val_min", double_t, 0, "speed model: minimum value", -1.0, -2.0, 0.0) -gen.add("spf_rate_max", double_t, 0, "speed model: maximum rate", 20.0, 0.0, 40.0) -gen.add("spf_rate_min", double_t, 0, "speed model: minimum rate", -25.0, -50.0, 0.0) +# Linear Tracking Filter Varibles (arbitrary max and min values) +gen.add("val_max", double_t, 0, "linear model: maximum value", 0, 0.0, 30.0) +gen.add("val_min", double_t, 0, "linear model: minimum value", 0, -5.0, 0.0) +gen.add("rate_max", double_t, 0, "linear model: maximum rate", 0, 0.0, 50.0) +gen.add("rate_min", double_t, 0, "linear model: minimum rate", 0, -50.0, 0.0) -# Linear Tracking Filter - Steering (TEMP NOW) -gen.add("stf_val_max", double_t, 0, "steering model: maximum value", 0.25, 0.00, 0.50) -gen.add("stf_val_min", double_t, 0, "steering model: minimum value", -0.25, -0.50, 0.0) -gen.add("stf_rate_max", double_t, 0, "steering model: maximum rate", 2.0, 0.0, 4.0) -gen.add("stf_rate_min", double_t, 0, "steering model: minimum rate", -2.0, -4.0, 0.0) exit(gen.generate(PACKAGE, "rr_common", "LinearTracking")) \ No newline at end of file diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index d9a19e3ac..b7393fb41 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -17,6 +17,7 @@ gen.add("dt", double_t, 0, "bicycle model: dt", 0.02, 0.001, 2.0) # Control Constants gen.add("steering_gain", double_t, 0, "steering gain", 1.4, 0.00, 3.0) gen.add("k_map_cost", double_t, 0, "cost: map constant", 0.1, 0.00, 1.0) +gen.add("k_global_path_cost", double_t, 0, "cost: global map cost", 0.0, -10.0, 100) gen.add("k_speed", double_t, 0, "cost: speed constant", 0.05, 0.00, 1.0) gen.add("k_steering", double_t, 0, "cost: steering constant", 0.01, 0.00, 1.0) gen.add("k_angle", double_t, 0, "cost: angle constant", 0.00, 0.00, 1.0) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index f7e102178..5f1c8e19f 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -16,6 +16,7 @@ class LinearTrackingFilter { double rate_min_; double rate_max_; double last_update_; + std::shared_ptr> dsrv_; public: explicit LinearTrackingFilter(const ros::NodeHandle& nh) { @@ -27,10 +28,8 @@ class LinearTrackingFilter { assertions::getParam(nh, "rate_max", rate_max_, { assertions::greater(0) }); last_update_ = 0; - std::unique_ptr> dsrv_; - dsrv_ = std::make_unique>(nh); + dsrv_ = std::make_shared>(nh); dsrv_->setCallback(boost::bind(&LinearTrackingFilter::dynamic_callback_linear, this, _1, _2)); - ROS_INFO("\n\n\nCtor called for lin tracking \n\n\n"); } LinearTrackingFilter(const LinearTrackingFilter& t) = default; @@ -58,13 +57,6 @@ class LinearTrackingFilter { target_ = x; } - /** - * Setter for the dynamic reconfigure variables in the linear tracking filter. - * @param val_max input max val - * @param val_min input min val - * @param rate_max input max rate - * @param rate_min input min rate - */ inline void SetDynParam(double val_max, double val_min, double rate_max, double rate_min) { val_max_ = val_max; val_min_ = val_min; @@ -72,30 +64,23 @@ class LinearTrackingFilter { rate_min_ = rate_min; } - inline void GetDynParamDefaultsSpeed(rr_common::LinearTrackingConfig& config) { - config.spf_val_max = val_max_; - config.spf_val_min = val_min_; - config.spf_rate_max = rate_max_; - config.spf_rate_min = rate_min_; - } - - inline void GetDynParamDefaultsTurn(rr_common::LinearTrackingConfig& config) { - config.stf_val_max = val_max_; - config.stf_val_min = val_min_; - config.stf_rate_max = rate_max_; - config.stf_rate_min = rate_min_; + inline void GetDynParamDefaults(rr_common::LinearTrackingConfig& config) { + config.val_max = val_max_; + config.val_min = val_min_; + config.rate_max = rate_max_; + config.rate_min = rate_min_; } inline void dynamic_callback_linear(rr_common::LinearTrackingConfig& config, uint32_t level) { static bool firstLoop = true; if (firstLoop) { - this->GetDynParamDefaultsSpeed(config); + this->GetDynParamDefaults(config); firstLoop = false; } else { - this->SetDynParam(config.spf_val_max, config.spf_val_min, config.spf_rate_max, config.spf_rate_min); + this->SetDynParam(config.val_max, config.val_min, config.rate_max, config.rate_min); } - ROS_INFO("Dyn Reconf Linear inside of callback Updated"); + ROS_INFO("Dyn Reconf Linear Updated"); } inline void Update(double t) { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index 7c2a0c803..a127f0b22 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -208,11 +208,13 @@ void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t lev config.n_segments = n_control_points_; config.k_map_cost = k_map_cost_; + config.k_global_path_cost = k_global_path_cost_; config.k_speed = k_speed_; config.k_steering = k_steering_; config.k_angle = k_angle_; config.collision_penalty = collision_penalty_; config.steering_gain = steering_gain; + firstLoop = false; } else { @@ -222,13 +224,13 @@ void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t lev g_last_controls.setZero(); k_map_cost_ = config.k_map_cost; + k_global_path_cost_ = config.k_global_path_cost; k_speed_ = config.k_speed; k_steering_ = config.k_steering; k_angle_ = config.k_angle; collision_penalty_ = config.collision_penalty; steering_gain = config.steering_gain; } - ROS_INFO("Dyn Reconf inside regular planner Updated"); } int main(int argc, char** argv) { @@ -294,12 +296,10 @@ int main(int argc, char** argv) { steer_pub = nh.advertise("plan/steering", 1); viz_pub = nh.advertise("plan/path", 1); - // init dynamic reconfigure dynamic_reconfigure::Server DynReconfigServerPlanner; dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&dynamic_callback_planner, _1, _2); DynReconfigServerPlanner.setCallback(f); - ROS_INFO("\n\n\ndyn reconf for planner (but empty??) \n\n\n"); speed_message.reset(new rr_msgs::speed); steer_message.reset(new rr_msgs::steering); From 9df03d1a78e660a852f27040b1d99978351e1028 Mon Sep 17 00:00:00 2001 From: Udit Subramanya Date: Sat, 3 Apr 2021 15:47:14 -0400 Subject: [PATCH 23/24] clean up --- rr_common/include/rr_common/linear_tracking_filter.hpp | 2 -- rr_common/src/planner/planner_node.cpp | 3 +-- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/rr_common/include/rr_common/linear_tracking_filter.hpp b/rr_common/include/rr_common/linear_tracking_filter.hpp index 5f1c8e19f..3eda231e1 100644 --- a/rr_common/include/rr_common/linear_tracking_filter.hpp +++ b/rr_common/include/rr_common/linear_tracking_filter.hpp @@ -76,11 +76,9 @@ class LinearTrackingFilter { if (firstLoop) { this->GetDynParamDefaults(config); firstLoop = false; - } else { this->SetDynParam(config.val_max, config.val_min, config.rate_max, config.rate_min); } - ROS_INFO("Dyn Reconf Linear Updated"); } inline void Update(double t) { diff --git a/rr_common/src/planner/planner_node.cpp b/rr_common/src/planner/planner_node.cpp index a127f0b22..614e7eef2 100644 --- a/rr_common/src/planner/planner_node.cpp +++ b/rr_common/src/planner/planner_node.cpp @@ -58,7 +58,7 @@ double viz_path_scale; double total_planning_time; size_t total_plans; -int n_control_points_ = 0; +int n_control_points_; void update_messages(double speed, double angle) { auto now = ros::Time::now(); @@ -216,7 +216,6 @@ void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t lev config.steering_gain = steering_gain; firstLoop = false; - } else { g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt); From 22a81315b32e5fe19beadb29abf1a8381517c0d7 Mon Sep 17 00:00:00 2001 From: Nicolas Bartholomai <32279539+NicoBartholomai@users.noreply.github.com> Date: Sun, 7 Nov 2021 16:53:47 -0500 Subject: [PATCH 24/24] Apply suggestions from code review --- rr_common/cfg/lin-tracking-reconf.cfg | 2 +- rr_common/cfg/planner_configuration.cfg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rr_common/cfg/lin-tracking-reconf.cfg b/rr_common/cfg/lin-tracking-reconf.cfg index 56775c260..8ef8a4934 100644 --- a/rr_common/cfg/lin-tracking-reconf.cfg +++ b/rr_common/cfg/lin-tracking-reconf.cfg @@ -12,4 +12,4 @@ gen.add("rate_max", double_t, 0, "linear model: maximum rate", 0, 0.0, 50.0) gen.add("rate_min", double_t, 0, "linear model: minimum rate", 0, -50.0, 0.0) -exit(gen.generate(PACKAGE, "rr_common", "LinearTracking")) \ No newline at end of file +exit(gen.generate(PACKAGE, "rr_common", "lin-tracking-reconf")) \ No newline at end of file diff --git a/rr_common/cfg/planner_configuration.cfg b/rr_common/cfg/planner_configuration.cfg index b7393fb41..614416769 100644 --- a/rr_common/cfg/planner_configuration.cfg +++ b/rr_common/cfg/planner_configuration.cfg @@ -23,4 +23,4 @@ gen.add("k_steering", double_t, 0, "cost: steering constant", 0.01, 0.00, 1.0) gen.add("k_angle", double_t, 0, "cost: angle constant", 0.00, 0.00, 1.0) gen.add("collision_penalty", double_t, 0, "cost: collision penalty", 10000, 100, 20000) -exit(gen.generate(PACKAGE, "rr_common", "PathPlanner")) \ No newline at end of file +exit(gen.generate(PACKAGE, "rr_common", "planner_configuration")) \ No newline at end of file