#include <undo_path_global_planner.h>

Inheritance diagram for cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner:

Collaboration diagram for cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner:

Public Member Functions
	UndoPathGlobalPlanner ()

virtual	~UndoPathGlobalPlanner ()

bool	makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)

bool	makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan, double &cost)

virtual bool	createDefaultUndoPathPlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)

virtual void	initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros_) override

Private Member Functions
void	onForwardTrailMsg (const nav_msgs::Path::ConstPtr &trailMessage)

void	publishGoalMarker (const geometry_msgs::Pose &pose, double r, double g, double b)

Private Attributes
ros::NodeHandle	nh_

ros::Subscriber	forwardPathSub_

ros::Publisher	planPub_

ros::Publisher	markersPub_

nav_msgs::Path	lastForwardPathMsg_

costmap_2d::Costmap2DROS *	costmap_ros_
	stored but almost not used More...

ros::ServiceServer	cmd_server_

double	skip_straight_motion_distance_

double	puresSpinningRadStep_

Detailed Description

Definition at line 17 of file undo_path_global_planner.h.

Constructor & Destructor Documentation

◆ UndoPathGlobalPlanner()

cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::UndoPathGlobalPlanner ( )

Constructor()

Definition at line 32 of file undo_path_global_planner.cpp.

        {
            skip_straight_motion_distance_ = 0.2;
        }

References skip_straight_motion_distance_.

◆ ~UndoPathGlobalPlanner()

cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::~UndoPathGlobalPlanner ( )

virtual

Definition at line 37 of file undo_path_global_planner.cpp.

        {
            //clear
            nav_msgs::Path planMsg;
            planMsg.header.stamp = ros::Time::now();
            planPub_.publish(planMsg);
        }

References planPub_.

Member Function Documentation

◆ createDefaultUndoPathPlan()

bool cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::createDefaultUndoPathPlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)

virtual

defaultBackwardPath()

