cl_move_base_z::backward_global_planner::BackwardGlobalPlanner Class Reference

#include <backward_global_planner.h>

Inheritance diagram for cl_move_base_z::backward_global_planner::BackwardGlobalPlanner:

Collaboration diagram for cl_move_base_z::backward_global_planner::BackwardGlobalPlanner:

Public Member Functions
	BackwardGlobalPlanner ()
virtual	~BackwardGlobalPlanner ()
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	createDefaultBackwardPath (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::Publisher	planPub_
ros::Publisher	markersPub_
costmap_2d::Costmap2DROS *	costmap_ros_
double	skip_straight_motion_distance_
double	puresSpinningRadStep_

Detailed Description

Definition at line 17 of file backward_global_planner.h.

Constructor & Destructor Documentation

BackwardGlobalPlanner()

cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::BackwardGlobalPlanner

(

)

Constructor()

Definition at line 32 of file backward_global_planner.cpp.

{
    skip_straight_motion_distance_ = 0.2;
}

References skip_straight_motion_distance_.

~BackwardGlobalPlanner()

cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::~BackwardGlobalPlanner ( )

virtual

Definition at line 37 of file backward_global_planner.cpp.

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

References planPub_.

Member Function Documentation

createDefaultBackwardPath()

bool cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::createDefaultBackwardPath ( const geometry_msgs::PoseStamped &  start, const geometry_msgs::PoseStamped &  goal, std::vector< geometry_msgs::PoseStamped > &  plan  )

virtual

defaultBackwardPath()

Definition at line 105 of file backward_global_planner.cpp.

{
    auto q = start.pose.orientation;
 
    geometry_msgs::PoseStamped pose;
    pose = start;
 
    double dx = start.pose.position.x - goal.pose.position.x;
    double dy = start.pose.position.y - goal.pose.position.y;
 
    double length = sqrt(dx * dx + dy * dy);
 
    geometry_msgs::PoseStamped prevState;
    if (length > skip_straight_motion_distance_)
    {
        // skip initial pure spinning and initial straight motion
        //ROS_INFO("1 - heading to goal position pure spinning");
        double heading_direction = atan2(dy, dx);
        double startyaw = tf::getYaw(q);
        double offset = angles::shortest_angular_distance(startyaw, heading_direction);
        heading_direction = startyaw + offset;
 
        prevState = cl_move_base_z::makePureSpinningSubPlan(start, heading_direction, plan, puresSpinningRadStep_);
        //ROS_INFO("2 - going forward keep orientation pure straight");
 
        prevState = cl_move_base_z::makePureStraightSubPlan(prevState, goal.pose.position, length, plan);
    }
    else
    {
        prevState = start;
    }
 
    ROS_WARN_STREAM( "[BackwardGlobalPlanner ] backward global plan size:  " <<plan.size());
    return true;
}

References cl_move_base_z::makePureSpinningSubPlan(), cl_move_base_z::makePureStraightSubPlan(), puresSpinningRadStep_, and skip_straight_motion_distance_.

Referenced by makePlan().

Here is the call graph for this function:

Here is the caller graph for this function:

initialize()

void cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::initialize ( std::string  name, costmap_2d::Costmap2DROS *  costmap_ros  )

overridevirtual

initialize()

Definition at line 50 of file backward_global_planner.cpp.

{
    //ROS_INFO_NAMED("Backwards", "BackwardGlobalPlanner initialize");
    costmap_ros_ = costmap_ros;
    //ROS_WARN_NAMED("Backwards", "initializating global planner, costmap address: %ld", (long)costmap_ros);
 
    ros::NodeHandle nh;
    planPub_ = nh.advertise<nav_msgs::Path>("backward_planner/global_plan", 1);
    markersPub_ = nh.advertise<visualization_msgs::MarkerArray>("backward_planner/markers", 1);
}

References costmap_ros_, markersPub_, and planPub_.

makePlan() [1/2]

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

makePlan()

Definition at line 147 of file backward_global_planner.cpp.

{
    //ROS_WARN_NAMED("Backwards", "Backwards global planner: Generating global plan ");
    //ROS_WARN_NAMED("Backwards", "Clearing...");
 
    plan.clear();
 
    this->createDefaultBackwardPath(start, goal, 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(goal.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_, createDefaultBackwardPath(), 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::backward_global_planner::BackwardGlobalPlanner::makePlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal,
		std::vector< geometry_msgs::PoseStamped > &	plan,
		double &	cost
	)

makePlan()

Definition at line 205 of file backward_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::backward_global_planner::BackwardGlobalPlanner::onForwardTrailMsg ( const nav_msgs::Path::ConstPtr &  trailMessage )

private

publishGoalMarker()

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

private

publishGoalMarker()

Definition at line 66 of file backward_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

costmap_ros_

costmap_2d::Costmap2DROS* cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::costmap_ros_

private

Definition at line 43 of file backward_global_planner.h.

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

markersPub_

ros::Publisher cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::markersPub_

private

Definition at line 41 of file backward_global_planner.h.

Referenced by initialize(), and publishGoalMarker().

nh_

ros::NodeHandle cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::nh_

private

Definition at line 37 of file backward_global_planner.h.

planPub_

ros::Publisher cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::planPub_

private

Definition at line 39 of file backward_global_planner.h.

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

puresSpinningRadStep_

double cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::puresSpinningRadStep_

private

Definition at line 51 of file backward_global_planner.h.

Referenced by createDefaultBackwardPath().

skip_straight_motion_distance_

double cl_move_base_z::backward_global_planner::BackwardGlobalPlanner::skip_straight_motion_distance_

private

Definition at line 49 of file backward_global_planner.h.

Referenced by BackwardGlobalPlanner(), and createDefaultBackwardPath().

---

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

• smacc_client_library/move_base_z_client/custom_planners/backward_global_planner/include/backward_global_planner/backward_global_planner.h
• smacc_client_library/move_base_z_client/custom_planners/backward_global_planner/src/backward_global_planner.cpp