#include <backward_local_planner.h>

Inheritance diagram for cl_move_base_z::backward_local_planner::BackwardLocalPlanner:

Collaboration diagram for cl_move_base_z::backward_local_planner::BackwardLocalPlanner:

Public Member Functions
	BackwardLocalPlanner ()

virtual	~BackwardLocalPlanner ()

virtual bool	computeVelocityCommands (geometry_msgs::Twist &cmd_vel) override
	Given the current position, orientation, and velocity of the robot: compute velocity commands to send to the robot mobile base. More...

virtual bool	isGoalReached () override
	Check if the goal pose has been achieved by the local planner. More...

virtual bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &plan) override
	Set the plan that the local planner is following. More...

void	initialize (std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmapRos_)
	Constructs the local planner. More...

void	initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROS costmapRos_)

void	initialize ()

Private Member Functions
void	reconfigCB (::backward_local_planner::BackwardLocalPlannerConfig &config, uint32_t level)

bool	findInitialCarrotGoal (tf::Stamped< tf::Pose > &pose)

bool	updateCarrotGoal (const tf::Stamped< tf::Pose > &tfpose)

bool	resamplePrecisePlan ()

void	straightBackwardsAndPureSpinCmd (const tf::Stamped< tf::Pose > &tfpose, double vetta, double gamma, double alpha_error, double betta_error, double rho_error, geometry_msgs::Twist &cmd_vel)

void	defaultBackwardCmd (const tf::Stamped< tf::Pose > &tfpose, double vetta, double gamma, double alpha_error, double betta_error, geometry_msgs::Twist &cmd_vel)

void	publishGoalMarker (double x, double y, double phi)

void	computeCurrentEuclideanAndAngularErrorsToCarrotGoal (const tf::Stamped< tf::Pose > &tfpose, double &dist, double &angular_error)

bool	checkCurrentPoseInGoalRange (const tf::Stamped< tf::Pose > &tfpose, double angle_error, bool &inLinearGoal)

bool	resetDivergenceDetection ()

bool	divergenceDetectionUpdate (const tf::Stamped< tf::Pose > &tfpose)

bool	checkCarrotHalfPlainConstraint (const tf::Stamped< tf::Pose > &tfpose)

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
dynamic_reconfigure::Server<::backward_local_planner::BackwardLocalPlannerConfig >	paramServer_

dynamic_reconfigure::Server<::backward_local_planner::BackwardLocalPlannerConfig >::CallbackType	f

std::vector< geometry_msgs::PoseStamped >	backwardsPlanPath_

costmap_2d::Costmap2DROS *	costmapRos_

ros::Publisher	goalMarkerPublisher_

double	k_rho_

double	k_alpha_

double	k_betta_

double	pure_spinning_allowed_betta_error_

double	linear_mode_rho_error_threshold_

bool	goalReached_

bool	initialPureSpinningStage_

bool	straightBackwardsAndPureSpinningMode_ = false

bool	enable_obstacle_checking_ = true

bool	inGoalPureSpinningState_ = false

const double	alpha_offset_ = M_PI

const double	betta_offset_ = 0

double	yaw_goal_tolerance_

double	xy_goal_tolerance_

meter	carrot_distance_

rad	carrot_angular_distance_

meter	divergenceDetectionLastCarrotLinearDistance_

double	max_linear_x_speed_

double	max_angular_z_speed_

uint64_t	currentCarrotPoseIndex_

bool	waiting_

ros::Duration	waitingTimeout_

ros::Time	waitingStamp_

Detailed Description

Definition at line 23 of file backward_local_planner.h.

Constructor & Destructor Documentation

◆ BackwardLocalPlanner()

cl_move_base_z::backward_local_planner::BackwardLocalPlanner::BackwardLocalPlanner ( )

BackwardLocalPlanner()

Definition at line 20 of file backward_local_planner.cpp.

            : paramServer_(ros::NodeHandle("~BackwardLocalPlanner"))
        {
        }

◆ ~BackwardLocalPlanner()

cl_move_base_z::backward_local_planner::BackwardLocalPlanner::~BackwardLocalPlanner ( )

virtual

~BackwardLocalPlanner()

Definition at line 30 of file backward_local_planner.cpp.

31 {

32 }

Member Function Documentation

◆ checkCarrotHalfPlainConstraint()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::checkCarrotHalfPlainConstraint ( const tf::Stamped< tf::Pose > & tfpose )

private

Definition at line 212 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::Point &carrot_point = carrot_pose.pose.position;
            double yaw = tf::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.getOrigin().x() + vy * tfpose.getOrigin().y() + c + C_OFFSET_METERS;
 
            ROS_DEBUG_STREAM("[BackwardLocalPlanner] half plane constraint:" << vx << "*" << carrot_point.x << " + " << vy << "*" << carrot_point.y << " + " << c);
            ROS_DEBUG_STREAM("[BackwardLocalPlanner] constraint evaluation: " << vx << "*" << tfpose.getOrigin().x() << " + " << vy << "*" << tfpose.getOrigin().y() << " + " << c << " = " << check);
 
            return check < 0;
        }