Definition at line 118 of file undo_path_global_planner.cpp.

        {
            //ROS_WARN_NAMED("Backwards", "Iterating in last forward cord path");
            int i = lastForwardPathMsg_.poses.size() - 1;
            double linear_mindist = std::numeric_limits<double>::max();
            int mindistindex = -1;
            double startPoseAngle = tf::getYaw(start.pose.orientation);
            geometry_msgs::Pose startPositionProjected;
 
            // The goal of this code is finding the most convenient initial path pose.
            // first, find closest linear point to the current robot position
            // we start from the final goal, that is, the beginning of the trajectory
            // (since this was the forward motion from the odom tracker)
            for (auto &p : lastForwardPathMsg_.poses /*| boost::adaptors::reversed*/)
            {
                geometry_msgs::PoseStamped pose = p;
                pose.header.frame_id = costmap_ros_->getGlobalFrameID();
                pose.header.stamp = ros::Time::now();
 
                double dx = pose.pose.position.x - start.pose.position.x;
                double dy = pose.pose.position.y - start.pose.position.y;
 
                double dist = sqrt(dx * dx + dy * dy);
                double angleOrientation = tf::getYaw(pose.pose.orientation);
                double angleError = fabs(angles::shortest_angular_distance(angleOrientation, startPoseAngle));
                if (dist <= linear_mindist)
                {
                    mindistindex = i;
                    linear_mindist = dist;
                    startPositionProjected = pose.pose;
 
                    ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] initial start point search, NEWBEST_LINEAR= " << i << ". error, linear: " << linear_mindist << ", angular: " << angleError);
                }
                else
                {
                    ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] initial start point search, skipped= " << i << ". best linear error: " << linear_mindist << ". current error, linear: " << dist << " angular: " << angleError);
                }
 
                i--;
            }
 
            double const ERROR_DISTANCE_PURE_SPINNING_FACTOR = 1.5;
            // Concept of second pass: now we only consider a pure spinning motion in this point. We want to consume some very close angular targets, (accepting a larger linear minerror of 1.5 besterror. That is, more or less in the same point).
 
            ROS_DEBUG("[UndoPathGlobalPlanner] second angular pass");
            double angularMinDist = std::numeric_limits<double>::max();
 
                if(mindistindex >=  lastForwardPathMsg_.poses.size())
                    mindistindex = lastForwardPathMsg_.poses.size() -1;// workaround, something is making a out of bound exception in poses array access
            {
                if(lastForwardPathMsg_.poses.size() == 0)
                {
                    ROS_WARN_STREAM("[UndoPathGlobalPlanner] Warning possible bug");
                }
 
                ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] second pass loop");
                for (int i = mindistindex; i >= 0; i--)
                {
                    // warning this index, i refers to some inverse interpretation from the previous loop,
                    // (last indexes in this path corresponds to the poses closer to our current position)
                    ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] " << i << "/" << lastForwardPathMsg_.poses.size());
                    auto index = lastForwardPathMsg_.poses.size() - i -1;
                    if(index < 0 || index >= lastForwardPathMsg_.poses.size())
                    {
                        ROS_WARN_STREAM("[UndoPathGlobalPlanner] this should not happen. Check implementation.");
                        break;
                    }
                    geometry_msgs::PoseStamped pose = lastForwardPathMsg_.poses[lastForwardPathMsg_.poses.size() - i -1];
 
                    ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] global frame");
                    pose.header.frame_id = costmap_ros_->getGlobalFrameID();
                    pose.header.stamp = ros::Time::now();
 
                    double dx = pose.pose.position.x - start.pose.position.x;
                    double dy = pose.pose.position.y - start.pose.position.y;
 
                    double dist = sqrt(dx * dx + dy * dy);
                    if (dist <= linear_mindist * ERROR_DISTANCE_PURE_SPINNING_FACTOR)
                    {
                        double angleOrientation = tf::getYaw(pose.pose.orientation);
                        double angleError = fabs(angles::shortest_angular_distance(angleOrientation, startPoseAngle));
                        if (angleError < angularMinDist)
                        {
                            angularMinDist = angleError;
                            mindistindex = i;
                            ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] initial start point search (angular update), NEWBEST_ANGULAR= " << i << ". error, linear: "<<  dist << "(" << linear_mindist << ")" <<", angular: " << angleError << "(" << angularMinDist << ")");
                        }
                        else
                        {
                            ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] initial start point search (angular update), skipped= " << i <<". error, linear: "<<  dist << "(" << linear_mindist << ")" <<", angular: "  << angleError << "(" << angularMinDist << ")");
                        }
                    }
                    else
                    {
                        ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] initial start point search (angular update) not in linear range, skipped= " << i << " linear error: " << dist << "(" << linear_mindist << ")");
                    }
                }
            }
 
            if (mindistindex != -1)
            {
                //plan.push_back(start);
 
                ROS_WARN_STREAM("[UndoPathGlobalPlanner] Creating the backwards plan from odom tracker path (" << lastForwardPathMsg_.poses.size() << ") poses");
                ROS_WARN_STREAM("[UndoPathGlobalPlanner] closer point to goal i=" << mindistindex << " (linear min dist " << linear_mindist << ")");
                // copy the path at the inverse direction
                for (int i = lastForwardPathMsg_.poses.size() - 1; i >= mindistindex; i--)
                {
                    auto &pose = lastForwardPathMsg_.poses[i];
                    ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] adding to plan i = " << i);
                    plan.push_back(pose);
                }
                ROS_WARN_STREAM("[UndoPathGlobalPlanner] refined plan has " << plan.size() << "  points");
            }
            else
            {
                ROS_ERROR_STREAM("[UndoPathGlobalPlanner ] backward global plan size:  " << plan.size());
            }
 
            return true;
        }

References costmap_ros_, and lastForwardPathMsg_.

Referenced by makePlan().

Here is the caller graph for this function:

◆ initialize()

void cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::initialize	(	std::string	name,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

overridevirtual

initialize()

Definition at line 50 of file undo_path_global_planner.cpp.

        {
            //ROS_INFO_NAMED("Backwards", "UndoPathGlobalPlanner initialize");
            costmap_ros_ = costmap_ros;
            //ROS_WARN_NAMED("Backwards", "initializating global planner, costmap address: %ld", (long)costmap_ros);
 
            forwardPathSub_ = nh_.subscribe("odom_tracker_path", 2, &UndoPathGlobalPlanner::onForwardTrailMsg, this);
 
            ros::NodeHandle nh;
            planPub_ = nh.advertise<nav_msgs::Path>("undo_path_planner/global_plan", 1);
            markersPub_ = nh.advertise<visualization_msgs::MarkerArray>("undo_path_planner/markers", 1);
        }

References costmap_ros_, forwardPathSub_, markersPub_, nh_, onForwardTrailMsg(), and planPub_.

Here is the call graph for this function:

◆ makePlan() [1/2]

bool cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::makePlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)

makePlan()

Definition at line 247 of file undo_path_global_planner.cpp.

        {
            //ROS_WARN_NAMED("Backwards", "Backwards global planner: Generating global plan ");
            //ROS_WARN_NAMED("Backwards", "Clearing...");
 
            plan.clear();
 
            if(lastForwardPathMsg_.poses.size() == 0)
            {
                return false;
            }
 
            auto forcedGoal = lastForwardPathMsg_.poses[lastForwardPathMsg_.poses.size() - 1]; // FORCE LAST POSE
            this->createDefaultUndoPathPlan(start, forcedGoal, plan);
            //this->createPureSpiningAndStragihtLineBackwardPath(start, goal, plan);
 
            //ROS_INFO_STREAM(" start - " << start);
            //ROS_INFO_STREAM(" end - " << goal.pose.position);
 
            //ROS_INFO("3 - heading to goal orientation");
            //double goalOrientation = angles::normalize_angle(tf::getYaw(goal.pose.orientation));
            //cl_move_base_z::makePureSpinningSubPlan(prevState,goalOrientation,plan);
 
            //ROS_WARN_STREAM( "MAKE PLAN INVOKED, plan size:"<< plan.size());
            publishGoalMarker(forcedGoal.pose, 1.0, 0, 1.0);
 
            nav_msgs::Path planMsg;
            planMsg.poses = plan;
            planMsg.header.frame_id = this->costmap_ros_->getGlobalFrameID();
 
            // check plan rejection
            bool acceptedGlobalPlan = true;
 
            // 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;
 
            costmap_2d::Costmap2D *costmap2d = this->costmap_ros_->getCostmap();
            for (auto &p : plan)
            {
                uint32_t mx, my;
                costmap2d->worldToMap(p.pose.position.x, p.pose.position.y, mx, my);
                auto cost = costmap2d->getCost(mx, my);
 
                if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
                {
                    acceptedGlobalPlan = false;
                    break;
                }
            }
 
            planPub_.publish(planMsg);
 
            // this was previously set to size() <= 1, but a plan with a single point is also a valid plan (the goal)
            return true;
        }

References costmap_ros_, createDefaultUndoPathPlan(), lastForwardPathMsg_, planPub_, and publishGoalMarker().

Referenced by makePlan().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ makePlan() [2/2]

bool cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::makePlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal,
		std::vector< geometry_msgs::PoseStamped > &	plan,
		double &	cost
	)

makePlan()

Definition at line 311 of file undo_path_global_planner.cpp.

        {
            cost = 0;
            makePlan(start, goal, plan);
            return true;
        }

References makePlan().

Here is the call graph for this function:

◆ onForwardTrailMsg()

void cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::onForwardTrailMsg ( const nav_msgs::Path::ConstPtr & trailMessage )

private

onForwardTrailMsg()

