Nav2 Navigation Stack - jazzy  jazzy
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/rclcpp.hpp"
19 #include "opennav_docking/controller.hpp"
20 #include "nav2_util/geometry_utils.hpp"
21 #include "nav2_util/node_utils.hpp"
22 #include "nav_2d_utils/conversions.hpp"
23 #include "tf2/utils.h"
24 
25 namespace opennav_docking
26 {
27 
28 Controller::Controller(
29  const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf,
30  std::string fixed_frame, std::string base_frame)
31 : tf2_buffer_(tf), fixed_frame_(fixed_frame), base_frame_(base_frame)
32 {
33  logger_ = node->get_logger();
34  clock_ = node->get_clock();
35 
36  nav2_util::declare_parameter_if_not_declared(
37  node, "controller.k_phi", rclcpp::ParameterValue(3.0));
38  nav2_util::declare_parameter_if_not_declared(
39  node, "controller.k_delta", rclcpp::ParameterValue(2.0));
40  nav2_util::declare_parameter_if_not_declared(
41  node, "controller.beta", rclcpp::ParameterValue(0.4));
42  nav2_util::declare_parameter_if_not_declared(
43  node, "controller.lambda", rclcpp::ParameterValue(2.0));
44  nav2_util::declare_parameter_if_not_declared(
45  node, "controller.v_linear_min", rclcpp::ParameterValue(0.1));
46  nav2_util::declare_parameter_if_not_declared(
47  node, "controller.v_linear_max", rclcpp::ParameterValue(0.25));
48  nav2_util::declare_parameter_if_not_declared(
49  node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
50  nav2_util::declare_parameter_if_not_declared(
51  node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
52  nav2_util::declare_parameter_if_not_declared(
53  node, "controller.use_collision_detection", rclcpp::ParameterValue(true));
54  nav2_util::declare_parameter_if_not_declared(
55  node, "controller.costmap_topic",
56  rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
57  nav2_util::declare_parameter_if_not_declared(
58  node, "controller.footprint_topic", rclcpp::ParameterValue(
59  std::string("local_costmap/published_footprint")));
60  nav2_util::declare_parameter_if_not_declared(
61  node, "controller.transform_tolerance", rclcpp::ParameterValue(0.1));
62  nav2_util::declare_parameter_if_not_declared(
63  node, "controller.projection_time", rclcpp::ParameterValue(5.0));
64  nav2_util::declare_parameter_if_not_declared(
65  node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1));
66  nav2_util::declare_parameter_if_not_declared(
67  node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3));
68 
69  node->get_parameter("controller.k_phi", k_phi_);
70  node->get_parameter("controller.k_delta", k_delta_);
71  node->get_parameter("controller.beta", beta_);
72  node->get_parameter("controller.lambda", lambda_);
73  node->get_parameter("controller.v_linear_min", v_linear_min_);
74  node->get_parameter("controller.v_linear_max", v_linear_max_);
75  node->get_parameter("controller.v_angular_max", v_angular_max_);
76  node->get_parameter("controller.slowdown_radius", slowdown_radius_);
77  control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
78  k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
79  v_angular_max_);
80 
81  // Add callback for dynamic parameters
82  dyn_params_handler_ = node->add_on_set_parameters_callback(
83  std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
84 
85  node->get_parameter("controller.use_collision_detection", use_collision_detection_);
86  node->get_parameter("controller.projection_time", projection_time_);
87  node->get_parameter("controller.simulation_time_step", simulation_time_step_);
88  node->get_parameter("controller.transform_tolerance", transform_tolerance_);
89 
90  if (use_collision_detection_) {
91  std::string costmap_topic, footprint_topic;
92  node->get_parameter("controller.costmap_topic", costmap_topic);
93  node->get_parameter("controller.footprint_topic", footprint_topic);
94  node->get_parameter("controller.dock_collision_threshold", dock_collision_threshold_);
95  configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_);
96  }
97 
98  trajectory_pub_ =
99  node->create_publisher<nav_msgs::msg::Path>("docking_trajectory", 1);
100 }
101 
102 Controller::~Controller()
103 {
104  control_law_.reset();
105  trajectory_pub_.reset();
106  collision_checker_.reset();
107  costmap_sub_.reset();
108  footprint_sub_.reset();
109 }
110 
111 bool Controller::computeVelocityCommand(
112  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking,
113  bool backward)
114 {
115  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
116  cmd = control_law_->calculateRegularVelocity(pose, backward);
117  return isTrajectoryCollisionFree(pose, is_docking, backward);
118 }
119 
120 bool Controller::isTrajectoryCollisionFree(
121  const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward)
122 {
123  // Visualization of the trajectory
124  nav_msgs::msg::Path trajectory;
125  trajectory.header.frame_id = base_frame_;
126  trajectory.header.stamp = clock_->now();
127 
128  // First pose
129  geometry_msgs::msg::PoseStamped next_pose;
130  next_pose.header.frame_id = base_frame_;
131  trajectory.poses.push_back(next_pose);
132 
133  // Get the transform from base_frame to fixed_frame
134  geometry_msgs::msg::TransformStamped base_to_fixed_transform;
135  try {
136  base_to_fixed_transform = tf2_buffer_->lookupTransform(
137  fixed_frame_, base_frame_, trajectory.header.stamp,
138  tf2::durationFromSec(transform_tolerance_));
139  } catch (tf2::TransformException & ex) {
140  RCLCPP_ERROR(
141  logger_, "Could not get transform from %s to %s: %s",
142  base_frame_.c_str(), fixed_frame_.c_str(), ex.what());
143  return false;
144  }
145 
146  // Generate path
147  double distance = std::numeric_limits<double>::max();
148  unsigned int max_iter = static_cast<unsigned int>(ceil(projection_time_ / simulation_time_step_));
149 
150  do{
151  // Apply velocities to calculate next pose
152  next_pose.pose = control_law_->calculateNextPose(
153  simulation_time_step_, target_pose, next_pose.pose, backward);
154 
155  // Add the pose to the trajectory for visualization
156  trajectory.poses.push_back(next_pose);
157 
158  // Transform pose from base_frame into fixed_frame
159  geometry_msgs::msg::PoseStamped local_pose = next_pose;
160  local_pose.header.stamp = trajectory.header.stamp;
161  tf2::doTransform(local_pose, local_pose, base_to_fixed_transform);
162 
163  // Determine the distance at which to check for collisions
164  // Skip the final segment of the trajectory for docking
165  // and the initial segment for undocking
166  // This avoids false positives when the robot is at the dock
167  double dock_collision_distance = is_docking ?
168  nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) :
169  std::hypot(next_pose.pose.position.x, next_pose.pose.position.y);
170 
171  // If this distance is greater than the dock_collision_threshold, check for collisions
172  if (use_collision_detection_ &&
173  dock_collision_distance > dock_collision_threshold_ &&
174  !collision_checker_->isCollisionFree(nav_2d_utils::poseToPose2D(local_pose.pose)))
175  {
176  RCLCPP_WARN(
177  logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s",
178  local_pose.pose.position.x, local_pose.pose.position.y, local_pose.pose.position.z,
179  local_pose.header.frame_id.c_str());
180  trajectory_pub_->publish(trajectory);
181  return false;
182  }
183 
184  // Check if we reach the goal
185  distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
186  }while(distance > 1e-2 && trajectory.poses.size() < max_iter);
187 
188  trajectory_pub_->publish(trajectory);
189 
190  return true;
191 }
192 
193 void Controller::configureCollisionChecker(
194  const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
195  std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
196 {
197  costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(node, costmap_topic);
198  footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
199  node, footprint_topic, *tf2_buffer_, base_frame_, transform_tolerance);
200  collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
201  *costmap_sub_, *footprint_sub_, node->get_name());
202 }
203 
204 rcl_interfaces::msg::SetParametersResult
205 Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
206 {
207  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
208 
209  rcl_interfaces::msg::SetParametersResult result;
210  for (auto parameter : parameters) {
211  const auto & type = parameter.get_type();
212  const auto & name = parameter.get_name();
213 
214  if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
215  if (name == "controller.k_phi") {
216  k_phi_ = parameter.as_double();
217  } else if (name == "controller.k_delta") {
218  k_delta_ = parameter.as_double();
219  } else if (name == "controller.beta") {
220  beta_ = parameter.as_double();
221  } else if (name == "controller.lambda") {
222  lambda_ = parameter.as_double();
223  } else if (name == "controller.v_linear_min") {
224  v_linear_min_ = parameter.as_double();
225  } else if (name == "controller.v_linear_max") {
226  v_linear_max_ = parameter.as_double();
227  } else if (name == "controller.v_angular_max") {
228  v_angular_max_ = parameter.as_double();
229  } else if (name == "controller.slowdown_radius") {
230  slowdown_radius_ = parameter.as_double();
231  } else if (name == "controller.projection_time") {
232  projection_time_ = parameter.as_double();
233  } else if (name == "controller.simulation_time_step") {
234  simulation_time_step_ = parameter.as_double();
235  } else if (name == "controller.dock_collision_threshold") {
236  dock_collision_threshold_ = parameter.as_double();
237  }
238 
239  // Update the smooth control law with the new params
240  control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
241  control_law_->setSlowdownRadius(slowdown_radius_);
242  control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
243  }
244  }
245 
246  result.successful = true;
247  return result;
248 }
249 
250 } // namespace opennav_docking
opennav_docking::Controller::isTrajectoryCollisionFree
bool isTrajectoryCollisionFree(const geometry_msgs::msg::Pose &target_pose, bool is_docking, bool backward=false)
Check if a trajectory is collision free.
Definition: controller.cpp:120
opennav_docking::Controller::computeVelocityCommand
bool computeVelocityCommand(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Twist &cmd, bool is_docking, bool backward=false)
Compute a velocity command using control law.
Definition: controller.cpp:111
opennav_docking::Controller::~Controller
~Controller()
A destructor for opennav_docking::Controller.
Definition: controller.cpp:102
opennav_docking::Controller::configureCollisionChecker
void configureCollisionChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
Configure the collision checker.
Definition: controller.cpp:193
opennav_docking::Controller::Controller
Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::shared_ptr< tf2_ros::Buffer > tf, std::string fixed_frame, std::string base_frame)
Create a controller instance. Configure ROS 2 parameters.
Definition: controller.cpp:28
opennav_docking::Controller::dynamicParametersCallback
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector< rclcpp::Parameter > parameters)
Callback executed when a parameter change is detected.
Definition: controller.cpp:205
Generated by doxygen 1.9.1