cl_nav2z::backward_local_planner::BackwardLocalPlanner Class Reference

#include <backward_local_planner.hpp>

Inheritance diagram for cl_nav2z::backward_local_planner::BackwardLocalPlanner:

Collaboration diagram for cl_nav2z::backward_local_planner::BackwardLocalPlanner:

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()

Definition at line 46 of file backward_local_planner.cpp.

: waitingTimeout_(0s) {}

~BackwardLocalPlanner()

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

virtual

~BackwardLocalPlanner()

Definition at line 53 of file backward_local_planner.cpp.

{}

Member Function Documentation

activate()

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

override

Definition at line 55 of file backward_local_planner.cpp.

{
  RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller BackwardLocalPlanner");
  updateParameters();
 
  goalMarkerPublisher_->on_activate();
  planPub_->on_activate();
  backwardsPlanPath_.clear();
}

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.

{
  // this function is specially useful when we want to reach the goal with a lot
  // of precision. We may pass the goal and then the controller enters in some
  // unstable state. With this, we are able to detect when stop moving.
 
  // only apply if the carrot is in goal position and also if we are not in a pure spinning behavior v!=0
 
  auto & carrot_pose = backwardsPlanPath_[currentCarrotPoseIndex_];
  const geometry_msgs::msg::Point & carrot_point = carrot_pose.pose.position;
  double yaw = tf2::getYaw(carrot_pose.pose.orientation);
 
  // direction vector
  double vx = cos(yaw);
  double vy = sin(yaw);
 
  // line implicit equation
  // ax + by + c = 0
  double c = -vx * carrot_point.x - vy * carrot_point.y;
  const double C_OFFSET_METERS = 0.05;  // 5 cm
  double check = vx * tfpose.pose.position.x + vy * tfpose.pose.position.y + c + C_OFFSET_METERS;
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    "[BackwardLocalPlanner] half plane constraint:" << vx << "*" << carrot_point.x << " + " << vy
                                                    << "*" << carrot_point.y << " + " << c);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] constraint evaluation: "
                         << vx << "*" << tfpose.pose.position.x << " + " << vy << "*"
                         << tfpose.pose.position.y << " + " << c << " = " << check);
 
  return check < 0;
}

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.

{
  auto & finalgoal = backwardsPlanPath_.back();
  double gdx = finalgoal.pose.position.x - tfpose.pose.position.x;
  double gdy = finalgoal.pose.position.y - tfpose.pose.position.y;
  double goaldist = sqrt(gdx * gdx + gdy * gdy);
 
  auto abs_angle_error = fabs(angle_error);
  // final_alpha_error =
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] goal check. linear dist: "
                         << goaldist << "(" << this->xy_goal_tolerance_ << ")"
                         << ", angular dist: " << abs_angle_error << "("
                         << this->yaw_goal_tolerance_ << ")");
 
  linearGoalReached = goaldist < this->xy_goal_tolerance_;
 
  return linearGoalReached && abs_angle_error < this->yaw_goal_tolerance_;
  // return goal_checker->isGoalReached(tfpose.pose, finalgoal.pose, currentTwist);
}

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.

{
  this->clearMarkers();
  RCLCPP_WARN_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] cleanup");
  this->backwardsPlanPath_.clear();
  this->currentCarrotPoseIndex_ = 0;
}

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.

{
  visualization_msgs::msg::Marker marker;
  marker.header.frame_id = this->costmapRos_->getGlobalFrameID();
  marker.header.stamp = nh_->now();
 
  marker.ns = "my_namespace2";
  marker.id = 0;
  marker.type = visualization_msgs::msg::Marker::ARROW;
  marker.action = visualization_msgs::msg::Marker::DELETEALL;
 
  visualization_msgs::msg::MarkerArray ma;
  ma.markers.push_back(marker);
 
  goalMarkerPublisher_->publish(ma);
}

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.

{
  double angle = tf2::getYaw(tfpose.pose.orientation);
  auto & carrot_pose = backwardsPlanPath_[currentCarrotPoseIndex_];
  const geometry_msgs::msg::Point & carrot_point = carrot_pose.pose.position;
 
  tf2::Quaternion carrot_orientation;
  tf2::convert(carrot_pose.pose.orientation, carrot_orientation);
  geometry_msgs::msg::Pose currentPoseDebugMsg = tfpose.pose;
 
  // take error from the current position to the path point
  double dx = carrot_point.x - tfpose.pose.position.x;
  double dy = carrot_point.y - tfpose.pose.position.y;
 
  dist = sqrt(dx * dx + dy * dy);
 
  double pangle = tf2::getYaw(carrot_orientation);
  angular_error = fabs(angles::shortest_angular_distance(pangle, angle));
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] Compute carrot errors from current pose. (linear "
                         << dist << ")(angular " << angular_error << ")" << std::endl
                         << "Current carrot pose: " << std::endl
                         << carrot_pose << std::endl
                         << "Current actual pose:" << std::endl
                         << currentPoseDebugMsg);
}

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.

{
  Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
  new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
  new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
  new_pos[2] = pos[2] + vel[2] * dt;
  return new_pos;
}

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

pose	Current robot pose
velocity	Current robot velocity

Returns

The best command for the robot to drive

computeVelocityCommands()

Definition at line 469 of file backward_local_planner.cpp.

