cl_move_base_z::forward_global_planner::ForwardGlobalPlanner Class Reference

#include <forward_global_planner.h>

Inheritance diagram for cl_move_base_z::forward_global_planner::ForwardGlobalPlanner:

Collaboration diagram for cl_move_base_z::forward_global_planner::ForwardGlobalPlanner:

Public Member Functions
	ForwardGlobalPlanner ()
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)
void	initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros_)

Private Attributes
ros::NodeHandle	nh_
ros::Publisher	planPub_
costmap_2d::Costmap2DROS *	costmap_ros_
	stored but almost not used More...
double	skip_straight_motion_distance_
double	puresSpinningRadStep_

Detailed Description

Definition at line 16 of file forward_global_planner.h.

Constructor & Destructor Documentation

ForwardGlobalPlanner()

cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::ForwardGlobalPlanner

(

)

Definition at line 23 of file forward_global_planner.cpp.

    : nh_("~/ForwardGlobalPlanner")
{
    skip_straight_motion_distance_ = 0.2; //meters
    puresSpinningRadStep_ = 1000;         // rads
}

References puresSpinningRadStep_, and skip_straight_motion_distance_.

Member Function Documentation

initialize()

void cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::initialize	(	std::string	name,
		costmap_2d::Costmap2DROS *	costmap_ros_
	)

Definition at line 30 of file forward_global_planner.cpp.

{
    ROS_INFO("[Forward Global Planner] initializing");
    planPub_ = nh_.advertise<nav_msgs::Path>("global_plan", 1);
    skip_straight_motion_distance_ = 0.2; //meters
    puresSpinningRadStep_ = 1000;         // rads
    this->costmap_ros_ = costmap_ros;
}

References costmap_ros_, nh_, planPub_, puresSpinningRadStep_, and skip_straight_motion_distance_.

makePlan() [1/2]

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

Definition at line 48 of file forward_global_planner.cpp.

{
    //ROS_WARN_STREAM("Forward global plan goal: " << goal);
 
    //three stages: 1 - heading to goal position, 2 - going forward keep orientation, 3 - heading to goal orientation
 
    // 1 - heading to goal position
    // orientation direction
 
    double dx = goal.pose.position.x - start.pose.position.x;
    double dy = goal.pose.position.y - start.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);
        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_INFO("3 - heading to goal orientation");
    double goalOrientation = angles::normalize_angle(tf::getYaw(goal.pose.orientation));
    cl_move_base_z::makePureSpinningSubPlan(prevState, goalOrientation, plan, puresSpinningRadStep_);
 
    nav_msgs::Path planMsg;
    planMsg.poses = plan;
    planMsg.header.stamp = ros::Time::now();
 
    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;
        }
    }
 
    if (acceptedGlobalPlan)
    {
        planPub_.publish(planMsg);
        //ROS_INFO_STREAM("global forward plan: " << planMsg);
        return true;
    }
    else
    {
        return false;
    }
}

References costmap_ros_, cl_move_base_z::makePureSpinningSubPlan(), cl_move_base_z::makePureStraightSubPlan(), planPub_, 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:

makePlan() [2/2]

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

Definition at line 39 of file forward_global_planner.cpp.

{
    ROS_INFO("[Forward Global Planner] planning");
    cost = 0;
    return makePlan(start, goal, plan);
}

References makePlan().

Here is the call graph for this function:

Member Data Documentation

costmap_ros_

costmap_2d::Costmap2DROS* cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::costmap_ros_

private

stored but almost not used

Definition at line 36 of file forward_global_planner.h.

Referenced by initialize(), and makePlan().

nh_

ros::NodeHandle cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::nh_

private

Definition at line 31 of file forward_global_planner.h.

Referenced by initialize().

planPub_

ros::Publisher cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::planPub_

private

Definition at line 33 of file forward_global_planner.h.

Referenced by initialize(), and makePlan().

puresSpinningRadStep_

double cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::puresSpinningRadStep_

private

Definition at line 40 of file forward_global_planner.h.

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

skip_straight_motion_distance_

double cl_move_base_z::forward_global_planner::ForwardGlobalPlanner::skip_straight_motion_distance_

private

Definition at line 38 of file forward_global_planner.h.

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

---

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

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