SMACC2
Public Member Functions | Private Member Functions | Private Attributes | List of all members
cl_nav2z::backward_local_planner::BackwardLocalPlanner Class Reference

#include <backward_local_planner.hpp>

Inheritance diagram for cl_nav2z::backward_local_planner::BackwardLocalPlanner:
Inheritance graph
Collaboration diagram for cl_nav2z::backward_local_planner::BackwardLocalPlanner:
Collaboration graph

Public Member Functions

 BackwardLocalPlanner ()
 
virtual ~BackwardLocalPlanner ()
 
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
 
void activate () override
 
void deactivate () override
 
void cleanup () override
 
void setPlan (const nav_msgs::msg::Path &path) override
 nav2_core setPlan - Sets the global plan More...
 
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 More...
 
bool isGoalReached ()
 
virtual void setSpeedLimit (const double &speed_limit, const bool &percentage) override
 

Private Member Functions

void updateParameters ()
 
bool findInitialCarrotGoal (geometry_msgs::msg::PoseStamped &pose)
 
bool updateCarrotGoal (const geometry_msgs::msg::PoseStamped &pose)
 
bool resamplePrecisePlan ()
 
void straightBackwardsAndPureSpinCmd (const geometry_msgs::msg::PoseStamped &pose, double &vetta, double &gamma, double alpha_error, double betta_error, double rho_error)
 
void clearMarkers ()
 
void publishGoalMarker (double x, double y, double phi)
 
void computeCurrentEuclideanAndAngularErrorsToCarrotGoal (const geometry_msgs::msg::PoseStamped &pose, double &dist, double &angular_error)
 
bool checkGoalReached (const geometry_msgs::msg::PoseStamped &pose, double vetta, double gamma, double alpha_error, geometry_msgs::msg::Twist &cmd_vel)
 
bool checkCurrentPoseInGoalRange (const geometry_msgs::msg::PoseStamped &tfpose, const geometry_msgs::msg::Twist &currentTwist, double angle_error, bool &linearGoalReached, nav2_core::GoalChecker *goalChecker)
 
bool resetDivergenceDetection ()
 
bool divergenceDetectionUpdate (const geometry_msgs::msg::PoseStamped &pose)
 
bool checkCarrotHalfPlainConstraint (const geometry_msgs::msg::PoseStamped &pose)
 
void generateTrajectory (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)
 
Eigen::Vector3f computeNewPositions (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
 

Private Attributes

rclcpp_lifecycle::LifecycleNode::SharedPtr nh_
 
std::string name_
 
std::vector< geometry_msgs::msg::PoseStamped > backwardsPlanPath_
 
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmapRos_
 
std::shared_ptr< tf2_ros::Buffer > tf_
 
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray > > goalMarkerPublisher_
 
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > > planPub_
 
double k_rho_
 
double k_alpha_
 
double k_betta_
 
double pure_spinning_allowed_betta_error_
 
double linear_mode_rho_error_threshold_
 
const double alpha_offset_ = M_PI
 
const double betta_offset_ = 0
 
double max_linear_x_speed_
 
double max_angular_z_speed_
 
double yaw_goal_tolerance_
 
double xy_goal_tolerance_
 
meter carrot_distance_
 
rad carrot_angular_distance_
 
meter divergenceDetectionLastCarrotLinearDistance_
 
double transform_tolerance_
 
rclcpp::Duration waitingTimeout_
 
rclcpp::Time waitingStamp_
 
bool goalReached_
 
bool initialPureSpinningStage_
 
bool straightBackwardsAndPureSpinningMode_ = false
 
bool enable_obstacle_checking_ = true
 
bool inGoalPureSpinningState_ = false
 
bool waiting_
 
int currentCarrotPoseIndex_
 

Detailed Description

Definition at line 40 of file backward_local_planner.hpp.

Constructor & Destructor Documentation

◆ BackwardLocalPlanner()

cl_nav2z::backward_local_planner::BackwardLocalPlanner::BackwardLocalPlanner ( )

◆ ~BackwardLocalPlanner()

cl_nav2z::backward_local_planner::BackwardLocalPlanner::~BackwardLocalPlanner ( )
virtual

~BackwardLocalPlanner()

Definition at line 53 of file backward_local_planner.cpp.

53{}

Member Function Documentation

◆ activate()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::activate ( )
override

Definition at line 55 of file backward_local_planner.cpp.

56{
57 RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller BackwardLocalPlanner");
59
60 goalMarkerPublisher_->on_activate();
61 planPub_->on_activate();
62 backwardsPlanPath_.clear();
63}
std::vector< geometry_msgs::msg::PoseStamped > backwardsPlanPath_
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray > > goalMarkerPublisher_
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > > planPub_

References backwardsPlanPath_, goalMarkerPublisher_, nh_, planPub_, and updateParameters().

Here is the call graph for this function:

◆ checkCarrotHalfPlainConstraint()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::checkCarrotHalfPlainConstraint ( const geometry_msgs::msg::PoseStamped &  pose)
private

Definition at line 384 of file backward_local_planner.cpp.

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}

