Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
controller.cpp
1 // Copyright (c) 2024 Open Navigation LLC
2 // Copyright (c) 2024 Alberto J. Tudela Roldán
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #include <memory>
17 
18 #include "rclcpp/logging.hpp"
19 #include "rclcpp/parameter.hpp"
20 #include "opennav_docking/controller.hpp"
21 #include "nav2_util/geometry_utils.hpp"
22 #include "nav2_ros_common/node_utils.hpp"
23 #include "tf2/utils.hpp"
24 #include "nav2_ros_common/tf2_factories.hpp"
25 
26 using rcl_interfaces::msg::ParameterType;
27 
28 namespace opennav_docking
29 {
30 
32  const nav2::LifecycleNode::SharedPtr & node, nav2::TransformBuffer::SharedPtr tf,
33  std::string base_frame)
34 : tf2_buffer_(tf), base_frame_(base_frame)
35 {
36  logger_ = node->get_logger();
37 
38  std::string costmap_topic, footprint_topic;
39  k_phi_ = node->declare_or_get_parameter("controller.k_phi", 3.0);
40  k_delta_ = node->declare_or_get_parameter("controller.k_delta", 2.0);
41  beta_ = node->declare_or_get_parameter("controller.beta", 0.4);
42  lambda_ = node->declare_or_get_parameter("controller.lambda", 2.0);
43  v_linear_min_ = node->declare_or_get_parameter("controller.v_linear_min", 0.1);
44  v_linear_max_ = node->declare_or_get_parameter("controller.v_linear_max", 0.25);
45  v_angular_max_ = node->declare_or_get_parameter("controller.v_angular_max", 0.75);
46  slowdown_radius_ = node->declare_or_get_parameter("controller.slowdown_radius", 0.25);
47  deceleration_max_ = node->declare_or_get_parameter("controller.deceleration_max", 2.5);
48  rotate_to_heading_angular_vel_ = node->declare_or_get_parameter(
49  "controller.rotate_to_heading_angular_vel", 1.0);
50  rotate_to_heading_max_angular_accel_ = node->declare_or_get_parameter(
51  "controller.rotate_to_heading_max_angular_accel", 3.2);
52  use_collision_detection_ = node->declare_or_get_parameter(
53  "controller.use_collision_detection", true);
54  costmap_topic = node->declare_or_get_parameter("controller.costmap_topic",
55  std::string("local_costmap/costmap_raw"));
56  footprint_topic = node->declare_or_get_parameter("controller.footprint_topic",
57  std::string("local_costmap/published_footprint"));
58  transform_tolerance_ = node->declare_or_get_parameter(
59  "controller.transform_tolerance", 0.1);
60  projection_time_ = node->declare_or_get_parameter(
61  "controller.projection_time", 5.0);
62  simulation_time_step_ = node->declare_or_get_parameter(
63  "controller.simulation_time_step", 0.1);
64  dock_collision_threshold_ = node->declare_or_get_parameter(
65  "controller.dock_collision_threshold", 0.3);
66 
67  control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
68  k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, deceleration_max_,
69  v_linear_min_, v_linear_max_, v_angular_max_);
70 
71  // Add callback for dynamic parameters
72  post_set_params_handler_ = node->add_post_set_parameters_callback(
73  std::bind(
75  this, std::placeholders::_1));
76  on_set_params_handler_ = node->add_on_set_parameters_callback(
77  std::bind(
79  this, std::placeholders::_1));
80 
81  if (use_collision_detection_) {
82  configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_);
83  }
84 
85  trajectory_pub_ =
86  node->create_publisher<nav_msgs::msg::Path>("docking_trajectory");
87 }
88 
90 {
91  control_law_.reset();
92  trajectory_pub_.reset();
93  collision_checker_.reset();
94  costmap_sub_.reset();
95  footprint_sub_.reset();
96 }
97 
99  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd,
100  const geometry_msgs::msg::TransformStamped & base_to_fixed_transform,
101  bool is_docking, bool backward)
102 {
103  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
104  cmd = control_law_->calculateRegularVelocity(pose, backward);
105  return isTrajectoryCollisionFree(pose, base_to_fixed_transform, is_docking, backward);
106 }
107 
109  const double & angular_distance_to_heading,
110  const geometry_msgs::msg::Twist & current_velocity,
111  const double & dt)
112 {
113  geometry_msgs::msg::Twist cmd_vel;
114  const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
115  const double angular_vel = sign * rotate_to_heading_angular_vel_;
116  const double min_feasible_angular_speed =
117  current_velocity.angular.z - rotate_to_heading_max_angular_accel_ * dt;
118  const double max_feasible_angular_speed =
119  current_velocity.angular.z + rotate_to_heading_max_angular_accel_ * dt;
120  cmd_vel.angular.z =
121  std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
122 
123  // Check if we need to slow down to avoid overshooting
124  double max_vel_to_stop =
125  std::sqrt(2 * rotate_to_heading_max_angular_accel_ * fabs(angular_distance_to_heading));
126  if (fabs(cmd_vel.angular.z) > max_vel_to_stop) {
127  cmd_vel.angular.z = sign * max_vel_to_stop;
128  }
129 
130  return cmd_vel;
131 }
132 
134  const geometry_msgs::msg::Pose & target_pose,
135  const geometry_msgs::msg::TransformStamped & base_to_fixed_transform,
136  bool is_docking, bool backward)
137 {
138  // Visualization of the trajectory
139  auto trajectory = std::make_unique<nav_msgs::msg::Path>();
140  trajectory->header.frame_id = base_frame_;
141  trajectory->header.stamp = base_to_fixed_transform.header.stamp;
142 
143  // First pose
144  geometry_msgs::msg::PoseStamped next_pose;
145  next_pose.header.frame_id = base_frame_;
146  trajectory->poses.push_back(next_pose);
147 
148  // Generate path
149  double distance = std::numeric_limits<double>::max();
150  unsigned int max_iter = static_cast<unsigned int>(ceil(projection_time_ / simulation_time_step_));
151 
152  do{
153  // Apply velocities to calculate next pose
154  next_pose.pose = control_law_->calculateNextPose(
155  simulation_time_step_, target_pose, next_pose.pose, backward);
156 
157  // Add the pose to the trajectory for visualization
158  trajectory->poses.push_back(next_pose);
159 
160  // Transform pose from base_frame into fixed_frame
161  geometry_msgs::msg::PoseStamped local_pose = next_pose;
162  local_pose.header.stamp = trajectory->header.stamp;
163  tf2::doTransform(local_pose, local_pose, base_to_fixed_transform);
164 
165  // Determine the distance at which to check for collisions
166  // Skip the final segment of the trajectory for docking
167  // and the initial segment for undocking
168  // This avoids false positives when the robot is at the dock
169  double dock_collision_distance = is_docking ?
170  nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) :
171  std::hypot(next_pose.pose.position.x, next_pose.pose.position.y);
172 
173  // If this distance is greater than the dock_collision_threshold, check for collisions
174  if (use_collision_detection_ &&
175  dock_collision_distance > dock_collision_threshold_ &&
176  !collision_checker_->isCollisionFree(local_pose.pose))
177  {
178  RCLCPP_WARN(
179  logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s",
180  local_pose.pose.position.x, local_pose.pose.position.y, local_pose.pose.position.z,
181  local_pose.header.frame_id.c_str());
182  trajectory_pub_->publish(std::move(trajectory));
183  return false;
184  }
185 
186  // Check if we reach the goal
187  distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
188  }while(distance > 1e-2 && trajectory->poses.size() < max_iter);
189 
190  trajectory_pub_->publish(std::move(trajectory));
191 
192  return true;
193 }
194 
196  const nav2::LifecycleNode::SharedPtr & node,
197  std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
198 {
199  costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(node, costmap_topic);
200  footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
201  node, footprint_topic, *tf2_buffer_, base_frame_, transform_tolerance);
202  collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
203  *costmap_sub_, *footprint_sub_, node->get_name());
204 }
205 
206 rcl_interfaces::msg::SetParametersResult Controller::validateParameterUpdatesCallback(
207  const std::vector<rclcpp::Parameter> & parameters)
208 {
209  rcl_interfaces::msg::SetParametersResult result;
210  result.successful = true;
211  for (const auto & parameter : parameters) {
212  const auto & param_type = parameter.get_type();
213  const auto & param_name = parameter.get_name();
214  if (param_name.find("controller.") != 0) {
215  continue;
216  }
217  if (param_type == ParameterType::PARAMETER_DOUBLE) {
218  if (parameter.as_double() < 0.0) {
219  RCLCPP_WARN(
220  logger_, "The value of parameter '%s' is incorrectly set to %f, "
221  "it should be >=0. Ignoring parameter update.",
222  param_name.c_str(), parameter.as_double());
223  result.successful = false;
224  }
225  }
226  }
227  return result;
228 }
229 
230 void
231 Controller::updateParametersCallback(const std::vector<rclcpp::Parameter> & parameters)
232 {
233  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
234 
235  for (auto parameter : parameters) {
236  const auto & param_type = parameter.get_type();
237  const auto & param_name = parameter.get_name();
238  if (param_name.find("controller.") != 0) {
239  continue;
240  }
241  if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
242  if (param_name == "controller.k_phi") {
243  k_phi_ = parameter.as_double();
244  } else if (param_name == "controller.k_delta") {
245  k_delta_ = parameter.as_double();
246  } else if (param_name == "controller.beta") {
247  beta_ = parameter.as_double();
248  } else if (param_name == "controller.lambda") {
249  lambda_ = parameter.as_double();
250  } else if (param_name == "controller.v_linear_min") {
251  v_linear_min_ = parameter.as_double();
252  } else if (param_name == "controller.v_linear_max") {
253  v_linear_max_ = parameter.as_double();
254  } else if (param_name == "controller.v_angular_max") {
255  v_angular_max_ = parameter.as_double();
256  } else if (param_name == "controller.slowdown_radius") {
257  slowdown_radius_ = parameter.as_double();
258  } else if (param_name == "controller.deceleration_max") {
259  deceleration_max_ = parameter.as_double();
260  } else if (param_name == "controller.rotate_to_heading_angular_vel") {
261  rotate_to_heading_angular_vel_ = parameter.as_double();
262  } else if (param_name == "controller.rotate_to_heading_max_angular_accel") {
263  rotate_to_heading_max_angular_accel_ = parameter.as_double();
264  } else if (param_name == "controller.projection_time") {
265  projection_time_ = parameter.as_double();
266  } else if (param_name == "controller.simulation_time_step") {
267  simulation_time_step_ = parameter.as_double();
268  } else if (param_name == "controller.dock_collision_threshold") {
269  dock_collision_threshold_ = parameter.as_double();
270  }
271 
272  // Update the smooth control law with the new params
273  control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
274  control_law_->setSlowdownRadius(slowdown_radius_);
275  control_law_->setMaxDeceleration(deceleration_max_);
276  control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
277  }
278  }
279 }
280 
281 } // namespace opennav_docking
void updateParametersCallback(const std::vector< rclcpp::Parameter > &parameters)
Apply parameter updates after validation This callback is executed when parameters have been successf...
Definition: controller.cpp:231
bool computeVelocityCommand(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Twist &cmd, const geometry_msgs::msg::TransformStamped &base_to_fixed_transform, bool is_docking, bool backward=false)
Compute a velocity command using control law.
Definition: controller.cpp:98
~Controller()
A destructor for opennav_docking::Controller.
Definition: controller.cpp:89
geometry_msgs::msg::Twist computeRotateToHeadingCommand(const double &angular_distance_to_heading, const geometry_msgs::msg::Twist &current_velocity, const double &dt)
Perform a command for in-place rotation.
Definition: controller.cpp:108
bool isTrajectoryCollisionFree(const geometry_msgs::msg::Pose &target_pose, const geometry_msgs::msg::TransformStamped &base_to_fixed_transform, bool is_docking, bool backward=false)
Check if a trajectory is collision free.
Definition: controller.cpp:133
Controller(const nav2::LifecycleNode::SharedPtr &node, nav2::TransformBuffer::SharedPtr tf, std::string base_frame)
Create a controller instance. Configure ROS 2 parameters.
Definition: controller.cpp:31
void configureCollisionChecker(const nav2::LifecycleNode::SharedPtr &node, std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
Configure the collision checker.
Definition: controller.cpp:195
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > &parameters)
Validate incoming parameter updates before applying them. This callback is triggered when one or more...
Definition: controller.cpp:206