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"
26 using rcl_interfaces::msg::ParameterType;
28 namespace opennav_docking
32 const nav2::LifecycleNode::SharedPtr & node, nav2::TransformBuffer::SharedPtr tf,
33 std::string base_frame)
34 : tf2_buffer_(tf), base_frame_(base_frame)
36 logger_ = node->get_logger();
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);
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_);
72 post_set_params_handler_ = node->add_post_set_parameters_callback(
75 this, std::placeholders::_1));
76 on_set_params_handler_ = node->add_on_set_parameters_callback(
79 this, std::placeholders::_1));
81 if (use_collision_detection_) {
86 node->create_publisher<nav_msgs::msg::Path>(
"docking_trajectory");
92 trajectory_pub_.reset();
93 collision_checker_.reset();
95 footprint_sub_.reset();
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)
103 std::lock_guard<std::mutex> lock(dynamic_params_lock_);
104 cmd = control_law_->calculateRegularVelocity(pose, backward);
109 const double & angular_distance_to_heading,
110 const geometry_msgs::msg::Twist & current_velocity,
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;
121 std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
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;
134 const geometry_msgs::msg::Pose & target_pose,
135 const geometry_msgs::msg::TransformStamped & base_to_fixed_transform,
136 bool is_docking,
bool backward)
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;
144 geometry_msgs::msg::PoseStamped next_pose;
145 next_pose.header.frame_id = base_frame_;
146 trajectory->poses.push_back(next_pose);
149 double distance = std::numeric_limits<double>::max();
150 unsigned int max_iter =
static_cast<unsigned int>(ceil(projection_time_ / simulation_time_step_));
154 next_pose.pose = control_law_->calculateNextPose(
155 simulation_time_step_, target_pose, next_pose.pose, backward);
158 trajectory->poses.push_back(next_pose);
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);
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);
174 if (use_collision_detection_ &&
175 dock_collision_distance > dock_collision_threshold_ &&
176 !collision_checker_->isCollisionFree(local_pose.pose))
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));
187 distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
188 }
while(distance > 1e-2 && trajectory->poses.size() < max_iter);
190 trajectory_pub_->publish(std::move(trajectory));
196 const nav2::LifecycleNode::SharedPtr & node,
197 std::string costmap_topic, std::string footprint_topic,
double transform_tolerance)
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());
207 const std::vector<rclcpp::Parameter> & parameters)
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) {
217 if (param_type == ParameterType::PARAMETER_DOUBLE) {
218 if (parameter.as_double() < 0.0) {
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;
233 std::lock_guard<std::mutex> lock(dynamic_params_lock_);
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) {
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();
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_);
void updateParametersCallback(const std::vector< rclcpp::Parameter > ¶meters)
Apply parameter updates after validation This callback is executed when parameters have been successf...
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.
~Controller()
A destructor for opennav_docking::Controller.
geometry_msgs::msg::Twist computeRotateToHeadingCommand(const double &angular_distance_to_heading, const geometry_msgs::msg::Twist ¤t_velocity, const double &dt)
Perform a command for in-place rotation.
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.
Controller(const nav2::LifecycleNode::SharedPtr &node, nav2::TransformBuffer::SharedPtr tf, std::string base_frame)
Create a controller instance. Configure ROS 2 parameters.
void configureCollisionChecker(const nav2::LifecycleNode::SharedPtr &node, std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
Configure the collision checker.
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > ¶meters)
Validate incoming parameter updates before applying them. This callback is triggered when one or more...