{
  RCLCPP_INFO(
    nh_->get_logger(),
    "[BackwardLocalPlanner] ------------------- LOCAL PLANNER LOOP -----------------");
  this->updateParameters();
 
  // consistency check
  if (this->backwardsPlanPath_.size() > 0)
  {
    RCLCPP_INFO_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] Current pose frame id: "
                           << backwardsPlanPath_.front().header.frame_id
                           << ", path pose frame id: " << pose.header.frame_id);
 
    if (backwardsPlanPath_.front().header.frame_id != pose.header.frame_id)
    {
      RCLCPP_ERROR_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] Inconsistent frames");
    }
  }
 
  // xy_goal_tolerance and yaw_goal_tolerance are just used for logging proposes and clamping the carrot
  // goal distance (parameter safety)
  if (xy_goal_tolerance_ == -1 || yaw_goal_tolerance_ == -1)
  {
    geometry_msgs::msg::Pose posetol;
    geometry_msgs::msg::Twist twistol;
    if (goal_checker->getTolerances(posetol, twistol))
    {
      xy_goal_tolerance_ = posetol.position.x;
      yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation);
      //xy_goal_tolerance_ = posetol.position.x * 0.35;  // WORKAROUND ISSUE GOAL CHECKER NAV_CONTROLLER DIFF
      //yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation) * 0.35;
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[BackwardLocalPlanner] xy_goal_tolerance_: "
                             << xy_goal_tolerance_
                             << ", yaw_goal_tolerance_: " << yaw_goal_tolerance_);
    }
    else
    {
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[BackwardLocalPlanner] could not get tolerances from goal checker");
    }
  }
 
  RCLCPP_INFO(
    nh_->get_logger(),
    "[BackwardLocalPlanner] ------------------- LOCAL PLANNER LOOP -----------------");
 
  geometry_msgs::msg::TwistStamped cmd_vel;
  RCLCPP_INFO(nh_->get_logger(), "[BackwardLocalPlanner] LOCAL PLANNER LOOP");
  geometry_msgs::msg::PoseStamped paux;
  geometry_msgs::msg::PoseStamped tfpose;
 
  if (!costmapRos_->getRobotPose(tfpose))
  {
    RCLCPP_ERROR(
      nh_->get_logger(),
      "[BackwardLocalPlanner] missing robot pose, canceling compute Velocity Command");
  }  // it is not working in the pure spinning reel example, maybe the hyperplane check is enough
  bool divergenceDetected = false;
 
  bool emergency_stop = false;
  if (divergenceDetected)
  {
    RCLCPP_ERROR(
      nh_->get_logger(), "[BackwardLocalPlanner] Divergence detected. Sending emergency stop.");
    emergency_stop = true;
  }
 
  bool carrotInLinearGoalRange = updateCarrotGoal(tfpose);
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] carrot goal created");
 
  if (emergency_stop)
  {
    cmd_vel.twist.linear.x = 0;
    cmd_vel.twist.angular.z = 0;
    RCLCPP_INFO_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] emergency stop, exit compute commands");
    // return false;
    return cmd_vel;
  }
 
  // ------ Evaluate the current context ----
  double rho_error, betta_error, alpha_error;
 
  // getting carrot goal information
  tf2::Quaternion q;
  tf2::convert(tfpose.pose.orientation, q);
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] carrot goal: " << currentCarrotPoseIndex_ << "/"
                                                              << backwardsPlanPath_.size());
  const geometry_msgs::msg::PoseStamped & carrotgoalpose =
    backwardsPlanPath_[currentCarrotPoseIndex_];
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] carrot goal pose current index: "
                         << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size() << ": "
                         << carrotgoalpose);
  const geometry_msgs::msg::Point & carrotGoalPosition = carrotgoalpose.pose.position;
 
  tf2::Quaternion goalQ;
  tf2::fromMsg(carrotgoalpose.pose.orientation, goalQ);
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] -- Control Policy --");
  // goal orientation (global frame)
  double betta = tf2::getYaw(goalQ);
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] goal orientation: " << betta);
  betta = betta + betta_offset_;
 
  double dx = carrotGoalPosition.x - tfpose.pose.position.x;
  double dy = carrotGoalPosition.y - tfpose.pose.position.y;
 
  // distance error to the targetpoint
  rho_error = sqrt(dx * dx + dy * dy);
 
  // heading to goal angle
  double theta = tf2::getYaw(q);
  double alpha = atan2(dy, dx);
  alpha = alpha + alpha_offset_;
 
  alpha_error = angles::shortest_angular_distance(alpha, theta);
  betta_error = angles::shortest_angular_distance(betta, theta);
  //------------- END CONTEXT EVAL ----------
 
  bool linearGoalReached;
  bool currentPoseInGoal =
    checkCurrentPoseInGoalRange(tfpose, velocity, betta_error, linearGoalReached, goal_checker);
 
  // Make sure the robot is very close to the goal and it is really in the the last goal point.
  bool carrotInFinalGoalIndex = currentCarrotPoseIndex_ == (int)backwardsPlanPath_.size() - 1;
 
  // checking if we are really in the end goal pose
  if (currentPoseInGoal && carrotInFinalGoalIndex)
  {
    goalReached_ = true;
    // backwardsPlanPath_.clear();
    RCLCPP_INFO_STREAM(
      nh_->get_logger(),
      "[BackwardLocalPlanner] GOAL REACHED. Send stop command and skipping trajectory collision: "
        << cmd_vel.twist);
    cmd_vel.twist.linear.x = 0;
    cmd_vel.twist.angular.z = 0;
    return cmd_vel;
  }
  else if (
    carrotInLinearGoalRange &&
    linearGoalReached)  // checking if we are in the end goal point but with incorrect
                        // orientation
  {
    // this means that we are not in the final angular distance, and we may even not be in the last carrot index
    // (several intermediate angular poses until the last goal pose)
    inGoalPureSpinningState_ = true;
  }
 
  // --------------------
  double vetta, gamma;
  if (straightBackwardsAndPureSpinningMode_)
  {
    // decorated control rule for this mode
    this->straightBackwardsAndPureSpinCmd(
      tfpose, vetta, gamma, alpha_error, betta_error, rho_error);
  }
  else  // default free navigation backward motion mode
  {
    // regular control rule
    vetta = k_rho_ * rho_error;
    gamma = k_alpha_ * alpha_error + k_betta_ * betta_error;
 
    // Even if we are in free navigation, we can enter in the pure spinning state.
    // then, the linear motion is deactivated.
    if (inGoalPureSpinningState_)
    {
      RCLCPP_INFO(
        nh_->get_logger(),
        "[BackwardLocalPlanner] we entered in a pure spinning state even in not pure-spining "
        "configuration, "
        "carrotDistanceGoalReached: %d",
        carrotInLinearGoalRange);
      gamma = k_betta_ * betta_error;
      vetta = 0;
    }
 
    // classical control to reach a goal backwards
  }
 
  // Apply command and Clamp to limits
  cmd_vel.twist.linear.x = vetta;
  cmd_vel.twist.angular.z = gamma;
 
  if (cmd_vel.twist.linear.x > max_linear_x_speed_)
  {
    cmd_vel.twist.linear.x = max_linear_x_speed_;
  }
  else if (cmd_vel.twist.linear.x < -max_linear_x_speed_)
  {
    cmd_vel.twist.linear.x = -max_linear_x_speed_;
  }
 
  if (cmd_vel.twist.angular.z > max_angular_z_speed_)
  {
    cmd_vel.twist.angular.z = max_angular_z_speed_;
  }
  else if (cmd_vel.twist.angular.z < -max_angular_z_speed_)
  {
    cmd_vel.twist.angular.z = -max_angular_z_speed_;
  }
 
  publishGoalMarker(carrotGoalPosition.x, carrotGoalPosition.y, betta);
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] local planner,"
                         << std::endl
                         << " current pose in goal: " << currentPoseInGoal << std::endl
                         << " carrot in final goal index: " << carrotInFinalGoalIndex << std::endl
                         << " carrot in linear goal range: " << carrotInLinearGoalRange << std::endl
                         << " straightAnPureSpiningMode: " << straightBackwardsAndPureSpinningMode_
                         << std::endl
                         << " inGoalPureSpinningState: " << inGoalPureSpinningState_ << std::endl
                         << " theta: " << theta << std::endl
                         << " betta: " << theta << std::endl
                         << " err_x: " << dx << std::endl
                         << " err_y:" << dy << std::endl
                         << " rho_error:" << rho_error << std::endl
                         << " alpha_error:" << alpha_error << std::endl
                         << " betta_error:" << betta_error << std::endl
                         << " vetta:" << vetta << std::endl
                         << " gamma:" << gamma << std::endl
                         << " cmd_vel.lin.x:" << cmd_vel.twist.linear.x << std::endl
                         << " cmd_vel.ang.z:" << cmd_vel.twist.angular.z);
 
  if (inGoalPureSpinningState_)
  {
    bool carrotHalfPlaneConstraintFailure = checkCarrotHalfPlainConstraint(tfpose);
 
    if (carrotHalfPlaneConstraintFailure)
    {
      RCLCPP_ERROR(
        nh_->get_logger(),
        "[BackwardLocalPlanner] CarrotHalfPlaneConstraintFailure detected. Sending "
        "emergency stop and success to the planner.");
      cmd_vel.twist.linear.x = 0;
    }
  }
 
  // ---------------------- TRAJECTORY PREDICTION AND COLLISION AVOIDANCE ---------------------
  // cmd_vel.twist.linear.x=0;
  // cmd_vel.twist.angular.z = 0;
 
  geometry_msgs::msg::PoseStamped global_pose;
  costmapRos_->getRobotPose(global_pose);
 
  auto * costmap2d = costmapRos_->getCostmap();
  auto yaw = tf2::getYaw(global_pose.pose.orientation);
 
  auto & pos = global_pose.pose.position;
 
  Eigen::Vector3f currentpose(pos.x, pos.y, yaw);
  Eigen::Vector3f currentvel(
    cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z);
  std::vector<Eigen::Vector3f> trajectory;
  this->generateTrajectory(
    currentpose, currentvel, 0.8 /*meters*/, M_PI / 8 /*rads*/, 3.0 /*seconds*/, 0.05 /*seconds*/,
    trajectory);
 
  // check plan rejection
  bool acceptedLocalTrajectoryFreeOfObstacles = true;
 
  unsigned int mx, my;
 
  if (this->enable_obstacle_checking_)
  {
    if (backwardsPlanPath_.size() > 0)
    {
      auto & finalgoalpose = backwardsPlanPath_.back();
 
      int i = 0;
      // RCLCPP_INFO_STREAM(nh_->get_logger(), "lplanner goal: " << finalgoalpose.pose.position);
      geometry_msgs::msg::Twist mockzerospeed;
 
      for (auto & p : trajectory)
      {
        /*geometry_msgs::msg::Pose pg;
        pg.position.x = p[0];
        pg.position.y = p[1];
        tf2::Quaternion q;
        q.setRPY(0, 0, p[2]);
        pg.orientation = tf2::toMsg(q);
 
        // WARNING I CAN'T USE isGoalReached because I can change the state of a stateful goal checker
        if (goal_checker->isGoalReached(pg, finalgoalpose.pose, mockzerospeed))*/
 
        float dx = p[0] - finalgoalpose.pose.position.x;
        float dy = p[1] - finalgoalpose.pose.position.y;
 
        float dst = sqrt(dx * dx + dy * dy);
        if (dst < xy_goal_tolerance_)
        {
          RCLCPP_INFO(
            nh_->get_logger(),
            "[BackwardLocalPlanner] trajectory simulation for collision checking: goal "
            "reached with no collision");
          break;
        }
 
        costmap2d->worldToMap(p[0], p[1], mx, my);
        //         unsigned int cost = costmap2d->getCost(mx, my);
 
        // RCLCPP_INFO(nh_->get_logger(),"[BackwardLocalPlanner] checking cost pt %d [%lf, %lf] cell[%d,%d] = %d", i,
        // p[0], p[1], mx, my, cost); RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] cost: " << cost);
 
        // static const unsigned char NO_INFORMATION = 255;
        // static const unsigned char LETHAL_OBSTACLE = 254;
        // static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
        // static const unsigned char FREE_SPACE = 0;
 
        if (costmap2d->getCost(mx, my) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
        {
          acceptedLocalTrajectoryFreeOfObstacles = false;
          RCLCPP_WARN_STREAM(
            nh_->get_logger(),
            "[BackwardLocalPlanner] ABORTED LOCAL PLAN BECAUSE OBSTACLE DETEDTED at point "
              << i << "/" << trajectory.size() << std::endl
              << p[0] << ", " << p[1]);
          break;
        }
        i++;
      }
    }
    else
    {
      RCLCPP_WARN(
        nh_->get_logger(), "[BackwardLocalPlanner] Abort local - Backwards global plan size: %ld",
        backwardsPlanPath_.size());
      cmd_vel.twist.angular.z = 0;
      cmd_vel.twist.linear.x = 0;
      // return false;
    }
  }
 
  if (acceptedLocalTrajectoryFreeOfObstacles)
  {
    waiting_ = false;
    RCLCPP_INFO(
      nh_->get_logger(),
      "[BackwardLocalPlanner] accepted local trajectory free of obstacle. Local planner "
      "continues.");
    return cmd_vel;
    // return true;
  }
  else  // that is not appceted because existence of obstacles
  {
    // emergency stop for collision: waiting a while before sending error
    cmd_vel.twist.linear.x = 0;
    cmd_vel.twist.angular.z = 0;
 
    if (waiting_ == false)
    {
      waiting_ = true;
      waitingStamp_ = nh_->now();
      RCLCPP_WARN(
        nh_->get_logger(), "[BackwardLocalPlanner][Not accepted local plan] starting countdown");
    }
    else
    {
      auto waitingduration = nh_->now() - waitingStamp_;
 
      if (waitingduration > this->waitingTimeout_)
      {
        RCLCPP_WARN(
          nh_->get_logger(), "[BackwardLocalPlanner][Abort local] timeout! duration %lf/%f",
          waitingduration.seconds(), waitingTimeout_.seconds());
        // return false;
        cmd_vel.twist.linear.x = 0;
        cmd_vel.twist.angular.z = 0;
        return cmd_vel;
      }
    }
 
    return cmd_vel;
  }
}

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.

