SMACC2
backward_local_planner.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
21#include <angles/angles.h>
24
25#include <boost/intrusive_ptr.hpp>
26#include <chrono>
27#include <nav_2d_utils/tf_help.hpp>
28#include <pluginlib/class_list_macros.hpp>
29#include <visualization_msgs/msg/marker_array.hpp>
30
31// register this planner as a BaseLocalPlanner plugin
32PLUGINLIB_EXPORT_CLASS(
34
35using namespace std::literals::chrono_literals;
36
37namespace cl_nav2z
38{
39namespace backward_local_planner
40{
47
54
56{
57 RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller BackwardLocalPlanner");
59
60 goalMarkerPublisher_->on_activate();
61 planPub_->on_activate();
62 backwardsPlanPath_.clear();
63}
64
66{
67 this->clearMarkers();
68 RCLCPP_WARN_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] deactivated");
69 planPub_->on_deactivate();
70 goalMarkerPublisher_->on_deactivate();
71}
72
74{
75 this->clearMarkers();
76 RCLCPP_WARN_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] cleanup");
77 this->backwardsPlanPath_.clear();
79}
80
87template <typename T>
88void tryGetOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::string param, T & value)
89{
90 if (!node->get_parameter(param, value))
91 {
92 node->set_parameter(rclcpp::Parameter(param, value));
93 }
94}
95
97 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name,
98 const std::shared_ptr<tf2_ros::Buffer> & tf,
99 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
100{
101 this->costmapRos_ = costmap_ros;
102 // rclcpp::Node::SharedPtr nh("~/BackwardLocalPlanner");
103 this->nh_ = parent.lock();
104 this->name_ = name;
105 this->tf_ = tf;
106
107 k_rho_ = -1.0;
108 k_alpha_ = 0.5;
109 k_betta_ = -1.0; // set to zero means that orientation is not important
117 waitingTimeout_ = rclcpp::Duration(10s);
118
119 this->currentCarrotPoseIndex_ = 0;
120
122 nh_, name_ + ".pure_spinning_straight_line_mode", straightBackwardsAndPureSpinningMode_);
123
124 declareOrSet(nh_, name_ + ".k_rho", k_rho_);
125 declareOrSet(nh_, name_ + ".k_alpha", k_alpha_);
126 declareOrSet(nh_, name_ + ".k_betta", k_betta_);
127 declareOrSet(nh_, name_ + ".linear_mode_rho_error_threshold", linear_mode_rho_error_threshold_);
128
129 declareOrSet(nh_, name_ + ".carrot_distance", carrot_distance_);
130 declareOrSet(nh_, name_ + ".carrot_angular_distance", carrot_angular_distance_);
131 declareOrSet(nh_, name_ + ".enable_obstacle_checking", enable_obstacle_checking_);
132
133 declareOrSet(nh_, name_ + ".max_linear_x_speed", max_linear_x_speed_);
134 declareOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
135
136 // we have to do this, for example for the case we are refining the final orientation.
137 // check at some point if the carrot is reached in "goal linear distance", then we go into
138 // some automatic pure-spinning mode where we only update the orientation
139 // This means that if we reach the carrot with precision we go into pure spinning mode but we cannot
140 // leave that point (maybe this could be improved)
141
143 {
144 RCLCPP_WARN_STREAM(
145 nh_->get_logger(), "[BackwardLocalPlanner] carrot_angular_distance ("
147 << ") cannot be lower than yaw_goal_tolerance (" << yaw_goal_tolerance_
148 << ") setting carrot_angular_distance = " << yaw_goal_tolerance_);
150 }
151
153 {
154 RCLCPP_WARN_STREAM(
155 nh_->get_logger(), "[BackwardLocalPlanner] carrot_linear_distance ("
156 << carrot_distance_ << ") cannot be lower than xy_goal_tolerance_ ("
158 << ") setting carrot_angular_distance = " << xy_goal_tolerance_);
160 }
161
162 goalMarkerPublisher_ = nh_->create_publisher<visualization_msgs::msg::MarkerArray>(
163 "backward_local_planner/goal_marker", 1);
164
165 planPub_ = nh_->create_publisher<nav_msgs::msg::Path>("backward_local_planner/path", 1);
166}
167
169{
170 RCLCPP_INFO_STREAM(nh_->get_logger(), "--- parameters ---");
171 tryGetOrSet(nh_, name_ + ".k_rho", k_rho_);
172 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".k_rho:" << k_rho_);
173 tryGetOrSet(nh_, name_ + ".k_alpha", k_alpha_);
174 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".k_alpha:" << k_alpha_);
175 tryGetOrSet(nh_, name_ + ".k_betta", k_betta_);
176 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".k_betta:" << k_betta_);
177
178 tryGetOrSet(nh_, name_ + ".enable_obstacle_checking", enable_obstacle_checking_);
179 RCLCPP_INFO_STREAM(
180 nh_->get_logger(), name_ + ".enable_obstacle_checking: " << enable_obstacle_checking_);
181
182 tryGetOrSet(nh_, name_ + ".carrot_distance", carrot_distance_);
183 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".carrot_distance:" << carrot_distance_);
184 tryGetOrSet(nh_, name_ + ".carrot_angular_distance", carrot_angular_distance_);
185 RCLCPP_INFO_STREAM(
186 nh_->get_logger(), name_ + ".carrot_angular_distance: " << carrot_angular_distance_);
187
189 nh_, name_ + ".pure_spinning_straight_line_mode", straightBackwardsAndPureSpinningMode_);
190 RCLCPP_INFO_STREAM(
191 nh_->get_logger(),
192 name_ + ".pure_spinning_straight_line_mode: " << straightBackwardsAndPureSpinningMode_);
193
194 tryGetOrSet(nh_, name_ + ".linear_mode_rho_error_threshold", linear_mode_rho_error_threshold_);
195 RCLCPP_INFO_STREAM(
196 nh_->get_logger(),
197 name_ + ".linear_mode_rho_error_threshold: " << linear_mode_rho_error_threshold_);
198 tryGetOrSet(nh_, name_ + ".max_linear_x_speed", max_linear_x_speed_);
199 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".max_linear_x_speed: " << max_linear_x_speed_);
200 tryGetOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
201 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".max_angular_z_speed: " << max_angular_z_speed_);
202
204 {
205 RCLCPP_WARN_STREAM(
206 nh_->get_logger(), "[BackwardLocalPlanner] carrot_angular_distance ("
208 << ") cannot be lower than yaw_goal_tolerance (" << yaw_goal_tolerance_
209 << ") setting carrot_angular_distance = " << yaw_goal_tolerance_);
211 nh_->set_parameter(
212 rclcpp::Parameter(name_ + ".carrot_angular_distance", carrot_angular_distance_));
213 }
214 RCLCPP_INFO_STREAM(
215 nh_->get_logger(), name_ + ".carrot_angular_distance: " << carrot_angular_distance_);
216
218 {
219 RCLCPP_WARN_STREAM(
220 nh_->get_logger(), "[BackwardLocalPlanner] carrot_linear_distance ("
221 << carrot_distance_ << ") cannot be lower than xy_goal_tolerance_ ("
223 << ") setting carrot_angular_distance = " << xy_goal_tolerance_);
225 nh_->set_parameter(rclcpp::Parameter(name_ + ".carrot_distance", carrot_distance_));
226 }
227 RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".carrot_distance:" << carrot_distance_);
228 RCLCPP_INFO_STREAM(nh_->get_logger(), "--- end params ---");
229}
230
232 const double & /*speed_limit*/, const bool & /*percentage*/)
233{
234 RCLCPP_WARN_STREAM(
235 nh_->get_logger(),
236 "BackwardLocalPlanner::setSpeedLimit invoked. Ignored, funcionality not "
237 "implemented.");
238}
245 const geometry_msgs::msg::PoseStamped & tfpose, double & dist, double & angular_error)
246{
247 double angle = tf2::getYaw(tfpose.pose.orientation);
248 auto & carrot_pose = backwardsPlanPath_[currentCarrotPoseIndex_];
249 const geometry_msgs::msg::Point & carrot_point = carrot_pose.pose.position;
250
251 tf2::Quaternion carrot_orientation;
252 tf2::convert(carrot_pose.pose.orientation, carrot_orientation);
253 geometry_msgs::msg::Pose currentPoseDebugMsg = tfpose.pose;
254
255 // take error from the current position to the path point
256 double dx = carrot_point.x - tfpose.pose.position.x;
257 double dy = carrot_point.y - tfpose.pose.position.y;
258
259 dist = sqrt(dx * dx + dy * dy);
260
261 double pangle = tf2::getYaw(carrot_orientation);
262 angular_error = fabs(angles::shortest_angular_distance(pangle, angle));
263
264 RCLCPP_INFO_STREAM(
265 nh_->get_logger(), "[BackwardLocalPlanner] Compute carrot errors from current pose. (linear "
266 << dist << ")(angular " << angular_error << ")" << std::endl
267 << "Current carrot pose: " << std::endl
268 << carrot_pose << std::endl
269 << "Current actual pose:" << std::endl
270 << currentPoseDebugMsg);
271}
272
278bool BackwardLocalPlanner::updateCarrotGoal(const geometry_msgs::msg::PoseStamped & tfpose)
279{
280 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardsLocalPlanner] --- Carrot update ---");
281 double disterr = 0, angleerr = 0;
282 // iterate the point from the current position and backward until reaching a new goal point in the path
283 // this algorithm among other advantages has that skip the looping with an eager global planner
284 // that recalls the same plan (the already performed part of the plan in the current pose is skipped)
285 while (currentCarrotPoseIndex_ < (long)backwardsPlanPath_.size() - 1)
286 {
288
289 RCLCPP_INFO_STREAM(
290 nh_->get_logger(), "[BackwardsLocalPlanner] update carrot goal: Current index: "
291 << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
292 RCLCPP_INFO(
293 nh_->get_logger(),
294 "[BackwardsLocalPlanner] update carrot goal: linear error %lf, angular error: %lf", disterr,
295 angleerr);
296
297 // target pose found, goal carrot tries to escape!
298 if (disterr < carrot_distance_ && angleerr < carrot_angular_distance_)
299 {
302 RCLCPP_INFO_STREAM(
303 nh_->get_logger(), "[BackwardsLocalPlanner] move carrot fw "
304 << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
305 }
306 else
307 {
308 // carrot already escaped
309 break;
310 }
311 }
312 // RCLCPP_INFO(nh_->get_logger(),"[BackwardsLocalPlanner] computing angular error");
313 if (
314 currentCarrotPoseIndex_ >= (long)backwardsPlanPath_.size() - 1 && backwardsPlanPath_.size() > 0)
315 {
317 // reupdated errors
319 }
320
321 RCLCPP_INFO(
322 nh_->get_logger(), "[BackwardsLocalPlanner] Current index carrot goal: %d",
324 RCLCPP_INFO(
325 nh_->get_logger(),
326 "[BackwardsLocalPlanner] Update carrot goal: linear error %lf (xytol: %lf), angular error: "
327 "%lf",
328 disterr, xy_goal_tolerance_, angleerr);
329
330 bool carrotInGoalLinearRange = disterr < xy_goal_tolerance_;
331 RCLCPP_INFO(
332 nh_->get_logger(), "[BackwardsLocalPlanner] carrot in goal radius: %d",
333 carrotInGoalLinearRange);
334
335 RCLCPP_INFO(nh_->get_logger(), "[BackwardsLocalPlanner] ---End carrot update---");
336
337 return carrotInGoalLinearRange;
338}
339
341{
342 // this function should be called always the carrot is updated
343 divergenceDetectionLastCarrotLinearDistance_ = std::numeric_limits<double>::max();
344 return true;
345}
346
347bool BackwardLocalPlanner::divergenceDetectionUpdate(const geometry_msgs::msg::PoseStamped & tfpose)
348{
349 double disterr = 0, angleerr = 0;
351
352 RCLCPP_INFO_STREAM(
353 nh_->get_logger(), "[BackwardLocalPlanner] Divergence check. carrot goal distance. was: "
355 << ", now it is: " << disterr);
357 {
358 // candidate of divergence, we do not throw the divergence alarm yet
359 // but we neither update the distance since it is worse than the one
360 // we had previously with the same carrot.
361 const double MARGIN_FACTOR = 1.2;
362 if (disterr > MARGIN_FACTOR * divergenceDetectionLastCarrotLinearDistance_)
363 {
364 RCLCPP_ERROR_STREAM(
365 nh_->get_logger(),
366 "[BackwardLocalPlanner] Divergence detected. The same carrot goal distance was previously: "
367 << divergenceDetectionLastCarrotLinearDistance_ << "but now it is: " << disterr);
368 return true;
369 }
370 else
371 {
372 // divergence candidate
373 return false;
374 }
375 }
376 else
377 {
378 // update:
380 return false;
381 }
382}
383
385 const geometry_msgs::msg::PoseStamped & tfpose)
386{
387 // this function is specially useful when we want to reach the goal with a lot
388 // of precision. We may pass the goal and then the controller enters in some
389 // unstable state. With this, we are able to detect when stop moving.
390
391 // only apply if the carrot is in goal position and also if we are not in a pure spinning behavior v!=0
392
393 auto & carrot_pose = backwardsPlanPath_[currentCarrotPoseIndex_];
394 const geometry_msgs::msg::Point & carrot_point = carrot_pose.pose.position;
395 double yaw = tf2::getYaw(carrot_pose.pose.orientation);
396
397 // direction vector
398 double vx = cos(yaw);
399 double vy = sin(yaw);
400
401 // line implicit equation
402 // ax + by + c = 0
403 double c = -vx * carrot_point.x - vy * carrot_point.y;
404 const double C_OFFSET_METERS = 0.05; // 5 cm
405 double check = vx * tfpose.pose.position.x + vy * tfpose.pose.position.y + c + C_OFFSET_METERS;
406
407 RCLCPP_INFO_STREAM(
408 nh_->get_logger(),
409 "[BackwardLocalPlanner] half plane constraint:" << vx << "*" << carrot_point.x << " + " << vy
410 << "*" << carrot_point.y << " + " << c);
411 RCLCPP_INFO_STREAM(
412 nh_->get_logger(), "[BackwardLocalPlanner] constraint evaluation: "
413 << vx << "*" << tfpose.pose.position.x << " + " << vy << "*"
414 << tfpose.pose.position.y << " + " << c << " = " << check);
415
416 return check < 0;
417}
418
420 const geometry_msgs::msg::PoseStamped & tfpose,
421 const geometry_msgs::msg::Twist & /*currentTwist*/, double angle_error, bool & linearGoalReached,
422 nav2_core::GoalChecker * /*goal_checker*/)
423{
424 auto & finalgoal = backwardsPlanPath_.back();
425 double gdx = finalgoal.pose.position.x - tfpose.pose.position.x;
426 double gdy = finalgoal.pose.position.y - tfpose.pose.position.y;
427 double goaldist = sqrt(gdx * gdx + gdy * gdy);
428
429 auto abs_angle_error = fabs(angle_error);
430 // final_alpha_error =
431 RCLCPP_INFO_STREAM(
432 nh_->get_logger(), "[BackwardLocalPlanner] goal check. linear dist: "
433 << goaldist << "(" << this->xy_goal_tolerance_ << ")"
434 << ", angular dist: " << abs_angle_error << "("
435 << this->yaw_goal_tolerance_ << ")");
436
437 linearGoalReached = goaldist < this->xy_goal_tolerance_;
438
439 return linearGoalReached && abs_angle_error < this->yaw_goal_tolerance_;
440 // return goal_checker->isGoalReached(tfpose.pose, finalgoal.pose, currentTwist);
441}
442
449 const geometry_msgs::msg::PoseStamped & /*tfpose*/, double & vetta, double & gamma,
450 double alpha_error, double betta_error, double rho_error)
451{
452 if (rho_error > linear_mode_rho_error_threshold_) // works in straight motion mode
453 {
454 vetta = k_rho_ * rho_error;
455 gamma = k_alpha_ * alpha_error;
456 }
457 else if (fabs(betta_error) >= this->yaw_goal_tolerance_) // works in pure spinning mode
458 {
459 vetta = 0; // disable linear
460 gamma = k_betta_ * betta_error;
461 }
462}
463
469geometry_msgs::msg::TwistStamped BackwardLocalPlanner::computeVelocityCommands(
470 const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity,
471 nav2_core::GoalChecker * goal_checker)
472{
473 RCLCPP_INFO(
474 nh_->get_logger(),
475 "[BackwardLocalPlanner] ------------------- LOCAL PLANNER LOOP -----------------");
476 this->updateParameters();
477
478 // consistency check
479 if (this->backwardsPlanPath_.size() > 0)
480 {
481 RCLCPP_INFO_STREAM(
482 nh_->get_logger(), "[BackwardLocalPlanner] Current pose frame id: "
483 << backwardsPlanPath_.front().header.frame_id
484 << ", path pose frame id: " << pose.header.frame_id);
485
486 if (backwardsPlanPath_.front().header.frame_id != pose.header.frame_id)
487 {
488 RCLCPP_ERROR_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] Inconsistent frames");
489 }
490 }
491
492 // xy_goal_tolerance and yaw_goal_tolerance are just used for logging proposes and clamping the carrot
493 // goal distance (parameter safety)
494 if (xy_goal_tolerance_ == -1 || yaw_goal_tolerance_ == -1)
495 {
496 geometry_msgs::msg::Pose posetol;
497 geometry_msgs::msg::Twist twistol;
498 if (goal_checker->getTolerances(posetol, twistol))
499 {
500 xy_goal_tolerance_ = posetol.position.x;
501 yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation);
502 //xy_goal_tolerance_ = posetol.position.x * 0.35; // WORKAROUND ISSUE GOAL CHECKER NAV_CONTROLLER DIFF
503 //yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation) * 0.35;
504 RCLCPP_INFO_STREAM(
505 nh_->get_logger(), "[BackwardLocalPlanner] xy_goal_tolerance_: "
507 << ", yaw_goal_tolerance_: " << yaw_goal_tolerance_);
508 }
509 else
510 {
511 RCLCPP_INFO_STREAM(
512 nh_->get_logger(), "[BackwardLocalPlanner] could not get tolerances from goal checker");
513 }
514 }
515
516 RCLCPP_INFO(
517 nh_->get_logger(),
518 "[BackwardLocalPlanner] ------------------- LOCAL PLANNER LOOP -----------------");
519
520 geometry_msgs::msg::TwistStamped cmd_vel;
521 RCLCPP_INFO(nh_->get_logger(), "[BackwardLocalPlanner] LOCAL PLANNER LOOP");
522 geometry_msgs::msg::PoseStamped paux;
523 geometry_msgs::msg::PoseStamped tfpose;
524
525 if (!costmapRos_->getRobotPose(tfpose))
526 {
527 RCLCPP_ERROR(
528 nh_->get_logger(),
529 "[BackwardLocalPlanner] missing robot pose, canceling compute Velocity Command");
530 } // it is not working in the pure spinning reel example, maybe the hyperplane check is enough
531 bool divergenceDetected = false;
532
533 bool emergency_stop = false;
534 if (divergenceDetected)
535 {
536 RCLCPP_ERROR(
537 nh_->get_logger(), "[BackwardLocalPlanner] Divergence detected. Sending emergency stop.");
538 emergency_stop = true;
539 }
540
541 bool carrotInLinearGoalRange = updateCarrotGoal(tfpose);
542 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] carrot goal created");
543
544 if (emergency_stop)
545 {
546 cmd_vel.twist.linear.x = 0;
547 cmd_vel.twist.angular.z = 0;
548 RCLCPP_INFO_STREAM(
549 nh_->get_logger(), "[BackwardLocalPlanner] emergency stop, exit compute commands");
550 // return false;
551 return cmd_vel;
552 }
553
554 // ------ Evaluate the current context ----
555 double rho_error, betta_error, alpha_error;
556
557 // getting carrot goal information
558 tf2::Quaternion q;
559 tf2::convert(tfpose.pose.orientation, q);
560
561 RCLCPP_INFO_STREAM(
562 nh_->get_logger(), "[BackwardLocalPlanner] carrot goal: " << currentCarrotPoseIndex_ << "/"
563 << backwardsPlanPath_.size());
564 const geometry_msgs::msg::PoseStamped & carrotgoalpose =
566 RCLCPP_INFO_STREAM(
567 nh_->get_logger(), "[BackwardLocalPlanner] carrot goal pose current index: "
568 << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size() << ": "
569 << carrotgoalpose);
570 const geometry_msgs::msg::Point & carrotGoalPosition = carrotgoalpose.pose.position;
571
572 tf2::Quaternion goalQ;
573 tf2::fromMsg(carrotgoalpose.pose.orientation, goalQ);
574 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] -- Control Policy --");
575 // goal orientation (global frame)
576 double betta = tf2::getYaw(goalQ);
577 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] goal orientation: " << betta);
578 betta = betta + betta_offset_;
579
580 double dx = carrotGoalPosition.x - tfpose.pose.position.x;
581 double dy = carrotGoalPosition.y - tfpose.pose.position.y;
582
583 // distance error to the targetpoint
584 rho_error = sqrt(dx * dx + dy * dy);
585
586 // heading to goal angle
587 double theta = tf2::getYaw(q);
588 double alpha = atan2(dy, dx);
589 alpha = alpha + alpha_offset_;
590
591 alpha_error = angles::shortest_angular_distance(alpha, theta);
592 betta_error = angles::shortest_angular_distance(betta, theta);
593 //------------- END CONTEXT EVAL ----------
594
595 bool linearGoalReached;
596 bool currentPoseInGoal =
597 checkCurrentPoseInGoalRange(tfpose, velocity, betta_error, linearGoalReached, goal_checker);
598
599 // Make sure the robot is very close to the goal and it is really in the the last goal point.
600 bool carrotInFinalGoalIndex = currentCarrotPoseIndex_ == (int)backwardsPlanPath_.size() - 1;
601
602 // checking if we are really in the end goal pose
603 if (currentPoseInGoal && carrotInFinalGoalIndex)
604 {
605 goalReached_ = true;
606 // backwardsPlanPath_.clear();
607 RCLCPP_INFO_STREAM(
608 nh_->get_logger(),
609 "[BackwardLocalPlanner] GOAL REACHED. Send stop command and skipping trajectory collision: "
610 << cmd_vel.twist);
611 cmd_vel.twist.linear.x = 0;
612 cmd_vel.twist.angular.z = 0;
613 return cmd_vel;
614 }
615 else if (
616 carrotInLinearGoalRange &&
617 linearGoalReached) // checking if we are in the end goal point but with incorrect
618 // orientation
619 {
620 // this means that we are not in the final angular distance, and we may even not be in the last carrot index
621 // (several intermediate angular poses until the last goal pose)
623 }
624
625 // --------------------
626 double vetta, gamma;
628 {
629 // decorated control rule for this mode
631 tfpose, vetta, gamma, alpha_error, betta_error, rho_error);
632 }
633 else // default free navigation backward motion mode
634 {
635 // regular control rule
636 vetta = k_rho_ * rho_error;
637 gamma = k_alpha_ * alpha_error + k_betta_ * betta_error;
638
639 // Even if we are in free navigation, we can enter in the pure spinning state.
640 // then, the linear motion is deactivated.
642 {
643 RCLCPP_INFO(
644 nh_->get_logger(),
645 "[BackwardLocalPlanner] we entered in a pure spinning state even in not pure-spining "
646 "configuration, "
647 "carrotDistanceGoalReached: %d",
648 carrotInLinearGoalRange);
649 gamma = k_betta_ * betta_error;
650 vetta = 0;
651 }
652
653 // classical control to reach a goal backwards
654 }
655
656 // Apply command and Clamp to limits
657 cmd_vel.twist.linear.x = vetta;
658 cmd_vel.twist.angular.z = gamma;
659
660 if (cmd_vel.twist.linear.x > max_linear_x_speed_)
661 {
662 cmd_vel.twist.linear.x = max_linear_x_speed_;
663 }
664 else if (cmd_vel.twist.linear.x < -max_linear_x_speed_)
665 {
666 cmd_vel.twist.linear.x = -max_linear_x_speed_;
667 }
668
669 if (cmd_vel.twist.angular.z > max_angular_z_speed_)
670 {
671 cmd_vel.twist.angular.z = max_angular_z_speed_;
672 }
673 else if (cmd_vel.twist.angular.z < -max_angular_z_speed_)
674 {
675 cmd_vel.twist.angular.z = -max_angular_z_speed_;
676 }
677
678 publishGoalMarker(carrotGoalPosition.x, carrotGoalPosition.y, betta);
679
680 RCLCPP_INFO_STREAM(
681 nh_->get_logger(), "[BackwardLocalPlanner] local planner,"
682 << std::endl
683 << " current pose in goal: " << currentPoseInGoal << std::endl
684 << " carrot in final goal index: " << carrotInFinalGoalIndex << std::endl
685 << " carrot in linear goal range: " << carrotInLinearGoalRange << std::endl
686 << " straightAnPureSpiningMode: " << straightBackwardsAndPureSpinningMode_
687 << std::endl
688 << " inGoalPureSpinningState: " << inGoalPureSpinningState_ << std::endl
689 << " theta: " << theta << std::endl
690 << " betta: " << theta << std::endl
691 << " err_x: " << dx << std::endl
692 << " err_y:" << dy << std::endl
693 << " rho_error:" << rho_error << std::endl
694 << " alpha_error:" << alpha_error << std::endl
695 << " betta_error:" << betta_error << std::endl
696 << " vetta:" << vetta << std::endl
697 << " gamma:" << gamma << std::endl
698 << " cmd_vel.lin.x:" << cmd_vel.twist.linear.x << std::endl
699 << " cmd_vel.ang.z:" << cmd_vel.twist.angular.z);
700
702 {
703 bool carrotHalfPlaneConstraintFailure = checkCarrotHalfPlainConstraint(tfpose);
704
705 if (carrotHalfPlaneConstraintFailure)
706 {
707 RCLCPP_ERROR(
708 nh_->get_logger(),
709 "[BackwardLocalPlanner] CarrotHalfPlaneConstraintFailure detected. Sending "
710 "emergency stop and success to the planner.");
711 cmd_vel.twist.linear.x = 0;
712 }
713 }
714
715 // ---------------------- TRAJECTORY PREDICTION AND COLLISION AVOIDANCE ---------------------
716 // cmd_vel.twist.linear.x=0;
717 // cmd_vel.twist.angular.z = 0;
718
719 geometry_msgs::msg::PoseStamped global_pose;
720 costmapRos_->getRobotPose(global_pose);
721
722 auto * costmap2d = costmapRos_->getCostmap();
723 auto yaw = tf2::getYaw(global_pose.pose.orientation);
724
725 auto & pos = global_pose.pose.position;
726
727 Eigen::Vector3f currentpose(pos.x, pos.y, yaw);
728 Eigen::Vector3f currentvel(
729 cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z);
730 std::vector<Eigen::Vector3f> trajectory;
731 this->generateTrajectory(
732 currentpose, currentvel, 0.8 /*meters*/, M_PI / 8 /*rads*/, 3.0 /*seconds*/, 0.05 /*seconds*/,
733 trajectory);
734
735 // check plan rejection
736 bool acceptedLocalTrajectoryFreeOfObstacles = true;
737
738 unsigned int mx, my;
739
741 {
742 if (backwardsPlanPath_.size() > 0)
743 {
744 auto & finalgoalpose = backwardsPlanPath_.back();
745
746 int i = 0;
747 // RCLCPP_INFO_STREAM(nh_->get_logger(), "lplanner goal: " << finalgoalpose.pose.position);
748 geometry_msgs::msg::Twist mockzerospeed;
749
750 for (auto & p : trajectory)
751 {
752 /*geometry_msgs::msg::Pose pg;
753 pg.position.x = p[0];
754 pg.position.y = p[1];
755 tf2::Quaternion q;
756 q.setRPY(0, 0, p[2]);
757 pg.orientation = tf2::toMsg(q);
758
759 // WARNING I CAN'T USE isGoalReached because I can change the state of a stateful goal checker
760 if (goal_checker->isGoalReached(pg, finalgoalpose.pose, mockzerospeed))*/
761
762 float dx = p[0] - finalgoalpose.pose.position.x;
763 float dy = p[1] - finalgoalpose.pose.position.y;
764
765 float dst = sqrt(dx * dx + dy * dy);
766 if (dst < xy_goal_tolerance_)
767 {
768 RCLCPP_INFO(
769 nh_->get_logger(),
770 "[BackwardLocalPlanner] trajectory simulation for collision checking: goal "
771 "reached with no collision");
772 break;
773 }
774
775 costmap2d->worldToMap(p[0], p[1], mx, my);
776 // unsigned int cost = costmap2d->getCost(mx, my);
777
778 // RCLCPP_INFO(nh_->get_logger(),"[BackwardLocalPlanner] checking cost pt %d [%lf, %lf] cell[%d,%d] = %d", i,
779 // p[0], p[1], mx, my, cost); RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] cost: " << cost);
780
781 // static const unsigned char NO_INFORMATION = 255;
782 // static const unsigned char LETHAL_OBSTACLE = 254;
783 // static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
784 // static const unsigned char FREE_SPACE = 0;
785
786 if (costmap2d->getCost(mx, my) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
787 {
788 acceptedLocalTrajectoryFreeOfObstacles = false;
789 RCLCPP_WARN_STREAM(
790 nh_->get_logger(),
791 "[BackwardLocalPlanner] ABORTED LOCAL PLAN BECAUSE OBSTACLE DETEDTED at point "
792 << i << "/" << trajectory.size() << std::endl
793 << p[0] << ", " << p[1]);
794 break;
795 }
796 i++;
797 }
798 }
799 else
800 {
801 RCLCPP_WARN(
802 nh_->get_logger(), "[BackwardLocalPlanner] Abort local - Backwards global plan size: %ld",
803 backwardsPlanPath_.size());
804 cmd_vel.twist.angular.z = 0;
805 cmd_vel.twist.linear.x = 0;
806 // return false;
807 }
808 }
809
810 if (acceptedLocalTrajectoryFreeOfObstacles)
811 {
812 waiting_ = false;
813 RCLCPP_INFO(
814 nh_->get_logger(),
815 "[BackwardLocalPlanner] accepted local trajectory free of obstacle. Local planner "
816 "continues.");
817 return cmd_vel;
818 // return true;
819 }
820 else // that is not appceted because existence of obstacles
821 {
822 // emergency stop for collision: waiting a while before sending error
823 cmd_vel.twist.linear.x = 0;
824 cmd_vel.twist.angular.z = 0;
825
826 if (waiting_ == false)
827 {
828 waiting_ = true;
829 waitingStamp_ = nh_->now();
830 RCLCPP_WARN(
831 nh_->get_logger(), "[BackwardLocalPlanner][Not accepted local plan] starting countdown");
832 }
833 else
834 {
835 auto waitingduration = nh_->now() - waitingStamp_;
836
837 if (waitingduration > this->waitingTimeout_)
838 {
839 RCLCPP_WARN(
840 nh_->get_logger(), "[BackwardLocalPlanner][Abort local] timeout! duration %lf/%f",
841 waitingduration.seconds(), waitingTimeout_.seconds());
842 // return false;
843 cmd_vel.twist.linear.x = 0;
844 cmd_vel.twist.angular.z = 0;
845 return cmd_vel;
846 }
847 }
848
849 return cmd_vel;
850 }
851}
852
859{
860 RCLCPP_INFO(nh_->get_logger(), "[BackwardLocalPlanner] isGoalReached call");
861 return goalReached_;
862}
863
864bool BackwardLocalPlanner::findInitialCarrotGoal(geometry_msgs::msg::PoseStamped & tfpose)
865{
866 double lineardisterr, angleerr;
867 bool inCarrotRange = false;
868
869 // initial state check
870 computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, lineardisterr, angleerr);
871
872 // double minpointdist = std::numeric_limits<double>::max();
873
874 // lets set the carrot-goal in the correct place with this loop
875 while (currentCarrotPoseIndex_ < (int)backwardsPlanPath_.size() && !inCarrotRange)
876 {
877 computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, lineardisterr, angleerr);
878
879 RCLCPP_INFO(
880 nh_->get_logger(),
881 "[BackwardLocalPlanner] Finding initial carrot goal i=%d - error to carrot, linear = %lf "
882 "(%lf), "
883 "angular : %lf (%lf)",
885
886 // current path point is inside the carrot distance range, goal carrot tries to escape!
887 if (lineardisterr < carrot_distance_ && angleerr < carrot_angular_distance_)
888 {
889 RCLCPP_INFO(
890 nh_->get_logger(),
891 "[BackwardLocalPlanner] Finding initial carrot goal i=%d - in carrot Range",
893 inCarrotRange = true;
894 // we are inside the goal range
895 }
896 else if (
897 inCarrotRange && (lineardisterr > carrot_distance_ || angleerr > carrot_angular_distance_))
898 {
899 // we were inside the carrot range but not anymore, now we are just leaving. we want to continue forward
900 // (currentCarrotPoseIndex_++) unless we go out of the carrot range
901
902 // but we rollback last index increment (to go back inside the carrot goal scope) and start motion with that
903 // carrot goal we found
905 break;
906 }
907 else
908 {
909 RCLCPP_INFO(
910 nh_->get_logger(),
911 "[BackwardLocalPlanner] Finding initial carrot goal i=%d - carrot out of range, searching "
912 "coincidence...",
914 }
915
917 RCLCPP_INFO_STREAM(
918 nh_->get_logger(), "[BackwardLocalPlanner] setPlan: fw" << currentCarrotPoseIndex_);
919 }
920
921 RCLCPP_INFO_STREAM(
922 nh_->get_logger(), "[BackwardLocalPlanner] setPlan: (found first carrot:"
923 << inCarrotRange << ") initial carrot point index: "
924 << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
925
926 return inCarrotRange;
927}
928
930{
931 // this algorithm is really important to have a precise carrot (linear or angular)
932 // and not being considered as a divergence from the path
933
934 RCLCPP_INFO(nh_->get_logger(), "[BackwardLocalPlanner] resample precise");
935 if (backwardsPlanPath_.size() <= 1)
936 {
937 RCLCPP_INFO_STREAM(
938 nh_->get_logger(),
939 "[BackwardLocalPlanner] resample precise skipping, size: " << backwardsPlanPath_.size());
940 return false;
941 }
942
943 int counter = 0;
944 double maxallowedAngularError = 0.45 * this->carrot_angular_distance_; // nyquist
945 double maxallowedLinearError = 0.45 * this->carrot_distance_; // nyquist
946
947 for (int i = 0; i < (int)backwardsPlanPath_.size() - 1; i++)
948 {
949 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] resample precise, check: " << i);
950 auto & currpose = backwardsPlanPath_[i];
951 auto & nextpose = backwardsPlanPath_[i + 1];
952
953 tf2::Quaternion qCurrent, qNext;
954 tf2::convert(currpose.pose.orientation, qCurrent);
955 tf2::convert(nextpose.pose.orientation, qNext);
956
957 double dx = nextpose.pose.position.x - currpose.pose.position.x;
958 double dy = nextpose.pose.position.y - currpose.pose.position.y;
959 double dist = sqrt(dx * dx + dy * dy);
960
961 bool resample = false;
962 if (dist > maxallowedLinearError)
963 {
964 RCLCPP_INFO_STREAM(
965 nh_->get_logger(), "[BackwardLocalPlanner] resampling point, linear distance:"
966 << dist << "(" << maxallowedLinearError << ")" << i);
967 resample = true;
968 }
969 else
970 {
971 double currentAngle = tf2::getYaw(qCurrent);
972 double nextAngle = tf2::getYaw(qNext);
973
974 double angularError = fabs(angles::shortest_angular_distance(currentAngle, nextAngle));
975 if (angularError > maxallowedAngularError)
976 {
977 resample = true;
978 RCLCPP_INFO_STREAM(
979 nh_->get_logger(), "[BackwardLocalPlanner] resampling point, angular distance:"
980 << angularError << "(" << maxallowedAngularError << ")" << i);
981 }
982 }
983
984 if (resample)
985 {
986 geometry_msgs::msg::PoseStamped pintermediate;
987 auto duration = rclcpp::Time(nextpose.header.stamp) - rclcpp::Time(currpose.header.stamp);
988
989 pintermediate.header.frame_id = currpose.header.frame_id;
990 pintermediate.header.stamp = rclcpp::Time(currpose.header.stamp) + duration * 0.5;
991
992 pintermediate.pose.position.x = 0.5 * (currpose.pose.position.x + nextpose.pose.position.x);
993 pintermediate.pose.position.y = 0.5 * (currpose.pose.position.y + nextpose.pose.position.y);
994 pintermediate.pose.position.z = 0.5 * (currpose.pose.position.z + nextpose.pose.position.z);
995 tf2::Quaternion intermediateQuat = tf2::slerp(qCurrent, qNext, 0.5);
996 pintermediate.pose.orientation = tf2::toMsg(intermediateQuat);
997
998 this->backwardsPlanPath_.insert(this->backwardsPlanPath_.begin() + i + 1, pintermediate);
999
1000 // retry this point
1001 i--;
1002 counter++;
1003 }
1004 }
1005
1006 RCLCPP_INFO_STREAM(
1007 nh_->get_logger(), "[BackwardLocalPlanner] End resampling. resampled:" << counter
1008 << " new inserted poses "
1009 "during precise "
1010 "resmapling.");
1011 return true;
1012}
1013
1019void BackwardLocalPlanner::setPlan(const nav_msgs::msg::Path & path)
1020{
1021 RCLCPP_INFO_STREAM(
1022 nh_->get_logger(),
1023 "[BackwardLocalPlanner] setPlan: new global plan received ( " << path.poses.size() << ")");
1024
1025 //------------- TRANSFORM TO LOCAL FRAME PATH ---------------------------
1026 nav_msgs::msg::Path transformedPlan;
1027 rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
1028 // transform global plan to the navigation reference frame
1029 for (auto & p : path.poses)
1030 {
1031 geometry_msgs::msg::PoseStamped transformedPose;
1032 nav_2d_utils::transformPose(tf_, costmapRos_->getGlobalFrameID(), p, transformedPose, ttol);
1033 transformedPose.header.frame_id = costmapRos_->getGlobalFrameID();
1034 transformedPlan.poses.push_back(transformedPose);
1035 }
1036
1037 backwardsPlanPath_ = transformedPlan.poses;
1038
1039 // --------- resampling path feature -----------
1040 geometry_msgs::msg::PoseStamped tfpose;
1041 if (!costmapRos_->getRobotPose(tfpose))
1042 {
1043 RCLCPP_ERROR(nh_->get_logger(), "Failure getting pose from Backward local planner");
1044 return;
1045 }
1046
1047 geometry_msgs::msg::PoseStamped posestamped = tfpose;
1048 backwardsPlanPath_.insert(backwardsPlanPath_.begin(), posestamped);
1049 this->resamplePrecisePlan();
1050
1051 nav_msgs::msg::Path planMsg;
1052 planMsg.poses = backwardsPlanPath_;
1053 planMsg.header.frame_id = costmapRos_->getGlobalFrameID();
1054 planMsg.header.stamp = nh_->now();
1055 planPub_->publish(planMsg);
1056
1057 // ------ reset controller state ----------------------
1058 goalReached_ = false;
1062
1063 if (path.poses.size() == 0)
1064 {
1065 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] received plan without any pose");
1066 // return true;
1067 return;
1068 }
1069
1070 // -------- initialize carrot ----------------
1071 bool foundInitialCarrotGoal = this->findInitialCarrotGoal(tfpose);
1072 if (!foundInitialCarrotGoal)
1073 {
1074 RCLCPP_ERROR(
1075 nh_->get_logger(),
1076 "[BackwardLocalPlanner] new plan rejected. The initial point in the global path is "
1077 "too much far away from the current state (according to carrot_distance "
1078 "parameter)");
1079 // return false; // in this case, the new plan broke the current execution
1080 return;
1081 }
1082 else
1083 {
1084 this->divergenceDetectionUpdate(tfpose);
1085 // SANDARD AND PREFERRED CASE ON NEW PLAN
1086 // return true;
1087 return;
1088 }
1089}
1090
1092 const Eigen::Vector3f & pos, const Eigen::Vector3f & vel, float maxdist, float maxanglediff,
1093 float maxtime, float dt, std::vector<Eigen::Vector3f> & outtraj)
1094{
1095 // simulate the trajectory and check for collisions, updating costs along the way
1096 bool end = false;
1097 float time = 0;
1098 Eigen::Vector3f currentpos = pos;
1099 int i = 0;
1100 while (!end)
1101 {
1102 // add the point to the trajectory so we can draw it later if we want
1103 // traj.addPoint(pos[0], pos[1], pos[2]);
1104
1105 // if (continued_acceleration_) {
1106 // //calculate velocities
1107 // loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
1108 // //RCLCPP_WARN_NAMED(nh_->get_logger(), "Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_,
1109 // loop_vel[0], loop_vel[1], loop_vel[2]);
1110 // }
1111
1112 auto loop_vel = vel;
1113 // update the position of the robot using the velocities passed in
1114 auto newpos = computeNewPositions(currentpos, loop_vel, dt);
1115
1116 auto dx = newpos[0] - currentpos[0];
1117 auto dy = newpos[1] - currentpos[1];
1118 float dist, angledist;
1119
1120 // RCLCPP_INFO(nh_->get_logger(), "traj point %d", i);
1121 dist = sqrt(dx * dx + dy * dy);
1122 if (dist > maxdist)
1123 {
1124 end = true;
1125 // RCLCPP_INFO(nh_->get_logger(), "dist break: %f", dist);
1126 }
1127 else
1128 {
1129 // ouble from, double to
1130 angledist = angles::shortest_angular_distance(currentpos[2], newpos[2]);
1131 if (angledist > maxanglediff)
1132 {
1133 end = true;
1134 // RCLCPP_INFO(nh_->get_logger(), "angle dist break: %f", angledist);
1135 }
1136 else
1137 {
1138 outtraj.push_back(newpos);
1139
1140 time += dt;
1141 if (time > maxtime)
1142 {
1143 end = true;
1144 // RCLCPP_INFO(nh_->get_logger(), "time break: %f", time);
1145 }
1146
1147 // RCLCPP_INFO(nh_->get_logger(), "dist: %f, angledist: %f, time: %f", dist, angledist, time);
1148 }
1149 }
1150
1151 currentpos = newpos;
1152 i++;
1153 } // end for simulation steps
1154}
1155
1157 const Eigen::Vector3f & pos, const Eigen::Vector3f & vel, double dt)
1158{
1159 Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
1160 new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
1161 new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
1162 new_pos[2] = pos[2] + vel[2] * dt;
1163 return new_pos;
1164}
1165
1167{
1168 visualization_msgs::msg::Marker marker;
1169 marker.header.frame_id = this->costmapRos_->getGlobalFrameID();
1170 marker.header.stamp = nh_->now();
1171
1172 marker.ns = "my_namespace2";
1173 marker.id = 0;
1174 marker.type = visualization_msgs::msg::Marker::ARROW;
1175 marker.action = visualization_msgs::msg::Marker::DELETEALL;
1176
1177 visualization_msgs::msg::MarkerArray ma;
1178 ma.markers.push_back(marker);
1179
1180 goalMarkerPublisher_->publish(ma);
1181}
1182
1188void BackwardLocalPlanner::publishGoalMarker(double x, double y, double phi)
1189{
1190 visualization_msgs::msg::Marker marker;
1191 marker.header.frame_id = this->costmapRos_->getGlobalFrameID();
1192 marker.header.stamp = nh_->now();
1193
1194 marker.ns = "my_namespace2";
1195 marker.id = 0;
1196 marker.type = visualization_msgs::msg::Marker::ARROW;
1197 marker.action = visualization_msgs::msg::Marker::ADD;
1198 marker.lifetime = rclcpp::Duration(1.0s);
1199
1200 marker.pose.orientation.w = 1;
1201
1202 marker.scale.x = 0.05;
1203 marker.scale.y = 0.15;
1204 marker.scale.z = 0.05;
1205 marker.color.a = 1.0;
1206
1207 // red marker
1208 marker.color.r = 1;
1209 marker.color.g = 0;
1210 marker.color.b = 0;
1211
1212 geometry_msgs::msg::Point start, end;
1213 start.x = x;
1214 start.y = y;
1215
1216 end.x = x + 0.5 * cos(phi);
1217 end.y = y + 0.5 * sin(phi);
1218
1219 marker.points.push_back(start);
1220 marker.points.push_back(end);
1221
1222 visualization_msgs::msg::MarkerArray ma;
1223 ma.markers.push_back(marker);
1224
1225 goalMarkerPublisher_->publish(ma);
1226}
1227} // namespace backward_local_planner
1228} // namespace cl_nav2z
bool updateCarrotGoal(const geometry_msgs::msg::PoseStamped &pose)
bool checkCarrotHalfPlainConstraint(const geometry_msgs::msg::PoseStamped &pose)
Eigen::Vector3f computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, const std::shared_ptr< tf2_ros::Buffer > &tf, const std::shared_ptr< nav2_costmap_2d::Costmap2DROS > &costmap_ros) override
virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker) override
nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity
void straightBackwardsAndPureSpinCmd(const geometry_msgs::msg::PoseStamped &pose, double &vetta, double &gamma, double alpha_error, double betta_error, double rho_error)
std::vector< geometry_msgs::msg::PoseStamped > backwardsPlanPath_
void generateTrajectory(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray > > goalMarkerPublisher_
bool findInitialCarrotGoal(geometry_msgs::msg::PoseStamped &pose)
bool divergenceDetectionUpdate(const geometry_msgs::msg::PoseStamped &pose)
void setPlan(const nav_msgs::msg::Path &path) override
nav2_core setPlan - Sets the global plan
void computeCurrentEuclideanAndAngularErrorsToCarrotGoal(const geometry_msgs::msg::PoseStamped &pose, double &dist, double &angular_error)
bool checkCurrentPoseInGoalRange(const geometry_msgs::msg::PoseStamped &tfpose, const geometry_msgs::msg::Twist &currentTwist, double angle_error, bool &linearGoalReached, nav2_core::GoalChecker *goalChecker)
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > > planPub_
virtual void setSpeedLimit(const double &speed_limit, const bool &percentage) override
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmapRos_
void declareOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)
Definition: common.hpp:34
void tryGetOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)