References backwardsPlanPath_, currentCarrotPoseIndex_, and nh_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ checkCurrentPoseInGoalRange()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::checkCurrentPoseInGoalRange ( const geometry_msgs::msg::PoseStamped &  tfpose,
const geometry_msgs::msg::Twist &  currentTwist,
double  angle_error,
bool linearGoalReached,
nav2_core::GoalChecker *  goalChecker 
)
private

Definition at line 419 of file backward_local_planner.cpp.

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}

References backwardsPlanPath_, nh_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ checkGoalReached()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::checkGoalReached ( const geometry_msgs::msg::PoseStamped &  pose,
double  vetta,
double  gamma,
double  alpha_error,
geometry_msgs::msg::Twist &  cmd_vel 
)
private

◆ cleanup()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::cleanup ( )
override

Definition at line 73 of file backward_local_planner.cpp.

74{
75 this->clearMarkers();
76 RCLCPP_WARN_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] cleanup");
77 this->backwardsPlanPath_.clear();
79}

References backwardsPlanPath_, clearMarkers(), currentCarrotPoseIndex_, and nh_.

Here is the call graph for this function:

◆ clearMarkers()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::clearMarkers ( )
private

Definition at line 1166 of file backward_local_planner.cpp.

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}
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmapRos_

References costmapRos_, goalMarkerPublisher_, and nh_.

Referenced by cleanup(), and deactivate().

Here is the caller graph for this function:

◆ computeCurrentEuclideanAndAngularErrorsToCarrotGoal()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::computeCurrentEuclideanAndAngularErrorsToCarrotGoal ( const geometry_msgs::msg::PoseStamped &  tfpose,
double &  dist,
double &  angular_error 
)
private

auxiliary functions

Definition at line 244 of file backward_local_planner.cpp.

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}

References backwardsPlanPath_, currentCarrotPoseIndex_, and nh_.

Referenced by divergenceDetectionUpdate(), findInitialCarrotGoal(), and updateCarrotGoal().

Here is the caller graph for this function:

◆ computeNewPositions()

Eigen::Vector3f cl_nav2z::backward_local_planner::BackwardLocalPlanner::computeNewPositions ( const Eigen::Vector3f &  pos,
const Eigen::Vector3f &  vel,
double  dt 
)
private

Definition at line 1156 of file backward_local_planner.cpp.

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}

Referenced by generateTrajectory().

Here is the caller graph for this function:

◆ computeVelocityCommands()

geometry_msgs::msg::TwistStamped cl_nav2z::backward_local_planner::BackwardLocalPlanner::computeVelocityCommands ( const geometry_msgs::msg::PoseStamped &  pose,
const geometry_msgs::msg::Twist &  velocity,
nav2_core::GoalChecker *  goal_checker 
)
overridevirtual

nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity

It is presumed that the global plan is already set.

This is mostly a wrapper for the protected computeVelocityCommands function which has additional debugging info.

Parameters
poseCurrent robot pose
velocityCurrent robot velocity
Returns
The best command for the robot to drive