{
  this->costmapRos_ = costmap_ros;
  // rclcpp::Node::SharedPtr nh("~/BackwardLocalPlanner");
  this->nh_ = parent.lock();
  this->name_ = name;
  this->tf_ = tf;
 
  k_rho_ = -1.0;
  k_alpha_ = 0.5;
  k_betta_ = -1.0;  // set to zero means that orientation is not important
  carrot_angular_distance_ = 0.4;
  linear_mode_rho_error_threshold_ = 0.02;
  straightBackwardsAndPureSpinningMode_ = true;
  max_linear_x_speed_ = 1.0;
  max_angular_z_speed_ = 2.0;
  yaw_goal_tolerance_ = -1;
  xy_goal_tolerance_ = -1;
  waitingTimeout_ = rclcpp::Duration(10s);
 
  this->currentCarrotPoseIndex_ = 0;
 
  declareOrSet(
    nh_, name_ + ".pure_spinning_straight_line_mode", straightBackwardsAndPureSpinningMode_);
 
  declareOrSet(nh_, name_ + ".k_rho", k_rho_);
  declareOrSet(nh_, name_ + ".k_alpha", k_alpha_);
  declareOrSet(nh_, name_ + ".k_betta", k_betta_);
  declareOrSet(nh_, name_ + ".linear_mode_rho_error_threshold", linear_mode_rho_error_threshold_);
 
  declareOrSet(nh_, name_ + ".carrot_distance", carrot_distance_);
  declareOrSet(nh_, name_ + ".carrot_angular_distance", carrot_angular_distance_);
  declareOrSet(nh_, name_ + ".enable_obstacle_checking", enable_obstacle_checking_);
 
  declareOrSet(nh_, name_ + ".max_linear_x_speed", max_linear_x_speed_);
  declareOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
 
  // we have to do this, for example for the case we are refining the final orientation.
  // check at some point if the carrot is reached in "goal linear distance", then we go into
  // some automatic pure-spinning mode where we only update the orientation
  // This means that if we reach the carrot with precision we go into pure spinning mode but we cannot
  // leave that point (maybe this could be improved)
 
  if (yaw_goal_tolerance_ != -1 && carrot_angular_distance_ < yaw_goal_tolerance_)
  {
    RCLCPP_WARN_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] carrot_angular_distance ("
                           << carrot_angular_distance_
                           << ") cannot be lower than yaw_goal_tolerance (" << yaw_goal_tolerance_
                           << ") setting carrot_angular_distance = " << yaw_goal_tolerance_);
    carrot_angular_distance_ = yaw_goal_tolerance_;
  }
 
  if (xy_goal_tolerance_ != -1 && carrot_distance_ < xy_goal_tolerance_)
  {
    RCLCPP_WARN_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] carrot_linear_distance ("
                           << carrot_distance_ << ") cannot be lower than xy_goal_tolerance_ ("
                           << yaw_goal_tolerance_
                           << ") setting carrot_angular_distance = " << xy_goal_tolerance_);
    carrot_distance_ = xy_goal_tolerance_;
  }
 
  goalMarkerPublisher_ = nh_->create_publisher<visualization_msgs::msg::MarkerArray>(
    "backward_local_planner/goal_marker", 1);
 
  planPub_ = nh_->create_publisher<nav_msgs::msg::Path>("backward_local_planner/path", 1);
}

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.

