[following] add lethal cost threshold config (#200)

* Add lethal cost threshold config * Format
RoboJackets · Jun 1, 2024 · e977673 · e977673
1 parent 42bbe04
commit e977673
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 1 deletion.
diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml
@@ -2,6 +2,7 @@ follower_action_server:
   ros__parameters:
     lookahead_distance: 0.9
     desired_linear_velocity: 0.5
+    lethal_cost_threshold: 10
     cmd_vel_stamped: true
     cmd_vel_topic: "/rover_drivetrain_controller/cmd_vel"
     odom_topic: "/rover_drivetrain_controller/odom"

diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp
@@ -19,6 +19,7 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options)
   declare_parameter("map_frame", "map");
   declare_parameter("goal_tolerance", 0.1);
   declare_parameter("cmd_vel_stamped", false);
+  declare_parameter("lethal_cost_threshold", 50);
 
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@@ -219,7 +220,7 @@ void FollowerActionServer::execute(
       break;
     } else if (getCost(
         current_costmap_, output.lookahead_point.point.x,
-        output.lookahead_point.point.y) > 0)
+        output.lookahead_point.point.y) > get_parameter("lethal_cost_threshold").as_int())
     {
       result->error_code = urc_msgs::action::FollowPath::Result::OBSTACLE_DETECTED;
       goal_handle->abort(result);