コード例 #1
0
ファイル: footprint_layer.cpp プロジェクト: Renda110/AGVC
  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_ );
  }
コード例 #2
0
    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();
    }