{
  this->clearMarkers();
  RCLCPP_WARN_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] deactivated");
  planPub_->on_deactivate();
  goalMarkerPublisher_->on_deactivate();
}

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.

{
  double disterr = 0, angleerr = 0;
  computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, disterr, angleerr);
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] Divergence check. carrot goal distance. was: "
                         << divergenceDetectionLastCarrotLinearDistance_
                         << ", now it is: " << disterr);
  if (disterr > divergenceDetectionLastCarrotLinearDistance_)
  {
    // candidate of divergence, we do not throw the divergence alarm yet
    // but we neither update the distance since it is worse than the one
    // we had previously with the same carrot.
    const double MARGIN_FACTOR = 1.2;
    if (disterr > MARGIN_FACTOR * divergenceDetectionLastCarrotLinearDistance_)
    {
      RCLCPP_ERROR_STREAM(
        nh_->get_logger(),
        "[BackwardLocalPlanner] Divergence detected. The same carrot goal distance was previously: "
          << divergenceDetectionLastCarrotLinearDistance_ << "but now it is: " << disterr);
      return true;
    }
    else
    {
      // divergence candidate
      return false;
    }
  }
  else
  {
    // update:
    divergenceDetectionLastCarrotLinearDistance_ = disterr;
    return false;
  }
}

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.

