Feat/267 planner dyn reconfig

rr_common/CMakeLists.txt

@@ -16,10 +16,20 @@ 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
+        cfg/lin-tracking-reconf.cfg
+)
+
 ###################################
 ## catkin specific configuration ##
 ###################################

rr_common/cfg/lin-tracking-reconf.cfg

@@ -0,0 +1,15 @@
+#!/usr/bin/env python
+PACKAGE = "rr_common"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# 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)
+
+
+exit(gen.generate(PACKAGE, "rr_common", "lin-tracking-reconf"))

rr_common/cfg/planner_configuration.cfg

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+PACKAGE = "rr_common"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Planner Params
+# 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)
+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)
+
+# 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)
+gen.add("collision_penalty", double_t, 0, "cost: collision penalty", 10000, 100, 20000)
+
+exit(gen.generate(PACKAGE, "rr_common", "planner_configuration"))

rr_common/include/rr_common/linear_tracking_filter.hpp

@@ -1,7 +1,9 @@
 #pragma once
 
+#include <dynamic_reconfigure/server.h>
 #include <parameter_assertions/assertions.h>
 #include <ros/ros.h>
+#include <rr_common/LinearTrackingConfig.h>
 
 namespace rr {
 
@@ -14,6 +16,7 @@ class LinearTrackingFilter {
     double rate_min_;
     double rate_max_;
     double last_update_;
+    std::shared_ptr<dynamic_reconfigure::Server<rr_common::LinearTrackingConfig>> dsrv_;
 
   public:
     explicit LinearTrackingFilter(const ros::NodeHandle& nh) {
@@ -24,6 +27,9 @@ class LinearTrackingFilter {
         assertions::getParam(nh, "rate_min", rate_min_, { assertions::less<double>(0) });
         assertions::getParam(nh, "rate_max", rate_max_, { assertions::greater<double>(0) });
         last_update_ = 0;
+
+        dsrv_ = std::make_shared<dynamic_reconfigure::Server<rr_common::LinearTrackingConfig>>(nh);
+        dsrv_->setCallback(boost::bind(&LinearTrackingFilter::dynamic_callback_linear, this, _1, _2));
     }
 
     LinearTrackingFilter(const LinearTrackingFilter& t) = default;
@@ -51,6 +57,30 @@ class LinearTrackingFilter {
         target_ = x;
     }
 
+    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 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->GetDynParamDefaults(config);
+            firstLoop = false;
+        } else {
+            this->SetDynParam(config.val_max, config.val_min, config.rate_max, config.rate_min);
+        }
+    }
+
     inline void Update(double t) {
         if (last_update_ > 0) {
             double dt = t - last_update_;

rr_common/include/rr_common/planning/bicycle_model.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <rr_common/PathPlannerConfig.h>
+
 #include <rr_common/linear_tracking_filter.hpp>
 #include <tuple>
 
@@ -32,6 +34,15 @@ class BicycleModel {
 
     void RollOutPath(const Controls<2>& controls, TrajectoryRollout& rollout) const;
 
+    /**
+     * 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 SetDynParam(double max_lateral_accel, int segment_size, double dt);
+    void GetDynParamDefaults(rr_common::PathPlannerConfig& config);
+
   private:
     /**
      * Calculate a desired speed from a steering angle based on

rr_common/src/planner/bicycle_model.cpp

@@ -63,6 +63,17 @@ void BicycleModel::RollOutPath(const Controls<1>& controls, TrajectoryRollout& r
     rollout.apply_speed = speed_model_temp.GetValue();
 }
 
+void BicycleModel::SetDynParam(double max_lateral_accel, int segment_size, double dt) {
+    max_lateral_accel_ = max_lateral_accel;
+    segment_size_ = segment_size;
+    dt_ = 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::RollOutPath(const Controls<2>& controls, TrajectoryRollout& rollout) const {
     const size_t path_size = 1 + (segment_size_ * controls.cols());
     if (rollout.path.size() != path_size) {

rr_common/src/planner/planner_node.cpp

@@ -1,8 +1,12 @@
+#include <dynamic_reconfigure/server.h>
 #include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
 #include <parameter_assertions/assertions.h>
 #include <pcl/PCLPointCloud2.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <ros/ros.h>
+#include <rr_common/PathPlannerConfig.h>
 #include <rr_common/planning/annealing_optimizer.h>
 #include <rr_common/planning/bicycle_model.h>
 #include <rr_common/planning/distance_map.h>
@@ -54,6 +58,8 @@ double viz_path_scale;
 double total_planning_time;
 size_t total_plans;
 
+int n_control_points_;
+
 void update_messages(double speed, double angle) {
     auto now = ros::Time::now();
 
@@ -195,6 +201,37 @@ void generatePath() {
     }
 }
 
+void dynamic_callback_planner(rr_common::PathPlannerConfig& config, uint32_t level) {
+    static bool firstLoop = true;
+    if (firstLoop) {
+        g_vehicle_model->GetDynParamDefaults(config);
+
+        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 {
+        g_vehicle_model->SetDynParam(config.max_lateral_accel, config.segment_size, config.dt);
+
+        g_last_controls = rr::Controls<ctrl_dim>(ctrl_dim, config.n_segments);
+        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;
+    }
+}
+
 int main(int argc, char** argv) {
     ros::init(argc, argv, "planner");
 
@@ -238,9 +275,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>(ctrl_dim, n_control_points);
+    assertions::getParam(nhp, "n_segments", n_control_points_);
+    g_last_controls = rr::Controls<ctrl_dim>(ctrl_dim, n_control_points_);
     g_last_controls.setZero();
 
     caution_duration = ros::Duration(assertions::param(nhp, "impasse_caution_duration", 0.0));
@@ -259,6 +295,11 @@ int main(int argc, char** argv) {
     steer_pub = nh.advertise<rr_msgs::steering>("plan/steering", 1);
     viz_pub = nh.advertise<visualization_msgs::Marker>("plan/path", 1);
 
+    dynamic_reconfigure::Server<rr_common::PathPlannerConfig> DynReconfigServerPlanner;
+    dynamic_reconfigure::Server<rr_common::PathPlannerConfig>::CallbackType f;
+    f = boost::bind(&dynamic_callback_planner, _1, _2);
+    DynReconfigServerPlanner.setCallback(f);
+
     speed_message.reset(new rr_msgs::speed);
     steer_message.reset(new rr_msgs::steering);
     update_messages(0, 0);