void FootprintLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
if(!enabled_) return;
//update transformed polygon
footprint_.header.stamp = ros::Time::now();
footprint_.polygon.points.clear();
double cos_th = cos(origin_yaw);
double sin_th = sin(origin_yaw);
const std::vector<geometry_msgs::Point>& footprint_spec = getFootprint();
for(unsigned int i = 0; i < footprint_spec.size(); ++i)
{
geometry_msgs::Point32 new_pt;
new_pt.x = origin_x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = origin_y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
footprint_.polygon.points.push_back(new_pt);
}
for(unsigned int i=0; i < footprint_.polygon.points.size(); i++)
{
double px = footprint_.polygon.points[i].x, py = footprint_.polygon.points[i].y;
*min_x = std::min(px, *min_x);
*min_y = std::min(py, *min_y);
*max_x = std::max(px, *max_x);
*max_y = std::max(py, *max_y);
}
footprint_pub_.publish( footprint_ );
}
MoveBaseSBPL::MoveBaseSBPL()
: MoveBase(),
env_(0),
pMgr_(0),
goalCount_(0)
{
try {
// We're throwing int exit values if something goes wrong, and
// clean up any new instances in the catch clause. The sentry
// gets destructed when we go out of scope, so unlock() gets
// called no matter what.
sentry<MoveBaseSBPL> guard(this);
local_param("planStatsFile", planStatsFile_, string("/tmp/move_base_sbpl.log"));
local_param("plannerTimeLimit", plannerTimeLimit_, 10.0);
/*
if (0 > plannerTimeLimit_) {
int blah;
local_param("plannerTimeLimit", blah, -1); // parameters are picky about dots
if (0 > blah) {
ROS_ERROR("invalid or no %s/plannerTimeLimit specified: %g",
get_name().c_str(), plannerTimeLimit_);
throw int(7);
}
plannerTimeLimit_ = blah;
}
*/
string environmentType;
local_param("environmentType", environmentType, string("2D"));
if ("2D" == environmentType) {
// Initial Configuration is set with the threshold for
// obstacles set to the inscribed obstacle threshold. These,
// lethal obstacles, and cells with no information will thus
// be regarded as obstacles
env_ = new ompl::EnvironmentWrapper2D(getCostMap(), 0, 0, 0, 0,
CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
}
else if ("3DKIN" == environmentType) {
string const prefix("env3d/");
string obst_cost_thresh_str;
local_param(prefix + "obst_cost_thresh", obst_cost_thresh_str, string("lethal"));
unsigned char obst_cost_thresh(0);
if ("lethal" == obst_cost_thresh_str)
obst_cost_thresh = costmap_2d::CostMap2D::LETHAL_OBSTACLE;
else if ("inscribed" == obst_cost_thresh_str)
obst_cost_thresh = costmap_2d::CostMap2D::INSCRIBED_INFLATED_OBSTACLE;
else if ("circumscribed" == obst_cost_thresh_str)
obst_cost_thresh = costmap_2d::CostMap2D::CIRCUMSCRIBED_INFLATED_OBSTACLE;
else {
ROS_ERROR("invalid env3d/obst_cost_thresh \"%s\"\n"
" valid options: lethal, inscribed, or circumscribed",
obst_cost_thresh_str.c_str());
throw int(6);
}
double goaltol_x, goaltol_y, goaltol_theta,
nominalvel_mpersecs, timetoturn45degsinplace_secs;
local_param(prefix + "goaltol_x", goaltol_x, 0.3);
local_param(prefix + "goaltol_y", goaltol_y, 0.3);
local_param(prefix + "goaltol_theta", goaltol_theta, 30.0);
local_param(prefix + "nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
local_param(prefix + "timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
// Could also sanity check the other parameters...
env_ = new ompl::EnvironmentWrapper3DKIN(getCostMap(), obst_cost_thresh,
0, 0, 0, // start (x, y, th)
0, 0, 0, // goal (x, y, th)
goaltol_x, goaltol_y, goaltol_theta,
getFootprint(), nominalvel_mpersecs,
timetoturn45degsinplace_secs);
}
else {
ROS_ERROR("in MoveBaseSBPL ctor: invalid environmentType \"%s\", use 2D or 3DKIN",
environmentType.c_str());
throw int(2);
}
// This (weird?) order of inits is historical, could maybe be
// cleaned up...
bool const success(env_->InitializeMDPCfg(&mdpCfg_));
// try {
// // This one throws a string if something goes awry... and
// // always returns true. So no use cecking its
// // retval. Another candidate for cleanup.
// isMapDataOK();
// }
// catch (char const * ee) {
// ROS_ERROR("in MoveBaseSBPL ctor: isMapDataOK() said %s", ee);
// throw int(3);
// }
if ( ! success) {
ROS_ERROR("in MoveBaseSBPL ctor: env_->InitializeMDPCfg() failed");
throw int(4);
}
string plannerType;
local_param("plannerType", plannerType, string("ARAPlanner"));
pMgr_ = new ompl::SBPLPlannerManager(env_->getDSI(), false, &mdpCfg_);
if ( ! pMgr_->select(plannerType, false, 0)) {
ROS_ERROR("in MoveBaseSBPL ctor: pMgr_->select(%s) failed", plannerType.c_str());
throw int(5);
}
pStat_.pushBack(plannerType, environmentType);
}
catch (int ii) {
delete env_;
delete pMgr_;
exit(ii);
}
//Now initialize
initialize();
}