{
  double lineardisterr, angleerr;
  bool inCarrotRange = false;
 
  // initial state check
  computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, lineardisterr, angleerr);
 
  // double minpointdist = std::numeric_limits<double>::max();
 
  // lets set the carrot-goal in the correct place with this loop
  while (currentCarrotPoseIndex_ < (int)backwardsPlanPath_.size() && !inCarrotRange)
  {
    computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, lineardisterr, angleerr);
 
    RCLCPP_INFO(
      nh_->get_logger(),
      "[BackwardLocalPlanner] Finding initial carrot goal i=%d - error to carrot, linear = %lf "
      "(%lf), "
      "angular : %lf (%lf)",
      currentCarrotPoseIndex_, lineardisterr, carrot_distance_, angleerr, carrot_angular_distance_);
 
    // current path point is inside the carrot distance range, goal carrot tries to escape!
    if (lineardisterr < carrot_distance_ && angleerr < carrot_angular_distance_)
    {
      RCLCPP_INFO(
        nh_->get_logger(),
        "[BackwardLocalPlanner] Finding initial carrot goal i=%d - in carrot Range",
        currentCarrotPoseIndex_);
      inCarrotRange = true;
      // we are inside the goal range
    }
    else if (
      inCarrotRange && (lineardisterr > carrot_distance_ || angleerr > carrot_angular_distance_))
    {
      // we were inside the carrot range but not anymore, now we are just leaving. we want to continue forward
      // (currentCarrotPoseIndex_++) unless we go out of the carrot range
 
      // but we rollback last index increment (to go back inside the carrot goal scope) and start motion with that
      // carrot goal we found
      currentCarrotPoseIndex_--;
      break;
    }
    else
    {
      RCLCPP_INFO(
        nh_->get_logger(),
        "[BackwardLocalPlanner] Finding initial carrot goal i=%d - carrot out of range, searching "
        "coincidence...",
        currentCarrotPoseIndex_);
    }
 
    currentCarrotPoseIndex_++;
    RCLCPP_INFO_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] setPlan: fw" << currentCarrotPoseIndex_);
  }
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] setPlan: (found first carrot:"
                         << inCarrotRange << ") initial carrot point index: "
                         << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
 
  return inCarrotRange;
}

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.

{
  // simulate the trajectory and check for collisions, updating costs along the way
  bool end = false;
  float time = 0;
  Eigen::Vector3f currentpos = pos;
  int i = 0;
  while (!end)
  {
    // add the point to the trajectory so we can draw it later if we want
    // traj.addPoint(pos[0], pos[1], pos[2]);
 
    // if (continued_acceleration_) {
    //   //calculate velocities
    //   loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
    //   //RCLCPP_WARN_NAMED(nh_->get_logger(), "Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_,
    //   loop_vel[0], loop_vel[1], loop_vel[2]);
    // }
 
    auto loop_vel = vel;
    // update the position of the robot using the velocities passed in
    auto newpos = computeNewPositions(currentpos, loop_vel, dt);
 
    auto dx = newpos[0] - currentpos[0];
    auto dy = newpos[1] - currentpos[1];
    float dist, angledist;
 
    // RCLCPP_INFO(nh_->get_logger(), "traj point %d", i);
    dist = sqrt(dx * dx + dy * dy);
    if (dist > maxdist)
    {
      end = true;
      // RCLCPP_INFO(nh_->get_logger(), "dist break: %f", dist);
    }
    else
    {
      // ouble from, double to
      angledist = angles::shortest_angular_distance(currentpos[2], newpos[2]);
      if (angledist > maxanglediff)
      {
        end = true;
        // RCLCPP_INFO(nh_->get_logger(), "angle dist break: %f", angledist);
      }
      else
      {
        outtraj.push_back(newpos);
 
        time += dt;
        if (time > maxtime)
        {
          end = true;
          // RCLCPP_INFO(nh_->get_logger(), "time break: %f", time);
        }
 
        // RCLCPP_INFO(nh_->get_logger(), "dist: %f, angledist: %f, time: %f", dist, angledist, time);
      }
    }
 
    currentpos = newpos;
    i++;
  }  // end for simulation steps
}

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.

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

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.

{
  visualization_msgs::msg::Marker marker;
  marker.header.frame_id = this->costmapRos_->getGlobalFrameID();
  marker.header.stamp = nh_->now();
 
  marker.ns = "my_namespace2";
  marker.id = 0;
  marker.type = visualization_msgs::msg::Marker::ARROW;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.lifetime = rclcpp::Duration(1.0s);
 
  marker.pose.orientation.w = 1;
 
  marker.scale.x = 0.05;
  marker.scale.y = 0.15;
  marker.scale.z = 0.05;
  marker.color.a = 1.0;
 
  // red marker
  marker.color.r = 1;
  marker.color.g = 0;
  marker.color.b = 0;
 
  geometry_msgs::msg::Point start, end;
  start.x = x;
  start.y = y;
 
  end.x = x + 0.5 * cos(phi);
  end.y = y + 0.5 * sin(phi);
 
  marker.points.push_back(start);
  marker.points.push_back(end);
 
  visualization_msgs::msg::MarkerArray ma;
  ma.markers.push_back(marker);
 
  goalMarkerPublisher_->publish(ma);
}

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.

{
  // this algorithm is really important to have a precise carrot (linear or angular)
  // and not being considered as a divergence from the path
 
  RCLCPP_INFO(nh_->get_logger(), "[BackwardLocalPlanner] resample precise");
  if (backwardsPlanPath_.size() <= 1)
  {
    RCLCPP_INFO_STREAM(
      nh_->get_logger(),
      "[BackwardLocalPlanner] resample precise skipping, size: " << backwardsPlanPath_.size());
    return false;
  }
 
  int counter = 0;
  double maxallowedAngularError = 0.45 * this->carrot_angular_distance_;  // nyquist
  double maxallowedLinearError = 0.45 * this->carrot_distance_;           // nyquist
 
  for (int i = 0; i < (int)backwardsPlanPath_.size() - 1; i++)
  {
    RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] resample precise, check: " << i);
    auto & currpose = backwardsPlanPath_[i];
    auto & nextpose = backwardsPlanPath_[i + 1];
 
    tf2::Quaternion qCurrent, qNext;
    tf2::convert(currpose.pose.orientation, qCurrent);
    tf2::convert(nextpose.pose.orientation, qNext);
 
    double dx = nextpose.pose.position.x - currpose.pose.position.x;
    double dy = nextpose.pose.position.y - currpose.pose.position.y;
    double dist = sqrt(dx * dx + dy * dy);
 
    bool resample = false;
    if (dist > maxallowedLinearError)
    {
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[BackwardLocalPlanner] resampling point, linear distance:"
                             << dist << "(" << maxallowedLinearError << ")" << i);
      resample = true;
    }
    else
    {
      double currentAngle = tf2::getYaw(qCurrent);
      double nextAngle = tf2::getYaw(qNext);
 
      double angularError = fabs(angles::shortest_angular_distance(currentAngle, nextAngle));
      if (angularError > maxallowedAngularError)
      {
        resample = true;
        RCLCPP_INFO_STREAM(
          nh_->get_logger(), "[BackwardLocalPlanner] resampling point, angular distance:"
                               << angularError << "(" << maxallowedAngularError << ")" << i);
      }
    }
 
    if (resample)
    {
      geometry_msgs::msg::PoseStamped pintermediate;
      auto duration = rclcpp::Time(nextpose.header.stamp) - rclcpp::Time(currpose.header.stamp);
 
      pintermediate.header.frame_id = currpose.header.frame_id;
      pintermediate.header.stamp = rclcpp::Time(currpose.header.stamp) + duration * 0.5;
 
      pintermediate.pose.position.x = 0.5 * (currpose.pose.position.x + nextpose.pose.position.x);
      pintermediate.pose.position.y = 0.5 * (currpose.pose.position.y + nextpose.pose.position.y);
      pintermediate.pose.position.z = 0.5 * (currpose.pose.position.z + nextpose.pose.position.z);
      tf2::Quaternion intermediateQuat = tf2::slerp(qCurrent, qNext, 0.5);
      pintermediate.pose.orientation = tf2::toMsg(intermediateQuat);
 
      this->backwardsPlanPath_.insert(this->backwardsPlanPath_.begin() + i + 1, pintermediate);
 
      // retry this point
      i--;
      counter++;
    }
  }
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[BackwardLocalPlanner] End resampling. resampled:" << counter
                                                                           << " new inserted poses "
                                                                              "during precise "
                                                                              "resmapling.");
  return true;
}

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.

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

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

path	The global plan

setPlan()

Definition at line 1019 of file backward_local_planner.cpp.

{
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    "[BackwardLocalPlanner] setPlan: new global plan received ( " << path.poses.size() << ")");
 
  //------------- TRANSFORM TO LOCAL FRAME PATH ---------------------------
  nav_msgs::msg::Path transformedPlan;
  rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
  // transform global plan to the navigation reference frame
  for (auto & p : path.poses)
  {
    geometry_msgs::msg::PoseStamped transformedPose;
    nav_2d_utils::transformPose(tf_, costmapRos_->getGlobalFrameID(), p, transformedPose, ttol);
    transformedPose.header.frame_id = costmapRos_->getGlobalFrameID();
    transformedPlan.poses.push_back(transformedPose);
  }
 
  backwardsPlanPath_ = transformedPlan.poses;
 
  // --------- resampling path feature -----------
  geometry_msgs::msg::PoseStamped tfpose;
  if (!costmapRos_->getRobotPose(tfpose))
  {
    RCLCPP_ERROR(nh_->get_logger(), "Failure getting pose from Backward local planner");
    return;
  }
 
  geometry_msgs::msg::PoseStamped posestamped = tfpose;
  backwardsPlanPath_.insert(backwardsPlanPath_.begin(), posestamped);
  this->resamplePrecisePlan();
 
  nav_msgs::msg::Path planMsg;
  planMsg.poses = backwardsPlanPath_;
  planMsg.header.frame_id = costmapRos_->getGlobalFrameID();
  planMsg.header.stamp = nh_->now();
  planPub_->publish(planMsg);
 
  // ------ reset controller state ----------------------
  goalReached_ = false;
  inGoalPureSpinningState_ = false;
  currentCarrotPoseIndex_ = 0;
  this->resetDivergenceDetection();
 
  if (path.poses.size() == 0)
  {
    RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardLocalPlanner] received plan without any pose");
    // return true;
    return;
  }
 
  // -------- initialize carrot ----------------
  bool foundInitialCarrotGoal = this->findInitialCarrotGoal(tfpose);
  if (!foundInitialCarrotGoal)
  {
    RCLCPP_ERROR(
      nh_->get_logger(),
      "[BackwardLocalPlanner] new plan rejected. The initial point in the global path is "
      "too much far away from the current state (according to carrot_distance "
      "parameter)");
    // return false; // in this case, the new plan broke the current execution
    return;
  }
  else
  {
    this->divergenceDetectionUpdate(tfpose);
    // SANDARD AND PREFERRED CASE ON NEW PLAN
    // return true;
    return;
  }
}

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.