Definition at line 68 of file undo_path_global_planner.cpp.

        {
            lastForwardPathMsg_ = *trailMessage;
            ROS_DEBUG_STREAM("[UndoPathGlobalPlanner] received backward path msg poses [" << lastForwardPathMsg_.poses.size() << "]");
        }

References lastForwardPathMsg_.

Referenced by initialize().

Here is the caller graph for this function:

◆ publishGoalMarker()

void cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::publishGoalMarker	(	const geometry_msgs::Pose &	pose,
		double	r,
		double	g,
		double	b
	)

private

publishGoalMarker()

Definition at line 79 of file undo_path_global_planner.cpp.

        {
            double phi = tf::getYaw(pose.orientation);
            visualization_msgs::Marker marker;
            marker.header.frame_id = this->costmap_ros_->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.scale.x = 0.1;
            marker.scale.y = 0.3;
            marker.scale.z = 0.1;
            marker.color.a = 1.0;
            marker.color.r = r;
            marker.color.g = g;
            marker.color.b = b;
 
            geometry_msgs::Point start, end;
            start.x = pose.position.x;
            start.y = pose.position.y;
 
            end.x = pose.position.x + 0.5 * cos(phi);
            end.y = pose.position.y + 0.5 * sin(phi);
 
            marker.points.push_back(start);
            marker.points.push_back(end);
 
            visualization_msgs::MarkerArray ma;
            ma.markers.push_back(marker);
 
            markersPub_.publish(ma);
        }

References costmap_ros_, and markersPub_.

Referenced by makePlan().

Here is the caller graph for this function:

Member Data Documentation

◆ cmd_server_

ros::ServiceServer cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::cmd_server_

private

Definition at line 54 of file undo_path_global_planner.h.

◆ costmap_ros_

costmap_2d::Costmap2DROS* cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::costmap_ros_

private

stored but almost not used

Definition at line 48 of file undo_path_global_planner.h.

Referenced by createDefaultUndoPathPlan(), initialize(), makePlan(), and publishGoalMarker().

◆ forwardPathSub_

ros::Subscriber cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::forwardPathSub_

private

Definition at line 39 of file undo_path_global_planner.h.

Referenced by initialize().

◆ lastForwardPathMsg_

nav_msgs::Path cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::lastForwardPathMsg_

private

Definition at line 45 of file undo_path_global_planner.h.

Referenced by createDefaultUndoPathPlan(), makePlan(), and onForwardTrailMsg().

◆ markersPub_

ros::Publisher cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::markersPub_

private

Definition at line 43 of file undo_path_global_planner.h.

Referenced by initialize(), and publishGoalMarker().

◆ nh_

ros::NodeHandle cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::nh_

private

Definition at line 37 of file undo_path_global_planner.h.

Referenced by initialize().

◆ planPub_

ros::Publisher cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::planPub_

private

Definition at line 41 of file undo_path_global_planner.h.

Referenced by initialize(), makePlan(), and ~UndoPathGlobalPlanner().

◆ puresSpinningRadStep_

double cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::puresSpinningRadStep_

private

Definition at line 58 of file undo_path_global_planner.h.

◆ skip_straight_motion_distance_

double cl_move_base_z::undo_path_global_planner::UndoPathGlobalPlanner::skip_straight_motion_distance_

private

Definition at line 56 of file undo_path_global_planner.h.

Referenced by UndoPathGlobalPlanner().

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

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ UndoPathGlobalPlanner()

◆ ~UndoPathGlobalPlanner()

Member Function Documentation

◆ createDefaultUndoPathPlan()

◆ initialize()

◆ makePlan() [1/2]

◆ makePlan() [2/2]

◆ onForwardTrailMsg()

◆ publishGoalMarker()

Member Data Documentation

◆ cmd_server_

◆ costmap_ros_

◆ forwardPathSub_

◆ lastForwardPathMsg_

◆ markersPub_

◆ nh_

◆ planPub_

◆ puresSpinningRadStep_

◆ skip_straight_motion_distance_