References backwardsPlanPath_, and currentCarrotPoseIndex_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ checkCurrentPoseInGoalRange()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::checkCurrentPoseInGoalRange	(	const tf::Stamped< tf::Pose > &	tfpose,
		double	angle_error,
		bool &	inLinearGoal
	)

private

Definition at line 240 of file backward_local_planner.cpp.

        {
            auto &finalgoal = backwardsPlanPath_.back();
            double gdx = finalgoal.pose.position.x - tfpose.getOrigin().x();
            double gdy = finalgoal.pose.position.y - tfpose.getOrigin().y();
            double goaldist = sqrt(gdx * gdx + gdy * gdy);
 
            auto abs_angle_error = fabs(angle_error);
            // final_alpha_error =
            ROS_DEBUG_STREAM("[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_;
 
            if (abs_angle_error < this->yaw_goal_tolerance_) // 5cm
            {
                return true;
            }
 
            return false;
        }

References backwardsPlanPath_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ computeCurrentEuclideanAndAngularErrorsToCarrotGoal()

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::computeCurrentEuclideanAndAngularErrorsToCarrotGoal	(	const tf::Stamped< tf::Pose > &	tfpose,
		double &	dist,
		double &	angular_error
	)

private

Definition at line 99 of file backward_local_planner.cpp.

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

References backwardsPlanPath_, and currentCarrotPoseIndex_.

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

Here is the caller graph for this function:

◆ computeNewPositions()

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

private

Definition at line 862 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()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel )

overridevirtual

Given the current position, orientation, and velocity of the robot: compute velocity commands to send to the robot mobile base.

Parameters

cmd_vel Will be filled with the velocity command to be passed to the robot base

Returns: True if a valid velocity command was found, false otherwise

computeVelocityCommands()