{
  RCLCPP_WARN_STREAM(
    nh_->get_logger(),
    "BackwardLocalPlanner::setSpeedLimit invoked. Ignored, funcionality not "
    "implemented.");
}

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.

{
  if (rho_error > linear_mode_rho_error_threshold_)  // works in straight motion mode
  {
    vetta = k_rho_ * rho_error;
    gamma = k_alpha_ * alpha_error;
  }
  else if (fabs(betta_error) >= this->yaw_goal_tolerance_)  // works in pure spinning mode
  {
    vetta = 0;  // disable linear
    gamma = k_betta_ * betta_error;
  }
}

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.

{
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardsLocalPlanner] --- Carrot update ---");
  double disterr = 0, angleerr = 0;
  // iterate the point from the current position and backward until reaching a new goal point in the path
  // this algorithm among other advantages has that skip the looping with an eager global planner
  // that recalls the same plan (the already performed part of the plan in the current pose is skipped)
  while (currentCarrotPoseIndex_ < (long)backwardsPlanPath_.size() - 1)
  {
    computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, disterr, angleerr);
 
    RCLCPP_INFO_STREAM(
      nh_->get_logger(), "[BackwardsLocalPlanner] update carrot goal: Current index: "
                           << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
    RCLCPP_INFO(
      nh_->get_logger(),
      "[BackwardsLocalPlanner] update carrot goal: linear error %lf, angular error: %lf", disterr,
      angleerr);
 
    // target pose found, goal carrot tries to escape!
    if (disterr < carrot_distance_ && angleerr < carrot_angular_distance_)
    {
      currentCarrotPoseIndex_++;
      resetDivergenceDetection();
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[BackwardsLocalPlanner] move carrot fw "
                             << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
    }
    else
    {
      // carrot already escaped
      break;
    }
  }
  // RCLCPP_INFO(nh_->get_logger(),"[BackwardsLocalPlanner] computing angular error");
  if (
    currentCarrotPoseIndex_ >= (long)backwardsPlanPath_.size() - 1 && backwardsPlanPath_.size() > 0)
  {
    currentCarrotPoseIndex_ = backwardsPlanPath_.size() - 1;
    // reupdated errors
    computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, disterr, angleerr);
  }
 
  RCLCPP_INFO(
    nh_->get_logger(), "[BackwardsLocalPlanner] Current index carrot goal: %d",
    currentCarrotPoseIndex_);
  RCLCPP_INFO(
    nh_->get_logger(),
    "[BackwardsLocalPlanner] Update carrot goal: linear error  %lf (xytol: %lf), angular error: "
    "%lf",
    disterr, xy_goal_tolerance_, angleerr);
 
  bool carrotInGoalLinearRange = disterr < xy_goal_tolerance_;
  RCLCPP_INFO(
    nh_->get_logger(), "[BackwardsLocalPlanner] carrot in goal radius: %d",
    carrotInGoalLinearRange);
 
  RCLCPP_INFO(nh_->get_logger(), "[BackwardsLocalPlanner] ---End carrot update---");
 
  return carrotInGoalLinearRange;
}

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.

{
  RCLCPP_INFO_STREAM(nh_->get_logger(), "--- parameters ---");
  tryGetOrSet(nh_, name_ + ".k_rho", k_rho_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".k_rho:" << k_rho_);
  tryGetOrSet(nh_, name_ + ".k_alpha", k_alpha_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".k_alpha:" << k_alpha_);
  tryGetOrSet(nh_, name_ + ".k_betta", k_betta_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".k_betta:" << k_betta_);
 
  tryGetOrSet(nh_, name_ + ".enable_obstacle_checking", enable_obstacle_checking_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), name_ + ".enable_obstacle_checking: " << enable_obstacle_checking_);
 
  tryGetOrSet(nh_, name_ + ".carrot_distance", carrot_distance_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".carrot_distance:" << carrot_distance_);
  tryGetOrSet(nh_, name_ + ".carrot_angular_distance", carrot_angular_distance_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), name_ + ".carrot_angular_distance: " << carrot_angular_distance_);
 
  tryGetOrSet(
    nh_, name_ + ".pure_spinning_straight_line_mode", straightBackwardsAndPureSpinningMode_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    name_ + ".pure_spinning_straight_line_mode: " << straightBackwardsAndPureSpinningMode_);
 
  tryGetOrSet(nh_, name_ + ".linear_mode_rho_error_threshold", linear_mode_rho_error_threshold_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    name_ + ".linear_mode_rho_error_threshold: " << linear_mode_rho_error_threshold_);
  tryGetOrSet(nh_, name_ + ".max_linear_x_speed", max_linear_x_speed_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".max_linear_x_speed: " << max_linear_x_speed_);
  tryGetOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".max_angular_z_speed: " << max_angular_z_speed_);
 
  if (yaw_goal_tolerance_ != -1 && carrot_angular_distance_ < yaw_goal_tolerance_)
  {
    RCLCPP_WARN_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] carrot_angular_distance ("
                           << carrot_angular_distance_
                           << ") cannot be lower than yaw_goal_tolerance (" << yaw_goal_tolerance_
                           << ") setting carrot_angular_distance = " << yaw_goal_tolerance_);
    carrot_angular_distance_ = yaw_goal_tolerance_;
    nh_->set_parameter(
      rclcpp::Parameter(name_ + ".carrot_angular_distance", carrot_angular_distance_));
  }
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), name_ + ".carrot_angular_distance: " << carrot_angular_distance_);
 
  if (xy_goal_tolerance_ != -1 && carrot_distance_ < xy_goal_tolerance_)
  {
    RCLCPP_WARN_STREAM(
      nh_->get_logger(), "[BackwardLocalPlanner] carrot_linear_distance ("
                           << carrot_distance_ << ") cannot be lower than xy_goal_tolerance_ ("
                           << yaw_goal_tolerance_
                           << ") setting carrot_angular_distance = " << xy_goal_tolerance_);
    carrot_distance_ = xy_goal_tolerance_;
    nh_->set_parameter(rclcpp::Parameter(name_ + ".carrot_distance", carrot_distance_));
  }
  RCLCPP_INFO_STREAM(nh_->get_logger(), name_ + ".carrot_distance:" << carrot_distance_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), "--- end params ---");
}

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

