git commit -m "first commit"
This commit is contained in:
602
navigations/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp
Executable file
602
navigations/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp
Executable file
@@ -0,0 +1,602 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Mike Phillips
|
||||
*********************************************************************/
|
||||
|
||||
#include <sbpl_lattice_planner/sbpl_lattice_planner.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <sbpl_lattice_planner/SBPLLatticePlannerStats.h>
|
||||
|
||||
#include <costmap_2d/inflation_layer.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf/transform_datatypes.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace ros;
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(sbpl_lattice_planner::SBPLLatticePlanner, nav_core::BaseGlobalPlanner)
|
||||
|
||||
namespace geometry_msgs
|
||||
{
|
||||
bool operator==(const Point &p1, const Point &p2)
|
||||
{
|
||||
return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
|
||||
}
|
||||
}
|
||||
|
||||
namespace sbpl_lattice_planner
|
||||
{
|
||||
|
||||
class LatticeSCQ : public StateChangeQuery
|
||||
{
|
||||
public:
|
||||
LatticeSCQ(EnvironmentNAVXYTHETALAT *env, std::vector<nav2dcell_t> const &changedcellsV)
|
||||
: env_(env), changedcellsV_(changedcellsV)
|
||||
{
|
||||
}
|
||||
|
||||
// lazy init, because we do not always end up calling this method
|
||||
virtual std::vector<int> const *getPredecessors() const
|
||||
{
|
||||
if (predsOfChangedCells_.empty() && !changedcellsV_.empty())
|
||||
env_->GetPredsofChangedEdges(&changedcellsV_, &predsOfChangedCells_);
|
||||
return &predsOfChangedCells_;
|
||||
}
|
||||
|
||||
// lazy init, because we do not always end up calling this method
|
||||
virtual std::vector<int> const *getSuccessors() const
|
||||
{
|
||||
if (succsOfChangedCells_.empty() && !changedcellsV_.empty())
|
||||
env_->GetSuccsofChangedEdges(&changedcellsV_, &succsOfChangedCells_);
|
||||
return &succsOfChangedCells_;
|
||||
}
|
||||
|
||||
EnvironmentNAVXYTHETALAT *env_;
|
||||
std::vector<nav2dcell_t> const &changedcellsV_;
|
||||
mutable std::vector<int> predsOfChangedCells_;
|
||||
mutable std::vector<int> succsOfChangedCells_;
|
||||
};
|
||||
|
||||
SBPLLatticePlanner::SBPLLatticePlanner()
|
||||
: initialized_(false), costmap_ros_(NULL)
|
||||
{
|
||||
}
|
||||
|
||||
SBPLLatticePlanner::SBPLLatticePlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
|
||||
: initialized_(false), costmap_ros_(NULL)
|
||||
{
|
||||
initialize(name, costmap_ros);
|
||||
}
|
||||
|
||||
void SBPLLatticePlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
ros::NodeHandle private_nh("~/" + name);
|
||||
|
||||
ROS_INFO("Name is %s", name.c_str());
|
||||
|
||||
private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
|
||||
private_nh.param("allocated_time", allocated_time_, 10.0);
|
||||
private_nh.param("initial_epsilon", initial_epsilon_, 3.0);
|
||||
private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
|
||||
private_nh.param("forward_search", forward_search_, bool(false));
|
||||
private_nh.param("primitive_filename", primitive_filename_, string(""));
|
||||
private_nh.param("force_scratch_limit", force_scratch_limit_, 500);
|
||||
|
||||
double nominalvel_mpersecs, timetoturn45degsinplace_secs;
|
||||
private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
|
||||
private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
|
||||
|
||||
int lethal_obstacle;
|
||||
private_nh.param("lethal_obstacle", lethal_obstacle, 20);
|
||||
lethal_obstacle_ = (unsigned char)lethal_obstacle;
|
||||
inscribed_inflated_obstacle_ = lethal_obstacle_ - 1;
|
||||
sbpl_cost_multiplier_ = (unsigned char)(costmap_2d::INSCRIBED_INFLATED_OBSTACLE / inscribed_inflated_obstacle_ + 1);
|
||||
ROS_INFO("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz", lethal_obstacle, inscribed_inflated_obstacle_, sbpl_cost_multiplier_);
|
||||
|
||||
private_nh.param("publish_footprint_path", publish_footprint_path_, bool(true));
|
||||
private_nh.param<int>("visualizer_skip_poses", visualizer_skip_poses_, 5);
|
||||
|
||||
private_nh.param("allow_unknown", allow_unknown_, bool(true));
|
||||
|
||||
name_ = name;
|
||||
costmap_ros_ = costmap_ros;
|
||||
|
||||
footprint_ = costmap_ros_->getRobotFootprint();
|
||||
|
||||
if ("XYThetaLattice" == environment_type_)
|
||||
{
|
||||
ROS_DEBUG("Using a 3D costmap for theta lattice\n");
|
||||
env_ = new EnvironmentNAVXYTHETALAT();
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("XYThetaLattice is currently the only supported environment!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
circumscribed_cost_ = computeCircumscribedCost();
|
||||
|
||||
if (circumscribed_cost_ == 0)
|
||||
{
|
||||
// Unfortunately, the inflation_radius is not taken into account by
|
||||
// inflation_layer->computeCost(). If inflation_radius is smaller than
|
||||
// the circumscribed radius, SBPL will ignore some obstacles, but we
|
||||
// cannot detect this problem. If the cost_scaling_factor is too large,
|
||||
// SBPL won't run into obstacles, but will always perform an expensive
|
||||
// footprint check, no matter how far the nearest obstacle is.
|
||||
ROS_WARN("The costmap value at the robot's circumscribed radius (%f m) is 0.", costmap_ros_->getLayeredCostmap()->getCircumscribedRadius());
|
||||
ROS_WARN("SBPL performance will suffer.");
|
||||
ROS_WARN("Please decrease the costmap's cost_scaling_factor.");
|
||||
}
|
||||
if (!env_->SetEnvParameter("cost_inscribed_thresh", costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)))
|
||||
{
|
||||
ROS_ERROR("Failed to set cost_inscribed_thresh parameter");
|
||||
exit(1);
|
||||
}
|
||||
if (!env_->SetEnvParameter("cost_possibly_circumscribed_thresh", circumscribed_cost_))
|
||||
{
|
||||
ROS_ERROR("Failed to set cost_possibly_circumscribed_thresh parameter");
|
||||
exit(1);
|
||||
}
|
||||
int obst_cost_thresh = costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE);
|
||||
vector<sbpl_2Dpt_t> perimeterptsV;
|
||||
perimeterptsV.reserve(footprint_.size());
|
||||
for (size_t ii(0); ii < footprint_.size(); ++ii)
|
||||
{
|
||||
sbpl_2Dpt_t pt;
|
||||
pt.x = footprint_[ii].x;
|
||||
pt.y = footprint_[ii].y;
|
||||
perimeterptsV.push_back(pt);
|
||||
}
|
||||
|
||||
bool ret;
|
||||
try
|
||||
{
|
||||
ret = env_->InitializeEnv(costmap_ros_->getCostmap()->getSizeInCellsX(), // width
|
||||
costmap_ros_->getCostmap()->getSizeInCellsY(), // height
|
||||
0, // mapdata
|
||||
0, 0, 0, // start (x, y, theta, t)
|
||||
0, 0, 0, // goal (x, y, theta)
|
||||
0, 0, 0, // goal tolerance
|
||||
perimeterptsV, costmap_ros_->getCostmap()->getResolution(), nominalvel_mpersecs,
|
||||
timetoturn45degsinplace_secs, obst_cost_thresh,
|
||||
primitive_filename_.c_str());
|
||||
current_env_width_ = costmap_ros_->getCostmap()->getSizeInCellsX();
|
||||
current_env_height_ = costmap_ros_->getCostmap()->getSizeInCellsY();
|
||||
}
|
||||
catch (SBPL_Exception *e)
|
||||
{
|
||||
ROS_ERROR("SBPL encountered a fatal exception: %s", e->what());
|
||||
ret = false;
|
||||
}
|
||||
if (!ret)
|
||||
{
|
||||
ROS_ERROR("SBPL initialization failed!");
|
||||
exit(1);
|
||||
}
|
||||
for (ssize_t ix(0); ix < costmap_ros_->getCostmap()->getSizeInCellsX(); ++ix)
|
||||
for (ssize_t iy(0); iy < costmap_ros_->getCostmap()->getSizeInCellsY(); ++iy)
|
||||
env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix, iy)));
|
||||
|
||||
if ("ARAPlanner" == planner_type_)
|
||||
{
|
||||
ROS_INFO("Planning with ARA*");
|
||||
planner_ = new ARAPlanner(env_, forward_search_);
|
||||
}
|
||||
else if ("ADPlanner" == planner_type_)
|
||||
{
|
||||
ROS_INFO("Planning with AD*");
|
||||
planner_ = new ADPlanner(env_, forward_search_);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("ARAPlanner and ADPlanner are currently the only supported planners!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ROS_INFO("[sbpl_lattice_planner] Initialized successfully");
|
||||
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
|
||||
stats_publisher_ = private_nh.advertise<sbpl_lattice_planner::SBPLLatticePlannerStats>("sbpl_lattice_planner_stats", 1);
|
||||
sbpl_plan_footprint_pub_ = private_nh.advertise<visualization_msgs::Marker>("footprint_markers", 1);
|
||||
|
||||
initialized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Taken from Sachin's sbpl_cart_planner
|
||||
// This rescales the costmap according to a rosparam which sets the obstacle cost
|
||||
unsigned char SBPLLatticePlanner::costMapCostToSBPLCost(unsigned char newcost)
|
||||
{
|
||||
if (newcost == costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == costmap_2d::NO_INFORMATION))
|
||||
return lethal_obstacle_;
|
||||
else if (newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|
||||
return inscribed_inflated_obstacle_;
|
||||
else if (newcost == 0)
|
||||
return 0;
|
||||
else if(newcost == costmap_2d::NO_INFORMATION)
|
||||
return costmap_2d::FREE_SPACE/sbpl_cost_multiplier_;
|
||||
else
|
||||
{
|
||||
unsigned char sbpl_cost = newcost / sbpl_cost_multiplier_;
|
||||
if (sbpl_cost == 0)
|
||||
sbpl_cost = 1;
|
||||
return sbpl_cost;
|
||||
}
|
||||
}
|
||||
|
||||
void SBPLLatticePlanner::publishStats(int solution_cost, int solution_size,
|
||||
const geometry_msgs::PoseStamped &start,
|
||||
const geometry_msgs::PoseStamped &goal)
|
||||
{
|
||||
// Fill up statistics and publish
|
||||
sbpl_lattice_planner::SBPLLatticePlannerStats stats;
|
||||
stats.initial_epsilon = initial_epsilon_;
|
||||
stats.plan_to_first_solution = false;
|
||||
stats.final_number_of_expands = planner_->get_n_expands();
|
||||
stats.allocated_time = allocated_time_;
|
||||
|
||||
stats.time_to_first_solution = planner_->get_initial_eps_planning_time();
|
||||
stats.actual_time = planner_->get_final_eps_planning_time();
|
||||
stats.number_of_expands_initial_solution = planner_->get_n_expands_init_solution();
|
||||
stats.final_epsilon = planner_->get_final_epsilon();
|
||||
|
||||
stats.solution_cost = solution_cost;
|
||||
stats.path_size = solution_size;
|
||||
stats.start = start;
|
||||
stats.goal = goal;
|
||||
stats_publisher_.publish(stats);
|
||||
}
|
||||
|
||||
unsigned char SBPLLatticePlanner::computeCircumscribedCost()
|
||||
{
|
||||
unsigned char result = 0;
|
||||
|
||||
if (!costmap_ros_)
|
||||
{
|
||||
ROS_ERROR("Costmap is not initialized");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// check if the costmap has an inflation layer
|
||||
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_ros_->getLayeredCostmap()->getPlugins()->begin();
|
||||
layer != costmap_ros_->getLayeredCostmap()->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
|
||||
if (!inflation_layer)
|
||||
continue;
|
||||
|
||||
result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_ros_->getLayeredCostmap()->getCircumscribedRadius() / costmap_ros_->getCostmap()->getResolution()));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool SBPLLatticePlanner::makePlan(const geometry_msgs::PoseStamped &start,
|
||||
const geometry_msgs::PoseStamped &goal,
|
||||
std::vector<geometry_msgs::PoseStamped> &plan)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
ROS_ERROR("Global planner is not initialized");
|
||||
return false;
|
||||
}
|
||||
|
||||
bool do_init = false;
|
||||
if (current_env_width_ != costmap_ros_->getCostmap()->getSizeInCellsX() ||
|
||||
current_env_height_ != costmap_ros_->getCostmap()->getSizeInCellsY())
|
||||
{
|
||||
ROS_INFO("Costmap dimensions have changed from (%d x %d) to (%d x %d), reinitializing sbpl_lattice_planner.",
|
||||
current_env_width_, current_env_height_,
|
||||
costmap_ros_->getCostmap()->getSizeInCellsX(), costmap_ros_->getCostmap()->getSizeInCellsY());
|
||||
do_init = true;
|
||||
}
|
||||
else if (footprint_ != costmap_ros_->getRobotFootprint())
|
||||
{
|
||||
ROS_INFO("Robot footprint has changed, reinitializing sbpl_lattice_planner.");
|
||||
do_init = true;
|
||||
}
|
||||
else if (circumscribed_cost_ != computeCircumscribedCost())
|
||||
{
|
||||
ROS_INFO("Cost at circumscribed radius has changed, reinitializing sbpl_lattice_planner.");
|
||||
do_init = true;
|
||||
}
|
||||
|
||||
if (do_init)
|
||||
{
|
||||
initialized_ = false;
|
||||
delete planner_;
|
||||
planner_ = NULL;
|
||||
delete env_;
|
||||
env_ = NULL;
|
||||
initialize(name_, costmap_ros_);
|
||||
}
|
||||
|
||||
plan.clear();
|
||||
|
||||
ROS_INFO("[sbpl_lattice_planner] getting start point (%g,%g) goal point (%g,%g)",
|
||||
start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
|
||||
double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
|
||||
double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
|
||||
|
||||
try
|
||||
{
|
||||
int ret = env_->SetStart(start.pose.position.x - costmap_ros_->getCostmap()->getOriginX(), start.pose.position.y - costmap_ros_->getCostmap()->getOriginY(), theta_start);
|
||||
if (ret < 0 || planner_->set_start(ret) == 0)
|
||||
{
|
||||
ROS_ERROR("ERROR: failed to set start state\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
catch (SBPL_Exception *e)
|
||||
{
|
||||
ROS_ERROR("SBPL encountered a fatal exception while setting the start state");
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
int ret = env_->SetGoal(goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX(), goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY(), theta_goal);
|
||||
if (ret < 0 || planner_->set_goal(ret) == 0)
|
||||
{
|
||||
ROS_ERROR("ERROR: failed to set goal state\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
catch (SBPL_Exception *e)
|
||||
{
|
||||
ROS_ERROR("SBPL encountered a fatal exception while setting the goal state");
|
||||
return false;
|
||||
}
|
||||
|
||||
int offOnCount = 0;
|
||||
int onOffCount = 0;
|
||||
int allCount = 0;
|
||||
vector<nav2dcell_t> changedcellsV;
|
||||
|
||||
for (unsigned int ix = 0; ix < costmap_ros_->getCostmap()->getSizeInCellsX(); ix++)
|
||||
{
|
||||
for (unsigned int iy = 0; iy < costmap_ros_->getCostmap()->getSizeInCellsY(); iy++)
|
||||
{
|
||||
|
||||
unsigned char oldCost = env_->GetMapCost(ix, iy);
|
||||
unsigned char newCost = costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix, iy));
|
||||
|
||||
if (oldCost == newCost)
|
||||
continue;
|
||||
|
||||
allCount++;
|
||||
|
||||
// first case - off cell goes on
|
||||
|
||||
if ((oldCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && oldCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
|
||||
(newCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || newCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)))
|
||||
{
|
||||
offOnCount++;
|
||||
}
|
||||
|
||||
if ((oldCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || oldCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
|
||||
(newCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && newCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)))
|
||||
{
|
||||
onOffCount++;
|
||||
}
|
||||
env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix, iy)));
|
||||
|
||||
nav2dcell_t nav2dcell;
|
||||
nav2dcell.x = ix;
|
||||
nav2dcell.y = iy;
|
||||
changedcellsV.push_back(nav2dcell);
|
||||
}
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (!changedcellsV.empty())
|
||||
{
|
||||
StateChangeQuery *scq = new LatticeSCQ(env_, changedcellsV);
|
||||
planner_->costs_changed(*scq);
|
||||
delete scq;
|
||||
}
|
||||
|
||||
if (allCount > force_scratch_limit_)
|
||||
planner_->force_planning_from_scratch();
|
||||
}
|
||||
catch (SBPL_Exception *e)
|
||||
{
|
||||
ROS_ERROR("SBPL failed to update the costmap");
|
||||
return false;
|
||||
}
|
||||
|
||||
// setting planner parameters
|
||||
ROS_DEBUG("allocated:%f, init eps:%f\n", allocated_time_, initial_epsilon_);
|
||||
planner_->set_initialsolution_eps(initial_epsilon_);
|
||||
planner_->set_search_mode(false);
|
||||
|
||||
ROS_DEBUG("[sbpl_lattice_planner] run planner");
|
||||
vector<int> solution_stateIDs;
|
||||
int solution_cost;
|
||||
try
|
||||
{
|
||||
int ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
|
||||
if (ret)
|
||||
ROS_DEBUG("Solution is found\n");
|
||||
else
|
||||
{
|
||||
ROS_INFO("Solution not found\n");
|
||||
publishStats(solution_cost, 0, start, goal);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
catch (SBPL_Exception *e)
|
||||
{
|
||||
ROS_ERROR("SBPL encountered a fatal exception while planning");
|
||||
return false;
|
||||
}
|
||||
|
||||
ROS_DEBUG("size of solution=%d", (int)solution_stateIDs.size());
|
||||
|
||||
vector<EnvNAVXYTHETALAT3Dpt_t> sbpl_path;
|
||||
try
|
||||
{
|
||||
env_->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
|
||||
}
|
||||
catch (SBPL_Exception *e)
|
||||
{
|
||||
ROS_ERROR("SBPL encountered a fatal exception while reconstructing the path");
|
||||
return false;
|
||||
}
|
||||
// if the plan has zero points, add a single point to make move_base happy
|
||||
if (sbpl_path.size() == 0)
|
||||
{
|
||||
EnvNAVXYTHETALAT3Dpt_t s(
|
||||
start.pose.position.x - costmap_ros_->getCostmap()->getOriginX(),
|
||||
start.pose.position.y - costmap_ros_->getCostmap()->getOriginY(),
|
||||
theta_start);
|
||||
sbpl_path.push_back(s);
|
||||
}
|
||||
|
||||
ROS_DEBUG("Plan has %d points.\n", (int)sbpl_path.size());
|
||||
if (sbpl_path.back().x != goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX() ||
|
||||
sbpl_path.back().y != goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY() ||
|
||||
sbpl_path.back().theta != theta_goal)
|
||||
|
||||
{
|
||||
EnvNAVXYTHETALAT3Dpt_t goal_stemp(goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX(),
|
||||
goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY(),
|
||||
theta_goal);
|
||||
sbpl_path.push_back(goal_stemp);
|
||||
}
|
||||
|
||||
ros::Time plan_time = ros::Time::now();
|
||||
|
||||
if (publish_footprint_path_)
|
||||
{
|
||||
visualization_msgs::Marker sbpl_plan_footprint;
|
||||
getFootprintList(sbpl_path, costmap_ros_->getGlobalFrameID(), sbpl_plan_footprint);
|
||||
sbpl_plan_footprint_pub_.publish(sbpl_plan_footprint);
|
||||
}
|
||||
|
||||
// create a message for the plan
|
||||
nav_msgs::Path gui_path;
|
||||
gui_path.poses.resize(sbpl_path.size());
|
||||
gui_path.header.frame_id = costmap_ros_->getGlobalFrameID();
|
||||
gui_path.header.stamp = plan_time;
|
||||
for (unsigned int i = 0; i < sbpl_path.size(); i++)
|
||||
{
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.header.stamp = plan_time;
|
||||
pose.header.frame_id = costmap_ros_->getGlobalFrameID();
|
||||
|
||||
pose.pose.position.x = sbpl_path[i].x + costmap_ros_->getCostmap()->getOriginX();
|
||||
pose.pose.position.y = sbpl_path[i].y + costmap_ros_->getCostmap()->getOriginY();
|
||||
pose.pose.position.z = start.pose.position.z;
|
||||
|
||||
tf2::Quaternion temp;
|
||||
temp.setRPY(0, 0, sbpl_path[i].theta);
|
||||
pose.pose.orientation.x = temp.getX();
|
||||
pose.pose.orientation.y = temp.getY();
|
||||
pose.pose.orientation.z = temp.getZ();
|
||||
pose.pose.orientation.w = temp.getW();
|
||||
|
||||
plan.push_back(pose);
|
||||
|
||||
gui_path.poses[i] = plan[i];
|
||||
}
|
||||
plan_pub_.publish(gui_path);
|
||||
publishStats(solution_cost, sbpl_path.size(), start, goal);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SBPLLatticePlanner::getFootprintList(const std::vector<EnvNAVXYTHETALAT3Dpt_t> &sbpl_path,
|
||||
const std::string &path_frame_id, visualization_msgs::Marker &ma)
|
||||
{
|
||||
ma.header.frame_id = path_frame_id;
|
||||
ma.header.stamp = ros::Time();
|
||||
ma.ns = "sbpl_robot_footprint";
|
||||
ma.id = 0;
|
||||
ma.type = visualization_msgs::Marker::LINE_LIST;
|
||||
ma.action = visualization_msgs::Marker::ADD;
|
||||
ma.scale.x = 0.05;
|
||||
ma.color.a = 1.0;
|
||||
ma.color.r = 0.0;
|
||||
ma.color.g = 0.0;
|
||||
ma.color.b = 1.0;
|
||||
ma.pose.orientation.w = 1.0;
|
||||
|
||||
for (unsigned int i = 0; i < sbpl_path.size(); i = i + visualizer_skip_poses_)
|
||||
{
|
||||
std::vector<geometry_msgs::Point> transformed_rfp;
|
||||
geometry_msgs::Pose robot_pose;
|
||||
robot_pose.position.x = sbpl_path[i].x + costmap_ros_->getCostmap()->getOriginX();
|
||||
;
|
||||
robot_pose.position.y = sbpl_path[i].y + costmap_ros_->getCostmap()->getOriginY();
|
||||
;
|
||||
robot_pose.position.z = 0.0;
|
||||
tf::Quaternion quat;
|
||||
quat.setRPY(0.0, 0.0, sbpl_path[i].theta);
|
||||
tf::quaternionTFToMsg(quat, robot_pose.orientation);
|
||||
transformFootprintToEdges(robot_pose, footprint_, transformed_rfp);
|
||||
|
||||
for (auto &point : transformed_rfp)
|
||||
ma.points.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
void SBPLLatticePlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose,
|
||||
const std::vector<geometry_msgs::Point> &footprint,
|
||||
std::vector<geometry_msgs::Point> &out_footprint)
|
||||
{
|
||||
out_footprint.resize(2 * footprint.size());
|
||||
double yaw = tf::getYaw(robot_pose.orientation);
|
||||
for (unsigned int i = 0; i < footprint.size(); i++)
|
||||
{
|
||||
out_footprint[2 * i].x = robot_pose.position.x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y;
|
||||
out_footprint[2 * i].y = robot_pose.position.y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y;
|
||||
if (i == 0)
|
||||
{
|
||||
out_footprint.back().x = out_footprint[i].x;
|
||||
out_footprint.back().y = out_footprint[i].y;
|
||||
}
|
||||
else
|
||||
{
|
||||
out_footprint[2 * i - 1].x = out_footprint[2 * i].x;
|
||||
out_footprint[2 * i - 1].y = out_footprint[2 * i].y;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
Reference in New Issue
Block a user