Definition at line 331 of file backward_local_planner.cpp.

        {
            ROS_DEBUG("[BackwardLocalPlanner] ------------------- LOCAL PLANNER LOOP -----------------");
            geometry_msgs::PoseStamped paux;
            tf::Stamped<tf::Pose> tfpose = optionalRobotPose(costmapRos_);
 
            //bool divergenceDetected = this->divergenceDetectionUpdate(tfpose);
            // 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)
            {
                ROS_ERROR("[BackwardLocalPlanner] Divergence detected. Sending emergency stop.");
                emergency_stop = true;
            }
 
            bool carrotInLinearGoalRange = updateCarrotGoal(tfpose);
            ROS_DEBUG_STREAM("[BackwardLocalPlanner] carrot goal created");
 
            if (emergency_stop)
            {
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = 0;
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] emergency stop, exit compute commands");
                return false;
            }
 
            // ------ Evaluate the current context ----
            double rho_error, betta_error, alpha_error;
 
 
                // getting carrot goal information
                tf::Quaternion q = tfpose.getRotation();
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] carrot goal: " << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
                const geometry_msgs::PoseStamped &carrotgoalpose = backwardsPlanPath_[currentCarrotPoseIndex_];
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] carrot goal pose current index: " << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size() << ": " << carrotgoalpose);
                const geometry_msgs::Point &carrotGoalPosition = carrotgoalpose.pose.position;
 
                tf::Quaternion goalQ;
                tf::quaternionMsgToTF(carrotgoalpose.pose.orientation, goalQ);
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] goal orientation: " << goalQ);
 
                //goal orientation (global frame)
                double betta = tf::getYaw(goalQ);
                betta = betta + betta_offset_;
 
                double dx = carrotGoalPosition.x - tfpose.getOrigin().x();
                double dy = carrotGoalPosition.y - tfpose.getOrigin().y();
 
                //distance error to the targetpoint
                rho_error = sqrt(dx * dx + dy * dy);
 
                //heading to goal angle
                double theta = tf::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,betta_error, linearGoalReached);
 
            bool carrotInFinalGoal = carrotInLinearGoalRange && currentCarrotPoseIndex_ == backwardsPlanPath_.size() - 1;
 
            if(currentPoseInGoal && carrotInFinalGoal)
            {
                goalReached_ = true;
                backwardsPlanPath_.clear();
                ROS_INFO_STREAM(" [BackwardLocalPlanner] GOAL REACHED. Send stop command and skipping trajectory collision: " << cmd_vel);
                cmd_vel.linear.x =0;
                cmd_vel.angular.z = 0;
                return true;
            }
 
            if (carrotInLinearGoalRange && linearGoalReached) // we miss here and not any carrot ahead is outside goal, replacing carrotInLinearGoalRange to carrotInFinalLinearGoalRange
            {
                inGoalPureSpinningState_ = true;
            }
 
 
                double vetta = k_rho_ * rho_error;
                double gamma = k_alpha_ * alpha_error + k_betta_ * betta_error;
                // --------------------
 
                if (straightBackwardsAndPureSpinningMode_)
                {
                    this->straightBackwardsAndPureSpinCmd(tfpose, vetta, gamma, alpha_error, betta_error, rho_error, cmd_vel);
                }
                else // default curved backward-free motion mode
                {
                    // case B: goal position reached but orientation not yet reached. deactivate linear motion.
                    if (inGoalPureSpinningState_)
                    {
                        ROS_DEBUG("[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
                    this->defaultBackwardCmd(tfpose, vetta, gamma, alpha_error, betta_error, cmd_vel);
                }
 
                if (cmd_vel.linear.x > max_linear_x_speed_)
                {
                    cmd_vel.linear.x = max_linear_x_speed_;
                }
                else if (cmd_vel.linear.x < -max_linear_x_speed_)
                {
                    cmd_vel.linear.x = -max_linear_x_speed_;
                }
 
                if (cmd_vel.angular.z > max_angular_z_speed_)
                {
                    cmd_vel.angular.z = max_angular_z_speed_;
                }
                else if (cmd_vel.angular.z < -max_angular_z_speed_)
                {
                    cmd_vel.angular.z = -max_angular_z_speed_;
                }
 
                publishGoalMarker(carrotGoalPosition.x, carrotGoalPosition.y, betta);
 
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] local planner," << std::endl
                                                                         << " straightAnPureSpiningMode: " << straightBackwardsAndPureSpinningMode_ << std::endl
                                                                         << " inGoalPureSpinningState: " << inGoalPureSpinningState_ << std::endl
                                                                         << "carrotInLinearGoalRange: " << carrotInLinearGoalRange << 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.linear.x << std::endl
                                                                         << " cmd_vel.ang.z:" << cmd_vel.angular.z);
 
                if (inGoalPureSpinningState_)
                {
                    bool carrotHalfPlaneConstraintFailure = checkCarrotHalfPlainConstraint(tfpose);
 
                    if (carrotHalfPlaneConstraintFailure)
                    {
                        ROS_ERROR("[BackwardLocalPlanner] CarrotHalfPlaneConstraintFailure detected. Sending emergency stop and success to the planner.");
                        cmd_vel.linear.x = 0;
                    }
                }
 
 
            // ---------------------- TRAJECTORY PREDICTION AND COLLISION AVOIDANCE ---------------------
            //cmd_vel.linear.x=0;
            //cmd_vel.angular.z = 0;
            tf::Stamped<tf::Pose> global_pose = optionalRobotPose(costmapRos_);
 
            auto *costmap2d = costmapRos_->getCostmap();
            auto yaw = tf::getYaw(global_pose.getRotation());
 
            auto &pos = global_pose.getOrigin();
 
            Eigen::Vector3f currentpose(pos.x(), pos.y(), yaw);
            Eigen::Vector3f currentvel(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.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;
 
            uint32_t mx, my;
 
            if (this->enable_obstacle_checking_)
            {
                if (backwardsPlanPath_.size() > 0)
                {
                    auto &finalgoalpose = backwardsPlanPath_.back();
 
                    int i = 0;
                    // ROS_INFO_STREAM("lplanner goal: " << finalgoalpose.pose.position);
                    for (auto &p : trajectory)
                    {
                        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_)
                        {
                            ROS_DEBUG("[BackwardLocalPlanner] trajectory simulation for collision checking: goal reached with no collision");
                            break;
                        }
 
                        costmap2d->worldToMap(p[0], p[1], mx, my);
                        /*uint64_t cost =*/ costmap2d->getCost(mx, my);
 
                        // ROS_DEBUG("[BackwardLocalPlanner] checking cost pt %d [%lf, %lf] cell[%d,%d] = %d", i, p[0], p[1], mx, my, cost);
                        // ROS_DEBUG_STREAM("[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) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
                        {
                            acceptedLocalTrajectoryFreeOfObstacles = false;
                            ROS_WARN_STREAM("[BackwardLocalPlanner] ABORTED LOCAL PLAN BECAUSE OBSTACLE DETEDTED at point " << i << "/" << trajectory.size() << std::endl
                                                                                                                            << p[0] << ", " << p[1]);
                            break;
                        }
                        i++;
                    }
                }
                else
                {
                    ROS_WARN("[BackwardLocalPlanner] Abort local - Backwards global plan size: %ld", backwardsPlanPath_.size());
                    return false;
                }
            }
 
            if (acceptedLocalTrajectoryFreeOfObstacles)
            {
                waiting_ = false;
                ROS_DEBUG("[BackwardLocalPlanner] accepted local trajectory free of obstacle. Local planner continues.");
                return true;
            }
            else // that is not appceted because existence of obstacles
            {
                // emergency stop for collision: waiting a while before sending error
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = 0;
 
                if (waiting_ == false)
                {
                    waiting_ = true;
                    waitingStamp_ = ros::Time::now();
                    ROS_WARN("[BackwardLocalPlanner][Not accepted local plan] starting countdown");
                }
                else
                {
                    auto waitingduration = ros::Time::now() - waitingStamp_;
 
                    if (waitingduration > this->waitingTimeout_)
                    {
                        ROS_WARN("[BackwardLocalPlanner][Abort local] timeout! duration %lf/%f", waitingduration.toSec(), waitingTimeout_.toSec());
                        return false;
                    }
                }
 
                return true;
            }
        }

References alpha_offset_, backwardsPlanPath_, betta_offset_, checkCarrotHalfPlainConstraint(), checkCurrentPoseInGoalRange(), costmapRos_, currentCarrotPoseIndex_, defaultBackwardCmd(), enable_obstacle_checking_, generateTrajectory(), goalReached_, inGoalPureSpinningState_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, cl_move_base_z::backward_local_planner::optionalRobotPose(), publishGoalMarker(), straightBackwardsAndPureSpinCmd(), straightBackwardsAndPureSpinningMode_, updateCarrotGoal(), waiting_, waitingStamp_, waitingTimeout_, and xy_goal_tolerance_.

Here is the call graph for this function:

◆ defaultBackwardCmd()

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::defaultBackwardCmd	(	const tf::Stamped< tf::Pose > &	tfpose,
		double	vetta,
		double	gamma,
		double	alpha_error,
		double	betta_error,
		geometry_msgs::Twist &	cmd_vel
	)

private

defaultBackwardCmd()

Definition at line 266 of file backward_local_planner.cpp.

        {
            cmd_vel.linear.x = vetta;
            cmd_vel.angular.z = gamma;
        }

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ divergenceDetectionUpdate()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::divergenceDetectionUpdate ( const tf::Stamped< tf::Pose > & tfpose )

private