computeVelocityCommands()

Definition at line 469 of file backward_local_planner.cpp.

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}
bool updateCarrotGoal(const geometry_msgs::msg::PoseStamped &pose)
bool checkCarrotHalfPlainConstraint(const geometry_msgs::msg::PoseStamped &pose)
void straightBackwardsAndPureSpinCmd(const geometry_msgs::msg::PoseStamped &pose, double &vetta, double &gamma, double alpha_error, double betta_error, double rho_error)
void generateTrajectory(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)
bool checkCurrentPoseInGoalRange(const geometry_msgs::msg::PoseStamped &tfpose, const geometry_msgs::msg::Twist &currentTwist, double angle_error, bool &linearGoalReached, nav2_core::GoalChecker *goalChecker)

References alpha_offset_, backwardsPlanPath_, betta_offset_, checkCarrotHalfPlainConstraint(), checkCurrentPoseInGoalRange(), costmapRos_, currentCarrotPoseIndex_, enable_obstacle_checking_, generateTrajectory(), goalReached_, inGoalPureSpinningState_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, nh_, publishGoalMarker(), straightBackwardsAndPureSpinCmd(), straightBackwardsAndPureSpinningMode_, updateCarrotGoal(), updateParameters(), waiting_, waitingStamp_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ configure()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::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

Definition at line 96 of file backward_local_planner.cpp.

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}
void declareOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)
Definition: common.hpp:34

References carrot_angular_distance_, carrot_distance_, costmapRos_, currentCarrotPoseIndex_, declareOrSet(), enable_obstacle_checking_, goalMarkerPublisher_, k_alpha_, k_betta_, k_rho_, linear_mode_rho_error_threshold_, max_angular_z_speed_, max_linear_x_speed_, name_, nh_, planPub_, service_node_3::s, straightBackwardsAndPureSpinningMode_, tf_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ deactivate()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::deactivate ( )
override

Definition at line 65 of file backward_local_planner.cpp.

66{
67 this->clearMarkers();
68 RCLCPP_WARN_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] deactivated");
69 planPub_->on_deactivate();
70 goalMarkerPublisher_->on_deactivate();
71}

References clearMarkers(), goalMarkerPublisher_, nh_, and planPub_.

Here is the call graph for this function:

◆ divergenceDetectionUpdate()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::divergenceDetectionUpdate ( const geometry_msgs::msg::PoseStamped &  pose)
private

Definition at line 347 of file backward_local_planner.cpp.

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}
void computeCurrentEuclideanAndAngularErrorsToCarrotGoal(const geometry_msgs::msg::PoseStamped &pose, double &dist, double &angular_error)

References computeCurrentEuclideanAndAngularErrorsToCarrotGoal(), divergenceDetectionLastCarrotLinearDistance_, and nh_.

Referenced by setPlan().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ findInitialCarrotGoal()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::findInitialCarrotGoal ( geometry_msgs::msg::PoseStamped &  pose)
private

Definition at line 864 of file backward_local_planner.cpp.

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}

References backwardsPlanPath_, carrot_angular_distance_, carrot_distance_, computeCurrentEuclideanAndAngularErrorsToCarrotGoal(), currentCarrotPoseIndex_, and nh_.

Referenced by setPlan().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ generateTrajectory()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::generateTrajectory ( const Eigen::Vector3f &  pos,
const Eigen::Vector3f &  vel,
float  maxdist,
float  maxangle,
float  maxtime,
float  dt,
std::vector< Eigen::Vector3f > &  outtraj 
)
private

Definition at line 1091 of file backward_local_planner.cpp.

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}
Eigen::Vector3f computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)

References computeNewPositions().

Referenced by computeVelocityCommands().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ isGoalReached()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::isGoalReached ( )

isGoalReached()

Definition at line 858 of file backward_local_planner.cpp.

859{
860 RCLCPP_INFO(nh_->get_logger(), "[BackwardLocalPlanner] isGoalReached call");
861 return goalReached_;
862}

References goalReached_, and nh_.