Definition at line 127 of file backward_local_planner.hpp.

Referenced by activate(), checkCarrotHalfPlainConstraint(), checkCurrentPoseInGoalRange(), cleanup(), computeCurrentEuclideanAndAngularErrorsToCarrotGoal(), computeVelocityCommands(), findInitialCarrotGoal(), resamplePrecisePlan(), setPlan(), and updateCarrotGoal().

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

Definition at line 152 of file backward_local_planner.hpp.

Referenced by configure(), findInitialCarrotGoal(), resamplePrecisePlan(), updateCarrotGoal(), and updateParameters().

carrot_distance_

meter cl_nav2z::backward_local_planner::BackwardLocalPlanner::carrot_distance_

private

Definition at line 151 of file backward_local_planner.hpp.

Referenced by configure(), findInitialCarrotGoal(), resamplePrecisePlan(), updateCarrotGoal(), and updateParameters().

costmapRos_

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

private

Definition at line 128 of file backward_local_planner.hpp.

Referenced by clearMarkers(), computeVelocityCommands(), configure(), publishGoalMarker(), and setPlan().

currentCarrotPoseIndex_

int cl_nav2z::backward_local_planner::BackwardLocalPlanner::currentCarrotPoseIndex_

private

Definition at line 169 of file backward_local_planner.hpp.

Referenced by checkCarrotHalfPlainConstraint(), cleanup(), computeCurrentEuclideanAndAngularErrorsToCarrotGoal(), computeVelocityCommands(), configure(), findInitialCarrotGoal(), setPlan(), and updateCarrotGoal().

divergenceDetectionLastCarrotLinearDistance_

meter cl_nav2z::backward_local_planner::BackwardLocalPlanner::divergenceDetectionLastCarrotLinearDistance_

private

Definition at line 153 of file backward_local_planner.hpp.

Referenced by divergenceDetectionUpdate(), and resetDivergenceDetection().

enable_obstacle_checking_

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

private

Definition at line 164 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

goalMarkerPublisher_

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

private

Definition at line 132 of file backward_local_planner.hpp.

Referenced by activate(), clearMarkers(), configure(), deactivate(), and publishGoalMarker().

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

Definition at line 137 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), straightBackwardsAndPureSpinCmd(), and updateParameters().

k_betta_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::k_betta_

private

Definition at line 138 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), straightBackwardsAndPureSpinCmd(), and updateParameters().

k_rho_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::k_rho_

private

Definition at line 136 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), straightBackwardsAndPureSpinCmd(), and updateParameters().

linear_mode_rho_error_threshold_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::linear_mode_rho_error_threshold_

private

Definition at line 140 of file backward_local_planner.hpp.

Referenced by configure(), straightBackwardsAndPureSpinCmd(), and updateParameters().

max_angular_z_speed_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::max_angular_z_speed_

private

Definition at line 146 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

max_linear_x_speed_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::max_linear_x_speed_

private

Definition at line 145 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

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

Definition at line 124 of file backward_local_planner.hpp.

Referenced by activate(), checkCarrotHalfPlainConstraint(), checkCurrentPoseInGoalRange(), cleanup(), clearMarkers(), computeCurrentEuclideanAndAngularErrorsToCarrotGoal(), computeVelocityCommands(), configure(), deactivate(), divergenceDetectionUpdate(), findInitialCarrotGoal(), isGoalReached(), publishGoalMarker(), resamplePrecisePlan(), setPlan(), setSpeedLimit(), updateCarrotGoal(), and updateParameters().

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

Definition at line 163 of file backward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

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

Definition at line 149 of file backward_local_planner.hpp.

Referenced by checkCurrentPoseInGoalRange(), computeVelocityCommands(), configure(), updateCarrotGoal(), and updateParameters().

yaw_goal_tolerance_

double cl_nav2z::backward_local_planner::BackwardLocalPlanner::yaw_goal_tolerance_

private

Definition at line 148 of file backward_local_planner.hpp.

Referenced by checkCurrentPoseInGoalRange(), computeVelocityCommands(), configure(), straightBackwardsAndPureSpinCmd(), and updateParameters().

---

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

• smacc2_client_library/nav2z_client/custom_planners/backward_local_planner/include/backward_local_planner/backward_local_planner.hpp
• smacc2_client_library/nav2z_client/custom_planners/backward_local_planner/src/backward_local_planner/backward_local_planner.cpp