Definition at line 181 of file backward_local_planner.cpp.

        {
            double disterr = 0, angleerr = 0;
            computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, disterr, angleerr);
 
            ROS_DEBUG_STREAM("[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_)
                {
                    ROS_ERROR_STREAM("[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(), and divergenceDetectionLastCarrotLinearDistance_.

Referenced by setPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ findInitialCarrotGoal()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::findInitialCarrotGoal ( tf::Stamped< tf::Pose > & pose )

private

Definition at line 633 of file backward_local_planner.cpp.

        {
            double lineardisterr, angleerr;
            bool inCarrotRange = false;
 
            // initial state check
            computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, lineardisterr, angleerr);
 
            // int closestIndex = -1;
            // double minpointdist = std::numeric_limits<double>::max();
 
            // lets set the carrot-goal in the correct place with this loop
            while (currentCarrotPoseIndex_ < backwardsPlanPath_.size() && !inCarrotRange)
            {
                computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, lineardisterr, angleerr);
 
                ROS_DEBUG("[BackwardLocalPlanner] Finding initial carrot goal i=%ld - 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_)
                {
                    ROS_DEBUG("[BackwardLocalPlanner] Finding initial carrot goal i=%ld - 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
                {
                    ROS_DEBUG("[BackwardLocalPlanner] Finding initial carrot goal i=%ld - carrot out of range, searching coincidence...", currentCarrotPoseIndex_);
                }
 
                currentCarrotPoseIndex_++;
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] setPlan: fw" << currentCarrotPoseIndex_);
            }
 
            ROS_INFO_STREAM("[BackwardLocalPlanner] setPlan: (found first carrot:" << inCarrotRange << ") initial carrot point index: " << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
 
            return inCarrotRange;
        }

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

Referenced by setPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ generateTrajectory()

void cl_move_base_z::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 800 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);
                //   //ROS_WARN_NAMED("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;
 
                //ROS_INFO("traj point %d", i);
                dist = sqrt(dx * dx + dy * dy);
                if (dist > maxdist)
                {
                    end = true;
                    //ROS_INFO("dist break: %f", dist);
                }
                else
                {
                    // ouble from, double to
                    angledist = angles::shortest_angular_distance(currentpos[2], newpos[2]);
                    if (angledist > maxanglediff)
                    {
                        end = true;
                        //ROS_INFO("angle dist break: %f", angledist);
                    }
                    else
                    {
                        outtraj.push_back(newpos);
 
                        time += dt;
                        if (time > maxtime)
                        {
                            end = true;
                            //ROS_INFO("time break: %f", time);
                        }
 
                        //ROS_INFO("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:

◆ initialize() [1/3]

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::initialize ( )

Definition at line 40 of file backward_local_planner.cpp.

        {
            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;
 
            f = boost::bind(&BackwardLocalPlanner::reconfigCB, this, _1, _2);
            paramServer_.setCallback(f);
            this->currentCarrotPoseIndex_ = 0;
 
            ros::NodeHandle nh("~/BackwardLocalPlanner");
            nh.param("pure_spinning_straight_line_mode", straightBackwardsAndPureSpinningMode_, true);
            nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
            nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);
            nh.param("k_rho", k_rho_, k_rho_);
            nh.param("k_betta", k_betta_, k_betta_);
            nh.param("linear_mode_rho_error_threshold", linear_mode_rho_error_threshold_, linear_mode_rho_error_threshold_);
 
            nh.param("carrot_distance", carrot_distance_, carrot_distance_);
            nh.param("carrot_angular_distance", carrot_angular_distance_, carrot_angular_distance_);
            nh.param("enable_obstacle_checking", enable_obstacle_checking_, enable_obstacle_checking_);
 
            nh.param("max_linear_x_speed", max_linear_x_speed_, 1.0);
            nh.param("max_angular_z_speed", max_angular_z_speed_, 2.0);
 
            // we have to do this, for example for the case we are refining the final orientation.
            // se 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 (carrot_angular_distance_ < yaw_goal_tolerance_)
            {
                ROS_WARN_STREAM("[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 (carrot_distance_ < xy_goal_tolerance_)
            {
                ROS_WARN_STREAM("[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.advertise<visualization_msgs::MarkerArray>("goal_marker", 1);
            waitingTimeout_ = ros::Duration(10);
        }

References carrot_angular_distance_, carrot_distance_, currentCarrotPoseIndex_, enable_obstacle_checking_, f, goalMarkerPublisher_, k_alpha_, k_betta_, k_rho_, linear_mode_rho_error_threshold_, max_angular_z_speed_, max_linear_x_speed_, paramServer_, reconfigCB(), straightBackwardsAndPureSpinningMode_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by initialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initialize() [2/3]

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::initialize	(	std::string	name,
		tf2_ros::Buffer *	tf,
		costmap_2d::Costmap2DROS *	costmapRos_
	)

Definition at line 34 of file backward_local_planner.cpp.

        {
            this->costmapRos_ = costmap_ros;
            this->initialize();
        }

References costmapRos_, and initialize().

Here is the call graph for this function:

◆ initialize() [3/3]

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::initialize	(	std::string	name,
		tf::TransformListener *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

Constructs the local planner.

Parameters

name	The name to give this instance of the local planner
tf	A pointer to a transform listener
costmap_ros	The cost map to use for assigning costs to local plans

initialize()

Definition at line 93 of file backward_local_planner.cpp.

        {
            this->costmapRos_ = costmap_ros;
            this->initialize();
        }

References costmapRos_, and initialize().

Here is the call graph for this function:

◆ isGoalReached()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::isGoalReached ( )

overridevirtual

Check if the goal pose has been achieved by the local planner.

Returns: True if achieved, false otherwise

isGoalReached()

Definition at line 627 of file backward_local_planner.cpp.

        {
            ROS_DEBUG_STREAM("[BackwardLocalPlanner] isGoalReached call: " << goalReached_);
            return goalReached_;
        }

References goalReached_.

◆ publishGoalMarker()

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

private

publishGoalMarker()

Definition at line 876 of file backward_local_planner.cpp.

        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = this->costmapRos_->getGlobalFrameID();
            marker.header.stamp = ros::Time::now();
 
            marker.ns = "my_namespace2";
            marker.id = 0;
            marker.type = visualization_msgs::Marker::ARROW;
            marker.action = visualization_msgs::Marker::ADD;
            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;
 
            marker.color.r = 1;
            marker.color.g = 0;
            marker.color.b = 0;
 
            geometry_msgs::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::MarkerArray ma;
            ma.markers.push_back(marker);
 
            goalMarkerPublisher_.publish(ma);
        }

References costmapRos_, and goalMarkerPublisher_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ reconfigCB()

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::reconfigCB	(	::backward_local_planner::BackwardLocalPlannerConfig &	config,
		uint32_t	level
	)

private

reconfigCB()

Definition at line 590 of file backward_local_planner.cpp.

        {
            ROS_INFO("[BackwardLocalPlanner] reconfigure Request");
            k_alpha_ = config.k_alpha;
            k_betta_ = config.k_betta;
            k_rho_ = config.k_rho;
            enable_obstacle_checking_ = config.enable_obstacle_checking;
 
            //alpha_offset_ = config.alpha_offset;
            //betta_offset_ = config.betta_offset;
 
            carrot_distance_ = config.carrot_distance;
            carrot_angular_distance_ = config.carrot_angular_distance;
            xy_goal_tolerance_ = config.xy_goal_tolerance;
            yaw_goal_tolerance_ = config.yaw_goal_tolerance;
            straightBackwardsAndPureSpinningMode_ = config.pure_spinning_straight_line_mode;
 
            if (carrot_angular_distance_ < yaw_goal_tolerance_)
            {
                ROS_WARN_STREAM("[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_;
                config.carrot_angular_distance = yaw_goal_tolerance_;
            }
 
            if (carrot_distance_ < xy_goal_tolerance_)
            {
                ROS_WARN_STREAM("[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_;
                config.carrot_distance = xy_goal_tolerance_;
            }
        }

References carrot_angular_distance_, carrot_distance_, enable_obstacle_checking_, k_alpha_, k_betta_, k_rho_, straightBackwardsAndPureSpinningMode_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by initialize().

Here is the caller graph for this function:

◆ resamplePrecisePlan()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::resamplePrecisePlan ( )

private

Definition at line 681 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
 
            ROS_DEBUG("[BackwardLocalPlanner] resample precise");
            if(backwardsPlanPath_.size() <=1)
            {
                ROS_DEBUG_STREAM("[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 (uint64_t i = 0; i < backwardsPlanPath_.size() - 1; i++)
            {
                ROS_DEBUG_STREAM("[BackwardLocalPlanner] resample precise, check: " << i);
                auto &currpose = backwardsPlanPath_[i];
                auto &nextpose = backwardsPlanPath_[i + 1];
 
                tf::Quaternion qCurrent, qNext;
                tf::quaternionMsgToTF(currpose.pose.orientation, qCurrent);
                tf::quaternionMsgToTF(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)
                {
                    ROS_DEBUG_STREAM("[BackwardLocalPlanner] resampling point, linear distance:" << dist << "(" << maxallowedLinearError << ")" << i);
                    resample = true;
                }
                else
                {
                    double currentAngle = tf::getYaw(qCurrent);
                    double nextAngle = tf::getYaw(qNext);
 
                    double angularError = fabs(angles::shortest_angular_distance(currentAngle, nextAngle));
                    if (angularError > maxallowedAngularError)
                    {
                        resample = true;
                        ROS_DEBUG_STREAM("[BackwardLocalPlanner] resampling point, angular distance:" << angularError << "(" << maxallowedAngularError << ")" << i);
                    }
                }
 
                if (resample)
                {
                    geometry_msgs::PoseStamped pintermediate;
                    auto duration = nextpose.header.stamp - currpose.header.stamp;
                    pintermediate.header.frame_id = currpose.header.frame_id;
                    pintermediate.header.stamp = 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);
                    tf::Quaternion intermediateQuat = tf::slerp(qCurrent, qNext, 0.5);
                    tf::quaternionTFToMsg(intermediateQuat, pintermediate.pose.orientation);
 
                    this->backwardsPlanPath_.insert(this->backwardsPlanPath_.begin() + i + 1, pintermediate);
 
                    // retry this point
                    i--;
                    counter++;
                }
            }
 
            ROS_DEBUG_STREAM("[BackwardLocalPlanner] End resampling. resampled:" << counter << " new inserted poses during precise resmapling.");
            return true;
        }

References backwardsPlanPath_, carrot_angular_distance_, and carrot_distance_.

Referenced by setPlan().

Here is the caller graph for this function:

◆ resetDivergenceDetection()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::resetDivergenceDetection ( )

private

Definition at line 174 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()

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::setPlan ( const std::vector< geometry_msgs::PoseStamped > & plan )

overridevirtual

Set the plan that the local planner is following.

Parameters

plan	The plan to pass to the local planner

Returns: True if the plan was updated successfully, false otherwise

setPlan()

Definition at line 760 of file backward_local_planner.cpp.

        {
            ROS_INFO_STREAM("[BackwardLocalPlanner] setPlan: new global plan received (" << plan.size() << ")");
            initialPureSpinningStage_ = true;
            goalReached_ = false;
            backwardsPlanPath_ = plan;
            inGoalPureSpinningState_ =false;
 
            // find again the new carrot goal, from the destiny direction
            tf::Stamped<tf::Pose> tfpose = optionalRobotPose(costmapRos_);
 
            geometry_msgs::PoseStamped posestamped;
            tf::poseStampedTFToMsg(tfpose,posestamped);
            backwardsPlanPath_.insert(backwardsPlanPath_.begin(), posestamped);
 
            this->resamplePrecisePlan();
 
            currentCarrotPoseIndex_ = 0;
            this->resetDivergenceDetection();
 
            if (plan.size() == 0)
            {
                ROS_WARN("[BackwardLocalPlanner] received plan without any pose");
                return true;
            }
 
            bool foundInitialCarrotGoal = this->findInitialCarrotGoal(tfpose);
            if (!foundInitialCarrotGoal)
            {
                ROS_ERROR("[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
            }
            else
            {
                this->divergenceDetectionUpdate(tfpose);
                // SANDARD AND PREFERRED CASE ON NEW PLAN
                return true;
            }
        }

References backwardsPlanPath_, costmapRos_, currentCarrotPoseIndex_, divergenceDetectionUpdate(), findInitialCarrotGoal(), goalReached_, inGoalPureSpinningState_, initialPureSpinningStage_, cl_move_base_z::backward_local_planner::optionalRobotPose(), resamplePrecisePlan(), and resetDivergenceDetection().

Here is the call graph for this function:

◆ straightBackwardsAndPureSpinCmd()

void cl_move_base_z::backward_local_planner::BackwardLocalPlanner::straightBackwardsAndPureSpinCmd	(	const tf::Stamped< tf::Pose > &	tfpose,
		double	vetta,
		double	gamma,
		double	alpha_error,
		double	betta_error,
		double	rho_error,
		geometry_msgs::Twist &	cmd_vel
	)

private

pureSpinningCmd()

Definition at line 276 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;
                gamma = k_betta_ * betta_error;
            }
            /*
            // THIS IS NOT TRUE, IF THE CARROT REACHES THE GOAL DOES NOT MEAN THE ROBOT IS
            // IN THE CORRECT POSITION
            else if (currentCarrotPoseIndex_ >= backwardsPlanPath_.size() - 1)
            {
                vetta = 0;
                gamma = 0;
                goalReached_ = true;
                backwardsPlanPath_.clear();
 
                ROS_INFO_STREAM("BACKWARD LOCAL PLANNER END: Goal Reached. Stop action [rhoerror: " << rho_error<<"]");
            }*/
 
            cmd_vel.linear.x = vetta;
            cmd_vel.angular.z = gamma;
        }

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_move_base_z::backward_local_planner::BackwardLocalPlanner::updateCarrotGoal ( const tf::Stamped< tf::Pose > & tfpose )

private

updateCarrotGoal()

Definition at line 131 of file backward_local_planner.cpp.

        {
            ROS_DEBUG("[BackwardsLocalPlanner] --- Computing carrot pose ---");
            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_ < backwardsPlanPath_.size())
            {
                computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, disterr, angleerr);
 
                ROS_DEBUG_STREAM("[BackwardsLocalPlanner] update carrot goal: Current index: " << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
                ROS_DEBUG("[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();
                    ROS_DEBUG_STREAM("[BackwardsLocalPlanner] fw " << currentCarrotPoseIndex_ << "/" << backwardsPlanPath_.size());
                }
                else
                {
                    break;
                }
            }
            //ROS_DEBUG("[BackwardsLocalPlanner] computing angular error");
            if (currentCarrotPoseIndex_ >= backwardsPlanPath_.size() && backwardsPlanPath_.size() > 0)
            {
                currentCarrotPoseIndex_ = backwardsPlanPath_.size() - 1;
                computeCurrentEuclideanAndAngularErrorsToCarrotGoal(tfpose, disterr, angleerr);
            }
 
            ROS_DEBUG("[BackwardsLocalPlanner] Current index carrot goal: %ld", currentCarrotPoseIndex_);
            ROS_DEBUG("[BackwardsLocalPlanner] Update carrot goal: linear error  %lf, angular error: %lf", disterr, angleerr);
            bool carrotInGoalLinear = disterr < xy_goal_tolerance_;
            ROS_DEBUG("[BackwardsLocalPlanner] carrot in goal radius: %d", carrotInGoalLinear);
 
            ROS_DEBUG("[BackwardsLocalPlanner] --- Computing carrot pose ---");
 
            return carrotInGoalLinear;
        }

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

Referenced by 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_move_base_z::backward_local_planner::BackwardLocalPlanner::alpha_offset_ = M_PI

private

Definition at line 113 of file backward_local_planner.h.

Referenced by computeVelocityCommands().

◆ backwardsPlanPath_

std::vector<geometry_msgs::PoseStamped> cl_move_base_z::backward_local_planner::BackwardLocalPlanner::backwardsPlanPath_

private

Definition at line 96 of file backward_local_planner.h.

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

◆ betta_offset_

const double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::betta_offset_ = 0

private

Definition at line 114 of file backward_local_planner.h.

Referenced by computeVelocityCommands().

◆ carrot_angular_distance_

rad cl_move_base_z::backward_local_planner::BackwardLocalPlanner::carrot_angular_distance_

private

Definition at line 120 of file backward_local_planner.h.

Referenced by findInitialCarrotGoal(), initialize(), reconfigCB(), resamplePrecisePlan(), and updateCarrotGoal().

◆ carrot_distance_

meter cl_move_base_z::backward_local_planner::BackwardLocalPlanner::carrot_distance_

private

Definition at line 119 of file backward_local_planner.h.

Referenced by findInitialCarrotGoal(), initialize(), reconfigCB(), resamplePrecisePlan(), and updateCarrotGoal().

◆ costmapRos_

costmap_2d::Costmap2DROS* cl_move_base_z::backward_local_planner::BackwardLocalPlanner::costmapRos_

private

Definition at line 97 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), publishGoalMarker(), and setPlan().

◆ currentCarrotPoseIndex_

uint64_t cl_move_base_z::backward_local_planner::BackwardLocalPlanner::currentCarrotPoseIndex_

private

Definition at line 127 of file backward_local_planner.h.

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

◆ divergenceDetectionLastCarrotLinearDistance_

meter cl_move_base_z::backward_local_planner::BackwardLocalPlanner::divergenceDetectionLastCarrotLinearDistance_

private

Definition at line 121 of file backward_local_planner.h.

Referenced by divergenceDetectionUpdate(), and resetDivergenceDetection().

◆ enable_obstacle_checking_

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::enable_obstacle_checking_ = true

private

Definition at line 110 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), and reconfigCB().

◆ f

dynamic_reconfigure::Server<::backward_local_planner::BackwardLocalPlannerConfig>::CallbackType cl_move_base_z::backward_local_planner::BackwardLocalPlanner::f

private

Definition at line 94 of file backward_local_planner.h.

Referenced by initialize().

◆ goalMarkerPublisher_

ros::Publisher cl_move_base_z::backward_local_planner::BackwardLocalPlanner::goalMarkerPublisher_

private

Definition at line 99 of file backward_local_planner.h.

Referenced by initialize(), and publishGoalMarker().

◆ goalReached_

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::goalReached_

private

Definition at line 107 of file backward_local_planner.h.

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

◆ inGoalPureSpinningState_

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::inGoalPureSpinningState_ = false

private

Definition at line 111 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), and setPlan().

◆ initialPureSpinningStage_

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::initialPureSpinningStage_

private

Definition at line 108 of file backward_local_planner.h.

Referenced by setPlan().

◆ k_alpha_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::k_alpha_

private

Definition at line 102 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), reconfigCB(), and straightBackwardsAndPureSpinCmd().

◆ k_betta_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::k_betta_

private

Definition at line 103 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), reconfigCB(), and straightBackwardsAndPureSpinCmd().

◆ k_rho_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::k_rho_

private

Definition at line 101 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), reconfigCB(), and straightBackwardsAndPureSpinCmd().

◆ linear_mode_rho_error_threshold_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::linear_mode_rho_error_threshold_

private

Definition at line 105 of file backward_local_planner.h.

Referenced by initialize(), and straightBackwardsAndPureSpinCmd().

◆ max_angular_z_speed_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::max_angular_z_speed_

private

Definition at line 124 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), and initialize().

◆ max_linear_x_speed_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::max_linear_x_speed_

private

Definition at line 123 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), and initialize().

◆ paramServer_

dynamic_reconfigure::Server<::backward_local_planner::BackwardLocalPlannerConfig> cl_move_base_z::backward_local_planner::BackwardLocalPlanner::paramServer_

private

Definition at line 93 of file backward_local_planner.h.

Referenced by initialize().

◆ pure_spinning_allowed_betta_error_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::pure_spinning_allowed_betta_error_

private

Definition at line 104 of file backward_local_planner.h.

◆ straightBackwardsAndPureSpinningMode_

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::straightBackwardsAndPureSpinningMode_ = false

private

Definition at line 109 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), and reconfigCB().

◆ waiting_

bool cl_move_base_z::backward_local_planner::BackwardLocalPlanner::waiting_

private

Definition at line 133 of file backward_local_planner.h.

Referenced by computeVelocityCommands().

◆ waitingStamp_

ros::Time cl_move_base_z::backward_local_planner::BackwardLocalPlanner::waitingStamp_

private

Definition at line 135 of file backward_local_planner.h.

Referenced by computeVelocityCommands().

◆ waitingTimeout_

ros::Duration cl_move_base_z::backward_local_planner::BackwardLocalPlanner::waitingTimeout_

private

Definition at line 134 of file backward_local_planner.h.

Referenced by computeVelocityCommands(), and initialize().

◆ xy_goal_tolerance_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::xy_goal_tolerance_

private

Definition at line 117 of file backward_local_planner.h.

Referenced by checkCurrentPoseInGoalRange(), computeVelocityCommands(), initialize(), reconfigCB(), and updateCarrotGoal().

◆ yaw_goal_tolerance_

double cl_move_base_z::backward_local_planner::BackwardLocalPlanner::yaw_goal_tolerance_

private

Definition at line 116 of file backward_local_planner.h.

Referenced by checkCurrentPoseInGoalRange(), initialize(), reconfigCB(), and straightBackwardsAndPureSpinCmd().

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

smacc_client_library/move_base_z_client/custom_planners/backward_local_planner/include/backward_local_planner/backward_local_planner.h
smacc_client_library/move_base_z_client/custom_planners/backward_local_planner/src/backward_local_planner.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ BackwardLocalPlanner()

◆ ~BackwardLocalPlanner()

Member Function Documentation

◆ checkCarrotHalfPlainConstraint()

◆ checkCurrentPoseInGoalRange()

◆ computeCurrentEuclideanAndAngularErrorsToCarrotGoal()

◆ computeNewPositions()

◆ computeVelocityCommands()

◆ defaultBackwardCmd()

◆ divergenceDetectionUpdate()