◆ publishGoalMarker()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::publishGoalMarker ( double  x,
double  y,
double  phi 
)
private

publishGoalMarker()

Definition at line 1188 of file backward_local_planner.cpp.

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}

References costmapRos_, goalMarkerPublisher_, nh_, and service_node_3::s.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ resamplePrecisePlan()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::resamplePrecisePlan ( )
private

Definition at line 929 of file backward_local_planner.cpp.

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}

References backwardsPlanPath_, carrot_angular_distance_, carrot_distance_, and nh_.

Referenced by setPlan().

Here is the caller graph for this function:

◆ resetDivergenceDetection()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::resetDivergenceDetection ( )
private

Definition at line 340 of file backward_local_planner.cpp.

341{
342 // this function should be called always the carrot is updated
343 divergenceDetectionLastCarrotLinearDistance_ = std::numeric_limits<double>::max();
344 return true;
345}

References divergenceDetectionLastCarrotLinearDistance_.

Referenced by setPlan(), and updateCarrotGoal().

Here is the caller graph for this function:

◆ setPlan()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::setPlan ( const nav_msgs::msg::Path &  path)
override

nav2_core setPlan - Sets the global plan

Parameters
pathThe global plan

setPlan()

Definition at line 1019 of file backward_local_planner.cpp.

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}
bool findInitialCarrotGoal(geometry_msgs::msg::PoseStamped &pose)
bool divergenceDetectionUpdate(const geometry_msgs::msg::PoseStamped &pose)

References backwardsPlanPath_, costmapRos_, currentCarrotPoseIndex_, divergenceDetectionUpdate(), findInitialCarrotGoal(), goalReached_, inGoalPureSpinningState_, nh_, planPub_, resamplePrecisePlan(), resetDivergenceDetection(), tf_, and transform_tolerance_.

Here is the call graph for this function:

◆ setSpeedLimit()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::setSpeedLimit ( const double &  speed_limit,
const bool percentage 
)
overridevirtual

Definition at line 231 of file backward_local_planner.cpp.

233{
234 RCLCPP_WARN_STREAM(
235 nh_->get_logger(),
236 "BackwardLocalPlanner::setSpeedLimit invoked. Ignored, funcionality not "
237 "implemented.");
238}

References nh_.

◆ straightBackwardsAndPureSpinCmd()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::straightBackwardsAndPureSpinCmd ( const geometry_msgs::msg::PoseStamped &  pose,
double &  vetta,
double &  gamma,
double  alpha_error,
double  betta_error,
double  rho_error 
)
private

pureSpinningCmd()

Definition at line 448 of file backward_local_planner.cpp.

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}

References k_alpha_, k_betta_, k_rho_, linear_mode_rho_error_threshold_, and yaw_goal_tolerance_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ updateCarrotGoal()

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::updateCarrotGoal ( const geometry_msgs::msg::PoseStamped &  tfpose)
private

updateCarrotGoal()

Definition at line 278 of file backward_local_planner.cpp.

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}

References backwardsPlanPath_, carrot_angular_distance_, carrot_distance_, computeCurrentEuclideanAndAngularErrorsToCarrotGoal(), currentCarrotPoseIndex_, nh_, resetDivergenceDetection(), and xy_goal_tolerance_.

Referenced by computeVelocityCommands().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ updateParameters()

void cl_nav2z::backward_local_planner::BackwardLocalPlanner::updateParameters ( )
private

Definition at line 168 of file backward_local_planner.cpp.

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}
void tryGetOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)

References carrot_angular_distance_, carrot_distance_, enable_obstacle_checking_, k_alpha_, k_betta_, k_rho_, linear_mode_rho_error_threshold_, max_angular_z_speed_, max_linear_x_speed_, name_, nh_, straightBackwardsAndPureSpinningMode_, cl_nav2z::backward_local_planner::tryGetOrSet(), xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by activate(), and computeVelocityCommands().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ alpha_offset_

const double cl_nav2z::backward_local_planner::BackwardLocalPlanner::alpha_offset_ = M_PI
private

