Recover previous changes

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

Author: SakshayMahna

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -34,6 +34,7 @@ struct Parameters
   double transform_tolerance;
   double min_lookahead;
   double max_lookahead;
+  bool interpolate_after_goal;
   double max_robot_pose_search_dist;
   double k_phi;
   double k_delta;

nav2_graceful_controller/src/parameter_handler.cpp

@@ -41,6 +41,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".interpolate_after_goal", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_robot_pose_search_dist",
     rclcpp::ParameterValue(costmap_size_x / 2.0));
@@ -76,6 +78,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
   node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
   node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead);
+  node->get_parameter(plugin_name_ + ".interpolate_after_goal", params_.interpolate_after_goal);
   node->get_parameter(
     plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist);
   if (params_.max_robot_pose_search_dist < 0.0) {
@@ -186,6 +189,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.initial_rotation = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".prefer_final_rotation") {
         params_.prefer_final_rotation = parameter.as_bool();
+      } else if (param_name == plugin_name_ + ".interpolate_after_goal") {
+        params_.interpolate_after_goal = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".allow_backward") {
         if (params_.initial_rotation && parameter.as_bool()) {
           RCLCPP_WARN(

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -270,6 +270,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
     {rclcpp::Parameter("test.transform_tolerance", 1.0),
       rclcpp::Parameter("test.min_lookahead", 1.0),
       rclcpp::Parameter("test.max_lookahead", 2.0),
+      rclcpp::Parameter("test.interpolate_after_goal", false),
       rclcpp::Parameter("test.k_phi", 4.0),
       rclcpp::Parameter("test.k_delta", 5.0),
       rclcpp::Parameter("test.beta", 6.0),
@@ -294,6 +295,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 1.0);
   EXPECT_EQ(node->get_parameter("test.min_lookahead").as_double(), 1.0);
   EXPECT_EQ(node->get_parameter("test.max_lookahead").as_double(), 2.0);
+  EXPECT_EQ(node->get_parameter("test.interpolate_after_goal").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.k_phi").as_double(), 4.0);
   EXPECT_EQ(node->get_parameter("test.k_delta").as_double(), 5.0);
   EXPECT_EQ(node->get_parameter("test.beta").as_double(), 6.0);