◆ findInitialCarrotGoal()

◆ generateTrajectory()

◆ initialize() [1/3]

◆ initialize() [2/3]

◆ initialize() [3/3]

◆ isGoalReached()

◆ publishGoalMarker()

◆ reconfigCB()

◆ resamplePrecisePlan()

◆ resetDivergenceDetection()

◆ setPlan()

◆ straightBackwardsAndPureSpinCmd()

◆ updateCarrotGoal()

Member Data Documentation

◆ alpha_offset_

◆ backwardsPlanPath_

◆ betta_offset_

◆ carrot_angular_distance_

◆ carrot_distance_

◆ costmapRos_

◆ currentCarrotPoseIndex_

◆ divergenceDetectionLastCarrotLinearDistance_

◆ enable_obstacle_checking_

◆ f

◆ goalMarkerPublisher_

◆ goalReached_

◆ inGoalPureSpinningState_

◆ initialPureSpinningStage_

◆ k_alpha_

◆ k_betta_

◆ k_rho_

◆ linear_mode_rho_error_threshold_

◆ max_angular_z_speed_

◆ max_linear_x_speed_

◆ paramServer_

◆ pure_spinning_allowed_betta_error_

◆ straightBackwardsAndPureSpinningMode_

◆ waiting_

◆ waitingStamp_

◆ waitingTimeout_

◆ xy_goal_tolerance_

◆ yaw_goal_tolerance_