Definition at line 142 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ backwardsPlanPath_

std::vector<geometry_msgs::msg::PoseStamped> cl_nav2z::backward_local_planner::BackwardLocalPlanner::backwardsPlanPath_
private

◆ betta_offset_

const double cl_nav2z::backward_local_planner::BackwardLocalPlanner::betta_offset_ = 0
private

Definition at line 143 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ carrot_angular_distance_

rad cl_nav2z::backward_local_planner::BackwardLocalPlanner::carrot_angular_distance_
private

◆ carrot_distance_

meter cl_nav2z::backward_local_planner::BackwardLocalPlanner::carrot_distance_
private

◆ costmapRos_

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> cl_nav2z::backward_local_planner::BackwardLocalPlanner::costmapRos_
private

◆ currentCarrotPoseIndex_

int cl_nav2z::backward_local_planner::BackwardLocalPlanner::currentCarrotPoseIndex_
private

◆ divergenceDetectionLastCarrotLinearDistance_

meter cl_nav2z::backward_local_planner::BackwardLocalPlanner::divergenceDetectionLastCarrotLinearDistance_
private

◆ enable_obstacle_checking_

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::enable_obstacle_checking_ = true
private

◆ goalMarkerPublisher_

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray> > cl_nav2z::backward_local_planner::BackwardLocalPlanner::goalMarkerPublisher_
private

◆ goalReached_

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::goalReached_
private

Definition at line 161 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), isGoalReached(), and setPlan().

◆ inGoalPureSpinningState_

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::inGoalPureSpinningState_ = false
private

Definition at line 165 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), and setPlan().

◆ initialPureSpinningStage_

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::initialPureSpinningStage_
private

Definition at line 162 of file backward_local_planner.hpp.

◆ k_alpha_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::k_alpha_
private

◆ k_betta_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::k_betta_
private

◆ k_rho_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::k_rho_
private

◆ linear_mode_rho_error_threshold_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::linear_mode_rho_error_threshold_
private

◆ max_angular_z_speed_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::max_angular_z_speed_
private

◆ max_linear_x_speed_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::max_linear_x_speed_
private

◆ name_

std::string cl_nav2z::backward_local_planner::BackwardLocalPlanner::name_
private

Definition at line 125 of file backward_local_planner.hpp.

Referenced by configure(), and updateParameters().

◆ nh_

rclcpp_lifecycle::LifecycleNode::SharedPtr cl_nav2z::backward_local_planner::BackwardLocalPlanner::nh_
private

◆ planPub_

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path> > cl_nav2z::backward_local_planner::BackwardLocalPlanner::planPub_
private

Definition at line 133 of file backward_local_planner.hpp.

Referenced by activate(), configure(), deactivate(), and setPlan().

◆ pure_spinning_allowed_betta_error_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::pure_spinning_allowed_betta_error_
private

Definition at line 139 of file backward_local_planner.hpp.

◆ straightBackwardsAndPureSpinningMode_

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::straightBackwardsAndPureSpinningMode_ = false
private

◆ tf_

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::backward_local_planner::BackwardLocalPlanner::tf_
private

Definition at line 129 of file backward_local_planner.hpp.

Referenced by configure(), and setPlan().

◆ transform_tolerance_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::transform_tolerance_
private

Definition at line 155 of file backward_local_planner.hpp.

Referenced by setPlan().

◆ waiting_

bool cl_nav2z::backward_local_planner::BackwardLocalPlanner::waiting_
private

Definition at line 166 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ waitingStamp_

rclcpp::Time cl_nav2z::backward_local_planner::BackwardLocalPlanner::waitingStamp_
private

Definition at line 158 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ waitingTimeout_

rclcpp::Duration cl_nav2z::backward_local_planner::BackwardLocalPlanner::waitingTimeout_
private

Definition at line 157 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), and configure().

◆ xy_goal_tolerance_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::xy_goal_tolerance_
private

◆ yaw_goal_tolerance_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::yaw_goal_tolerance_
private

The documentation for this class was generated from the following files: