00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <sbpl_cart_planner/sbpl_cart_planner.h>
00039 #include <cart_pushing_msgs/RobotCartPath.h>
00040 #include <pluginlib/class_list_macros.h>
00041 #include <nav_msgs/Path.h>
00042 #include <sbpl_cart_planner/SBPLCartPlannerStats.h>
00043
00044 using namespace std;
00045 using namespace ros;
00046
00047
00048
00049 PLUGINLIB_REGISTER_CLASS(SBPLCartPlanner, SBPLCartPlanner, nav_core::BaseGlobalPlanner);
00050
00051 class LatticeSCQ : public StateChangeQuery{
00052 public:
00053 LatticeSCQ(EnvironmentNAVXYTHETACARTLAT* env, std::vector<nav2dcell_t> const & changedcellsV)
00054 : env_(env), changedcellsV_(changedcellsV) {
00055 }
00056
00057
00058 virtual std::vector<int> const * getPredecessors() const{
00059 if(predsOfChangedCells_.empty() && !changedcellsV_.empty())
00060 env_->GetPredsofChangedEdges(&changedcellsV_, &predsOfChangedCells_);
00061 return &predsOfChangedCells_;
00062 }
00063
00064
00065 virtual std::vector<int> const * getSuccessors() const{
00066 if(succsOfChangedCells_.empty() && !changedcellsV_.empty())
00067 env_->GetSuccsofChangedEdges(&changedcellsV_, &succsOfChangedCells_);
00068 return &succsOfChangedCells_;
00069 }
00070
00071 EnvironmentNAVXYTHETACARTLAT * env_;
00072 std::vector<nav2dcell_t> const & changedcellsV_;
00073 mutable std::vector<int> predsOfChangedCells_;
00074 mutable std::vector<int> succsOfChangedCells_;
00075 };
00076
00077 double SBPLCartPlanner::sign(double x)
00078 {
00079 return x < 0.0 ? -1.0 : 1.0;
00080 }
00081
00082 SBPLCartPlanner::SBPLCartPlanner()
00083 : initialized_(false), costmap_ros_(NULL){
00084 }
00085
00086 SBPLCartPlanner::SBPLCartPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00087 : initialized_(false), costmap_ros_(NULL){
00088 initialize(name, costmap_ros);
00089 if(!initialized_) {
00090 ROS_ERROR("Could not initialize global planner");
00091 }
00092 }
00093
00094
00095 void SBPLCartPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
00096 if(!initialized_){
00097 num_sbpl_markers_ = 0;
00098 ros::NodeHandle private_nh("~/"+name);
00099 ros::NodeHandle nh(name);
00100
00101 ROS_DEBUG("Name is %s", name.c_str());
00102 ROS_DEBUG("Costmap resolution: %.2f", costmap_ros->getResolution());
00103
00104 private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
00105 private_nh.param("allocated_time", allocated_time_, 10.0);
00106 private_nh.param("initial_epsilon",initial_epsilon_,3.0);
00107 private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
00108 private_nh.param("forward_search", forward_search_, bool(false));
00109 private_nh.param("primitive_filename",primitive_filename_,string(""));
00110 private_nh.param("force_scratch_limit",force_scratch_limit_,500);
00111
00112 double nominalvel_mpersecs, timetoturn45degsinplace_secs;
00113 private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
00114 private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
00115
00116 private_nh.param<int>("visualizer_skip_poses",visualizer_skip_poses_,5);
00117
00118 int lethal_obstacle;
00119 private_nh.param("lethal_obstacle",lethal_obstacle,20);
00120 lethal_obstacle_ = (unsigned char) lethal_obstacle;
00121 inscribed_inflated_obstacle_ = lethal_obstacle_-1;
00122 sbpl_cost_multiplier_ = (unsigned char) (costmap_2d::INSCRIBED_INFLATED_OBSTACLE/inscribed_inflated_obstacle_ + 1);
00123
00124 ROS_DEBUG("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz",lethal_obstacle,inscribed_inflated_obstacle_,sbpl_cost_multiplier_);
00125 costmap_ros_ = costmap_ros;
00126 costmap_ros_->clearRobotFootprint();
00127
00128
00129 costmap_ros_->getCostmapCopy(cost_map_);
00130
00131 std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
00132 robot_footprint_ = footprint;
00133 if ("XYThetaLattice" == environment_type_){
00134 ROS_DEBUG("Using a 3D costmap for theta lattice\n");
00135 env_ = new EnvironmentNAVXYTHETACARTLAT();
00136 }
00137 else
00138 {
00139 ROS_FATAL("XYThetaLattice is currently the only supported environment!\n");
00140 return;
00141 }
00142
00143 if(!env_->SetEnvParameter("cost_inscribed_thresh",inscribed_inflated_obstacle_))
00144 {
00145 ROS_FATAL("Failed to set cost_inscribed_thresh parameter");
00146 return;
00147 }
00148
00149 int obst_cost_thresh = costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE);
00150 vector<sbpl_2Dpt_t> perimeterptsV, cart_perimeterptsV;
00151 private_nh.param("cart_pivot_offset/x", cart_offset_.x, 0.35);
00152 private_nh.param("cart_pivot_offset/y", cart_offset_.y, 0.0);
00153 ROS_DEBUG("Cart pivot point offset (in base_footprint): %f %f",cart_offset_.x,cart_offset_.y);
00154
00155 ros::NodeHandle cart_pushing_nh("cart_pushing");
00156 double cart_length, cart_width;
00157 double fp_x_offset, fp_y_offset;
00158 cart_pushing_nh.param("length", cart_length, 0.0);
00159 cart_pushing_nh.param("width", cart_width, 0.0);
00160 cart_pushing_nh.param("footprint_x_offset", fp_x_offset, 0.0);
00161 cart_pushing_nh.param("footprint_y_offset", fp_y_offset, 0.0);
00162
00163 ROS_DEBUG("Length: %f %f, Footprint offset: %f %f",cart_length,cart_width,fp_x_offset,fp_y_offset);
00164 XmlRpc::XmlRpcValue init_position;
00165 geometry_msgs::Pose cart_init_pose;
00166 cart_pushing_nh.getParam("cart_init_pose/position", init_position);
00167 if(!(init_position.getType() == XmlRpc::XmlRpcValue::TypeArray && init_position.size() == 3)){
00168 ROS_FATAL("The init pose must be specified as a 3-Vector");
00169 return;
00170 }
00171 for(int i = 0; i < init_position.size(); ++i)
00172 if(!init_position[i].getType() == XmlRpc::XmlRpcValue::TypeDouble)
00173 ROS_FATAL("Values in the footprint specification must be numbers");
00174 cart_init_pose.position.x = (double) init_position[0];
00175 cart_init_pose.position.y = (double) init_position[1];
00176 cart_init_pose.position.z = (double) init_position[2];
00177
00178 ROS_DEBUG("Init pose: %f %f %f",cart_init_pose.position.x,cart_init_pose.position.y,cart_init_pose.position.z);
00179
00180 cart_cp_offset_.x = cart_init_pose.position.x - cart_offset_.x;
00181 cart_cp_offset_.y = cart_init_pose.position.y - cart_offset_.y;
00182 ROS_DEBUG("Cart control point offset (from pivot point): %f %f",cart_cp_offset_.x,cart_cp_offset_.y);
00183
00184
00185 double padding;
00186 std::string padding_param;
00187 if(!private_nh.searchParam("footprint_padding", padding_param))
00188 padding = 0.01;
00189 else
00190 private_nh.param(padding_param, padding, 0.01);
00191
00192
00193 std::vector<geometry_msgs::Point> cart_footprint;
00194 cart_footprint.resize(4);
00195 cart_footprint[0].x = -cart_length/2.0-padding;
00196 cart_footprint[0].y = -cart_width/2.0-padding;
00197 cart_footprint[0].z = 0.0;
00198
00199 cart_footprint[1].x = -cart_length/2.0-padding;
00200 cart_footprint[1].y = cart_width/2.0+padding;
00201 cart_footprint[1].z = 0.0;
00202
00203 cart_footprint[2].x = cart_length/2.0+padding;
00204 cart_footprint[2].y = cart_width/2.0+padding;
00205 cart_footprint[2].z = 0.0;
00206
00207 cart_footprint[3].x = cart_length/2.0+padding;
00208 cart_footprint[3].y = -cart_width/2.0-padding;
00209 cart_footprint[3].z = 0.0;
00210
00211 for(unsigned int i=0; i < cart_footprint.size(); i++)
00212 cart_footprint[i].x += (cart_init_pose.position.x + cart_length/2.0 - fp_x_offset -cart_offset_.x);
00213
00214
00215
00216 cart_footprint_ = cart_footprint;
00217 cart_perimeterptsV.reserve(cart_footprint.size());
00218 ROS_DEBUG("Cart footprint");
00219 for (size_t ii(0); ii < cart_footprint.size(); ++ii) {
00220 sbpl_2Dpt_t pt;
00221 pt.x = cart_footprint[ii].x;
00222 pt.y = cart_footprint[ii].y;
00223 cart_perimeterptsV.push_back(pt);
00224 ROS_DEBUG("%d: %f %f",(int)ii,pt.x,pt.y);
00225 }
00226 perimeterptsV.reserve(footprint.size());
00227 for (size_t ii(0); ii < footprint.size(); ++ii) {
00228 sbpl_2Dpt_t pt;
00229 pt.x = footprint[ii].x;
00230 pt.y = footprint[ii].y;
00231 perimeterptsV.push_back(pt);
00232 }
00233
00234 bool ret;
00235 try{
00236 ret = env_->InitializeEnv(costmap_ros_->getSizeInCellsX(),
00237 costmap_ros_->getSizeInCellsY(),
00238 0,
00239 0, 0, 0, 0,
00240 0, 0, 0, 0,
00241 0, 0, 0, 0,
00242 perimeterptsV,
00243 cart_offset_,
00244 cart_perimeterptsV,
00245 costmap_ros_->getResolution(), nominalvel_mpersecs,
00246 timetoturn45degsinplace_secs, obst_cost_thresh,
00247 primitive_filename_.c_str());
00248 }
00249 catch(SBPL_Exception e){
00250 ROS_ERROR("SBPL encountered a fatal exception");
00251 ret = false;
00252 }
00253 if(!ret){
00254 ROS_FATAL("SBPL initialization failed!");
00255 initialized_ = false;
00256
00257 throw SBPLPlannerException("Unable to initialize planner!");
00258 return;
00259 }
00260
00261 for (ssize_t ix(0); ix < costmap_ros_->getSizeInCellsX(); ++ix)
00262 for (ssize_t iy(0); iy < costmap_ros_->getSizeInCellsY(); ++iy)
00263 env_->UpdateCost(ix, iy, costMapCostToSBPLCost(cost_map_.getCost(ix,iy)));
00264
00265 if ("ARAPlanner" == planner_type_){
00266 planner_ = new ARAPlanner(env_, forward_search_);
00267 }
00268 else if ("ADPlanner" == planner_type_){
00269 planner_ = new ADPlanner(env_, forward_search_);
00270 }
00271 else{
00272 initialized_ = false;
00273 ROS_FATAL("ARAPlanner and ADPlanner are currently the only supported planners!\n");
00274 return;
00275 }
00276
00277 ROS_DEBUG("[sbpl_cart_planner] Initialized successfully");
00278 plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1, true);
00279 sbpl_plan_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("sbpl_plan_array", 1, true);
00280 sbpl_plan_footprint_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("sbpl_plan_footprint_array", 1, true);
00281 sbpl_robot_cart_plan_pub_ = private_nh.advertise<cart_pushing_msgs::RobotCartPath>("sbpl_robot_cart_plan", 1, true);
00282 stats_publisher_ = private_nh.advertise<sbpl_cart_planner::SBPLCartPlannerStats>("sbpl_cart_planner_stats", 1, true);
00283
00284 if(!env_->initialized_)
00285 initialized_ = false;
00286 else
00287 initialized_ = true;
00288 }
00289
00290 }
00291
00292 unsigned char SBPLCartPlanner::costMapCostToSBPLCost(unsigned char newcost)
00293 {
00294 if(newcost == costmap_2d::LETHAL_OBSTACLE)
00295 return lethal_obstacle_;
00296 else if(newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00297 return inscribed_inflated_obstacle_;
00298 else if(newcost == 0 || newcost == costmap_2d::NO_INFORMATION)
00299 return 0;
00300 else
00301 return (unsigned char) (newcost/sbpl_cost_multiplier_ + 0.5);
00302
00303 }
00304
00305 std::vector<geometry_msgs::Point> SBPLCartPlanner::loadRobotFootprint(ros::NodeHandle node)
00306 {
00307 std::vector<geometry_msgs::Point> footprint;
00308 geometry_msgs::Point pt;
00309 double padding;
00310
00311 std::string padding_param, footprint_param;
00312 if(!node.searchParam("footprint_padding", padding_param))
00313 padding = 0.01;
00314 else
00315 node.param(padding_param, padding, 0.01);
00316
00317
00318 XmlRpc::XmlRpcValue footprint_list;
00319 if(node.searchParam("cart_footprint", footprint_param)){
00320 ROS_DEBUG("Footprint param: %s", footprint_param.c_str());
00321 node.getParam(footprint_param, footprint_list);
00322
00323 if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2)){
00324 ROS_FATAL("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00325 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00326 }
00327 for(int i = 0; i < footprint_list.size(); ++i){
00328
00329 XmlRpc::XmlRpcValue point = footprint_list[i];
00330 if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
00331 ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
00332 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
00333 }
00334
00335
00336 if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00337 ROS_FATAL("Values in the footprint specification must be numbers");
00338 throw std::runtime_error("Values in the footprint specification must be numbers");
00339 }
00340 pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
00341 pt.x += sign(pt.x) * padding;
00342
00343
00344 if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00345 ROS_FATAL("Values in the footprint specification must be numbers");
00346 throw std::runtime_error("Values in the footprint specification must be numbers");
00347 }
00348 pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
00349 pt.y += sign(pt.y) * padding;
00350
00351 footprint.push_back(pt);
00352
00353 }
00354 }
00355 return footprint;
00356 }
00357
00358
00359 bool SBPLCartPlanner::makePlan(const geometry_msgs::PoseStamped& start,
00360 const geometry_msgs::PoseStamped& goal,
00361 std::vector<geometry_msgs::PoseStamped>& plan){
00362 if(!initialized_)
00363 {
00364 ROS_ERROR("Global planner is not initialized");
00365 return false;
00366 }
00367
00368 plan.clear();
00369
00370 ROS_DEBUG("[sbpl_lattice_planner] getting fresh copy of costmap");
00371 costmap_ros_->clearRobotFootprint();
00372 ROS_DEBUG("[sbpl_lattice_planner] robot footprint cleared");
00373 geometry_msgs::Pose cart_pose = getGlobalCartPose(start.pose,0.0);
00374 costmap_ros_->getCostmapCopy(cost_map_);
00375 clearFootprint(cart_pose,cart_footprint_);
00376
00377 ROS_DEBUG("[sbpl_lattice_planner] getting start point (%g,%g) goal point (%g,%g)",
00378 start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
00379 double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
00380 double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
00381 double cart_start = 0.0;
00382 double cart_goal = 0.0;
00383
00384 try
00385 {
00386 int ret = env_->SetStart(start.pose.position.x - cost_map_.getOriginX(), start.pose.position.y - cost_map_.getOriginY(), theta_start, cart_start);
00387 if(ret < 0 || planner_->set_start(ret) == 0){
00388 ROS_ERROR("Failed to set start state");
00389 return false;
00390 }
00391 }
00392 catch(SBPL_Exception e){
00393 ROS_ERROR("SBPL encountered a fatal exception while setting the start state");
00394 return false;
00395 }
00396
00397 try{
00398 int ret = env_->SetGoal(goal.pose.position.x - cost_map_.getOriginX(), goal.pose.position.y - cost_map_.getOriginY(), theta_goal, cart_goal);
00399 if(ret < 0 || planner_->set_goal(ret) == 0){
00400 ROS_ERROR("Failed to set goal state");
00401 return false;
00402 }
00403 }
00404 catch(SBPL_Exception e){
00405 ROS_ERROR("SBPL encountered a fatal exception while setting the goal state");
00406 return false;
00407 }
00408
00409
00410 int offOnCount = 0;
00411 int onOffCount = 0;
00412 int allCount = 0;
00413 vector<nav2dcell_t> changedcellsV;
00414
00415 for(unsigned int ix = 0; ix < cost_map_.getSizeInCellsX(); ix++) {
00416 for(unsigned int iy = 0; iy < cost_map_.getSizeInCellsY(); iy++) {
00417
00418 unsigned char oldCost = env_->GetMapCost(ix,iy);
00419 unsigned char newCost = cost_map_.getCost(ix,iy);
00420
00421 if(oldCost == newCost) continue;
00422
00423 allCount++;
00424
00425
00426
00427 if((oldCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && oldCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
00428 (newCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || newCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
00429 offOnCount++;
00430 }
00431
00432 if((oldCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || oldCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
00433 (newCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && newCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
00434 onOffCount++;
00435 }
00436 env_->UpdateCost(ix, iy, costMapCostToSBPLCost(cost_map_.getCost(ix,iy)));
00437
00438 nav2dcell_t nav2dcell;
00439 nav2dcell.x = ix;
00440 nav2dcell.y = iy;
00441 changedcellsV.push_back(nav2dcell);
00442 }
00443 }
00444
00445 try{
00446 if(!changedcellsV.empty()){
00447 StateChangeQuery* scq = new LatticeSCQ(env_, changedcellsV);
00448 planner_->costs_changed(*scq);
00449 delete scq;
00450 }
00451 if(allCount > force_scratch_limit_)
00452 planner_->force_planning_from_scratch();
00453 }
00454 catch(SBPL_Exception e){
00455 ROS_ERROR("SBPL failed to update the costmap");
00456 return false;
00457 }
00458
00459
00460 ROS_DEBUG("allocated:%f, init eps:%f\n",allocated_time_,initial_epsilon_);
00461 planner_->set_initialsolution_eps(initial_epsilon_);
00462 planner_->set_search_mode(false);
00463
00464 ROS_DEBUG("[sbpl_lattice_planner] run planner");
00465 vector<int> solution_stateIDs;
00466 int solution_cost;
00467 try{
00468 int ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
00469 if(ret)
00470 ROS_DEBUG("Solution is found\n");
00471 else
00472 {
00473 ROS_ERROR("Solution does not exist\n");
00474 ARAPlanner *ara_planner = dynamic_cast<ARAPlanner*>(planner_);
00475
00476 sbpl_cart_planner::SBPLCartPlannerStats stats;
00477 stats.initial_epsilon = initial_epsilon_;
00478 stats.plan_to_first_solution = false;
00479 stats.allocated_time = allocated_time_;
00480 if(ara_planner)
00481 {
00482 stats.actual_time = ara_planner->get_final_eps_planning_time();
00483 stats.actual_time = ara_planner->get_initial_eps_planning_time();
00484 stats.number_of_expands_initial_solution = ara_planner->get_n_expands_init_solution();
00485 stats.final_epsilon = ara_planner->get_final_epsilon();
00486 }
00487 stats.solution_cost = solution_cost;
00488 stats.path_size = 0;
00489 stats.start = start;
00490 stats.goal = goal;
00491 stats.start_cart_angle = cart_start;
00492 stats.goal_cart_angle = cart_goal;
00493 stats_publisher_.publish(stats);
00494 stats.final_number_of_expands = planner_->get_n_expands();
00495 return false;
00496 }
00497 }
00498 catch(SBPL_Exception e){
00499 ROS_ERROR("SBPL encountered a fatal exception while planning");
00500 return false;
00501 }
00502
00503 ROS_DEBUG("size of solution=%d", (int)solution_stateIDs.size());
00504
00505 vector<EnvNAVXYTHETACARTLAT3Dpt_t> sbpl_path;
00506 try
00507 {
00508 env_->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
00509 }
00510 catch(SBPL_Exception e)
00511 {
00512 ROS_ERROR("SBPL encountered a fatal exception while reconstructing the path");
00513 return false;
00514 }
00515
00516 ROS_DEBUG("Plan has %d points.\n", (int)sbpl_path.size());
00517 ros::Time plan_time = ros::Time::now();
00518
00519
00520 visualization_msgs::MarkerArray sbpl_plan_marker_array;
00521 convertPathToMarkerArray(sbpl_path,costmap_ros_->getGlobalFrameID(),sbpl_plan_marker_array);
00522 sbpl_plan_pub_.publish(sbpl_plan_marker_array);
00523
00524 visualization_msgs::MarkerArray sbpl_plan_footprint;
00525 getFootprintList(sbpl_path,costmap_ros_->getGlobalFrameID(),sbpl_plan_footprint);
00526 sbpl_plan_footprint_pub_.publish(sbpl_plan_footprint);
00527
00528
00529 nav_msgs::Path gui_path;
00530 gui_path.poses.resize(sbpl_path.size());
00531 gui_path.header.frame_id = costmap_ros_->getGlobalFrameID();
00532 gui_path.header.stamp = plan_time;
00533
00534 cart_pushing_msgs::RobotCartPath sbpl_robot_cart_plan;
00535 sbpl_robot_cart_plan.header.frame_id = costmap_ros_->getGlobalFrameID();
00536 sbpl_robot_cart_plan.header.stamp = plan_time;
00537
00538
00539 for(unsigned int i=0; i<sbpl_path.size(); i++){
00540 geometry_msgs::PoseStamped pose;
00541 pose.header.stamp = plan_time;
00542 pose.header.frame_id = costmap_ros_->getGlobalFrameID();
00543
00544 pose.pose.position.x = sbpl_path[i].x + cost_map_.getOriginX();
00545 pose.pose.position.y = sbpl_path[i].y + cost_map_.getOriginY();
00546 pose.pose.position.z = start.pose.position.z;
00547
00548 pose.pose.orientation = tf::createQuaternionMsgFromYaw(sbpl_path[i].theta);
00549
00550 plan.push_back(pose);
00551
00552 cart_pushing_msgs::RobotCartConfiguration robot_cart_configuration;
00553 robot_cart_configuration.robot_pose = pose.pose;
00554 robot_cart_configuration.cart_pose = getLocalCartControlFramePose(sbpl_path[i]);
00555 ROS_DEBUG_NAMED("cart_angle", "Cartangle: %d %f",i,sbpl_path[i].cartangle);
00556 sbpl_robot_cart_plan.path.push_back(robot_cart_configuration);
00557
00558 gui_path.poses[i].pose.position.x = plan[i].pose.position.x;
00559 gui_path.poses[i].pose.position.y = plan[i].pose.position.y;
00560 gui_path.poses[i].pose.position.z = plan[i].pose.position.z;
00561 }
00562 plan_pub_.publish(gui_path);
00563 sbpl_robot_cart_plan_pub_.publish(sbpl_robot_cart_plan);
00564
00565 ARAPlanner *ara_planner = dynamic_cast<ARAPlanner*>(planner_);
00566
00567
00568 sbpl_cart_planner::SBPLCartPlannerStats stats;
00569 stats.initial_epsilon = initial_epsilon_;
00570 stats.plan_to_first_solution = false;
00571 stats.final_number_of_expands = planner_->get_n_expands();
00572 stats.allocated_time = allocated_time_;
00573 if(ara_planner)
00574 {
00575 stats.time_to_first_solution = ara_planner->get_initial_eps_planning_time();
00576 stats.actual_time = ara_planner->get_final_eps_planning_time();
00577 stats.number_of_expands_initial_solution = ara_planner->get_n_expands_init_solution();
00578 stats.final_epsilon = ara_planner->get_final_epsilon();
00579 }
00580 stats.solution_cost = solution_cost;
00581 stats.path_size = sbpl_path.size();
00582 stats.start = start;
00583 stats.goal = goal;
00584 stats.start_cart_angle = cart_start;
00585 stats.goal_cart_angle = cart_goal;
00586 stats.solution = sbpl_robot_cart_plan;
00587 stats_publisher_.publish(stats);
00588
00589 return true;
00590 }
00591
00592 void SBPLCartPlanner::convertPathToMarkerArray(const std::vector<EnvNAVXYTHETACARTLAT3Dpt_t> &sbpl_path,
00593 const std::string &path_frame_id,
00594 visualization_msgs::MarkerArray &ma)
00595 {
00596 unsigned int num_markers = sbpl_path.size();
00597 ma.markers.resize(2*num_markers);
00598
00599 for(unsigned int i=0; i< num_markers; i++)
00600 {
00601 ma.markers[2*i].header.frame_id = path_frame_id;
00602 ma.markers[2*i].header.stamp = ros::Time();
00603 ma.markers[2*i].ns = "sbpl_cart_plan";
00604 ma.markers[2*i].id = 2*i;
00605 ma.markers[2*i].type = visualization_msgs::Marker::ARROW;
00606 ma.markers[2*i].action = visualization_msgs::Marker::ADD;
00607 ma.markers[2*i].pose.position.x = sbpl_path[i].x;
00608 ma.markers[2*i].pose.position.y = sbpl_path[i].y;
00609 ma.markers[2*i].pose.position.z = 0.0;
00610 btQuaternion quat;
00611 quat.setRPY(0.0,0.0,sbpl_path[i].theta);
00612 tf::quaternionTFToMsg(quat,ma.markers[2*i].pose.orientation);
00613 ma.markers[2*i].scale.x = 0.1;
00614 ma.markers[2*i].scale.y = 0.05;
00615 ma.markers[2*i].scale.z = 0.05;
00616 ma.markers[2*i].color.a = 1.0;
00617 ma.markers[2*i].color.r = 0.0;
00618 ma.markers[2*i].color.g = 1.0;
00619 ma.markers[2*i].color.b = 0.0;
00620
00621 geometry_msgs::Pose cart_pose;
00622 cart_pose.position.x = sbpl_path[i].x + cos(sbpl_path[i].theta)*cart_offset_.x - sin(sbpl_path[i].theta)*cart_offset_.y;
00623 cart_pose.position.y = sbpl_path[i].y + sin(sbpl_path[i].theta)*cart_offset_.x + cos(sbpl_path[i].theta)*cart_offset_.y;
00624 cart_pose.position.z = 0.0;
00625 double yaw = sbpl_path[i].theta + sbpl_path[i].cartangle;
00626 quat.setRPY(0.0,0.0,yaw);
00627 tf::quaternionTFToMsg(quat,cart_pose.orientation);
00628
00629 ma.markers[2*i+1].header.frame_id = path_frame_id;
00630 ma.markers[2*i+1].header.stamp = ros::Time();
00631 ma.markers[2*i+1].ns = "sbpl_cart_plan";
00632 ma.markers[2*i+1].id = 2*i+1;
00633 ma.markers[2*i+1].type = visualization_msgs::Marker::ARROW;
00634 ma.markers[2*i+1].action = visualization_msgs::Marker::ADD;
00635 ma.markers[2*i+1].pose = cart_pose;
00636 ma.markers[2*i+1].scale.x = 0.1;
00637 ma.markers[2*i+1].scale.y = 0.05;
00638 ma.markers[2*i+1].scale.z = 0.05;
00639 ma.markers[2*i+1].color.a = 1.0;
00640 ma.markers[2*i+1].color.r = 1.0;
00641 ma.markers[2*i+1].color.g = 0.0;
00642 ma.markers[2*i+1].color.b = 0.0;
00643 }
00644 if(num_markers < num_sbpl_markers_)
00645 {
00646 for(unsigned int i = 0; i < (num_sbpl_markers_ - num_markers); i++)
00647 {
00648 visualization_msgs::Marker new_marker;
00649 new_marker.header.frame_id = path_frame_id;
00650 new_marker.header.stamp = ros::Time();
00651 new_marker.ns = "sbpl_cart_plan";
00652 new_marker.id = 2*(num_markers+i);
00653 new_marker.type = visualization_msgs::Marker::ARROW;
00654 new_marker.action = visualization_msgs::Marker::DELETE;
00655
00656 ma.markers.push_back(new_marker);
00657
00658 new_marker.id = 2*(num_markers+i)+1;
00659
00660 ma.markers.push_back(new_marker);
00661
00662 }
00663 }
00664 num_sbpl_markers_ = num_markers;
00665 }
00666
00667
00668
00669 geometry_msgs::Pose SBPLCartPlanner::getGlobalCartPose(const EnvNAVXYTHETACARTLAT3Dpt_t& sbpl_pose)
00670 {
00671 geometry_msgs::Pose cart_pose;
00672 btQuaternion quat;
00673 cart_pose.position.x = sbpl_pose.x + cos(sbpl_pose.theta)*cart_offset_.x - sin(sbpl_pose.theta)*cart_offset_.y;
00674 cart_pose.position.y = sbpl_pose.y + sin(sbpl_pose.theta)*cart_offset_.x + cos(sbpl_pose.theta)*cart_offset_.y;
00675 cart_pose.position.z = 0.0;
00676 double yaw = sbpl_pose.theta + sbpl_pose.cartangle;
00677 quat.setRPY(0.0,0.0,yaw);
00678 tf::quaternionTFToMsg(quat,cart_pose.orientation);
00679 return cart_pose;
00680 }
00681
00682
00683 geometry_msgs::Pose SBPLCartPlanner::getLocalCartControlFramePose(const EnvNAVXYTHETACARTLAT3Dpt_t& sbpl_pose)
00684 {
00685 geometry_msgs::Pose cart_pose;
00686 btQuaternion quat;
00687 cart_pose.position.x = cart_offset_.x + cos(sbpl_pose.cartangle)*cart_cp_offset_.x - sin(sbpl_pose.cartangle)*cart_cp_offset_.y;
00688 cart_pose.position.y = cart_offset_.y + sin(sbpl_pose.cartangle)*cart_cp_offset_.x + cos(sbpl_pose.cartangle)*cart_cp_offset_.y;
00689 cart_pose.position.z = 0.0;
00690 double yaw = sbpl_pose.cartangle;
00691 quat.setRPY(0.0,0.0,yaw);
00692 tf::quaternionTFToMsg(quat,cart_pose.orientation);
00693 return cart_pose;
00694 }
00695
00696 geometry_msgs::Pose SBPLCartPlanner::getLocalCartPose(const EnvNAVXYTHETACARTLAT3Dpt_t& sbpl_pose)
00697 {
00698 geometry_msgs::Pose cart_pose;
00699 btQuaternion quat;
00700 cart_pose.position.x = cart_offset_.x;
00701 cart_pose.position.y = cart_offset_.y;
00702 cart_pose.position.z = 0.0;
00703 double yaw = sbpl_pose.cartangle;
00704 quat.setRPY(0.0,0.0,yaw);
00705 tf::quaternionTFToMsg(quat,cart_pose.orientation);
00706 return cart_pose;
00707 }
00708
00709 void SBPLCartPlanner::getFootprintList(const std::vector<EnvNAVXYTHETACARTLAT3Dpt_t> &sbpl_path,
00710 const std::string &path_frame_id,
00711 visualization_msgs::MarkerArray & ma)
00712 {
00713 ma.markers.resize(2);
00714 ma.markers[0].header.frame_id = path_frame_id;
00715 ma.markers[0].header.stamp = ros::Time();
00716 ma.markers[0].ns = "sbpl_cart_footprint";
00717 ma.markers[0].id = 0;
00718 ma.markers[0].type = visualization_msgs::Marker::LINE_LIST;
00719 ma.markers[0].action = visualization_msgs::Marker::ADD;
00720
00721 ma.markers[0].scale.x = 0.05;
00722 ma.markers[0].color.a = 1.0;
00723 ma.markers[0].color.r = 0.0;
00724 ma.markers[0].color.g = 0.0;
00725 ma.markers[0].color.b = 1.0;
00726
00727 ma.markers[1].header.frame_id = path_frame_id;
00728 ma.markers[1].header.stamp = ros::Time();
00729 ma.markers[1].ns = "sbpl_cart_footprint";
00730 ma.markers[1].id = 1;
00731 ma.markers[1].type = visualization_msgs::Marker::LINE_LIST;
00732 ma.markers[1].action = visualization_msgs::Marker::ADD;
00733
00734 ma.markers[1].scale.x = 0.05;
00735 ma.markers[1].color.a = 1.0;
00736 ma.markers[1].color.r = 0.0;
00737 ma.markers[1].color.g = 1.0;
00738 ma.markers[1].color.b = 0.0;
00739
00740
00741 for(unsigned int i=0; i < sbpl_path.size(); i= i+visualizer_skip_poses_)
00742 {
00743 std::vector<geometry_msgs::Point> transformed_cfp, transformed_rfp;
00744 geometry_msgs::Pose robot_pose;
00745 robot_pose.position.x = sbpl_path[i].x;
00746 robot_pose.position.y = sbpl_path[i].y;
00747 robot_pose.position.z = 0.0;
00748 btQuaternion quat;
00749 quat.setRPY(0.0,0.0,sbpl_path[i].theta);
00750 tf::quaternionTFToMsg(quat,robot_pose.orientation);
00751
00752 geometry_msgs::Pose cart_pose;
00753 cart_pose.position.x = sbpl_path[i].x + cos(sbpl_path[i].theta)*cart_offset_.x - sin(sbpl_path[i].theta)*cart_offset_.y;
00754 cart_pose.position.y = sbpl_path[i].y + sin(sbpl_path[i].theta)*cart_offset_.x + cos(sbpl_path[i].theta)*cart_offset_.y;
00755 cart_pose.position.z = 0.0;
00756 double yaw = sbpl_path[i].theta + sbpl_path[i].cartangle;
00757 quat.setRPY(0.0,0.0,yaw);
00758 tf::quaternionTFToMsg(quat,cart_pose.orientation);
00759
00760 transformFootprintToEdges(robot_pose,robot_footprint_,transformed_rfp);
00761 transformFootprintToEdges(cart_pose,cart_footprint_,transformed_cfp);
00762
00763 for(unsigned int i=0; i < transformed_rfp.size(); i++)
00764 ma.markers[0].points.push_back(transformed_rfp[i]);
00765
00766 for(unsigned int i=0; i < transformed_cfp.size(); i++)
00767 ma.markers[1].points.push_back(transformed_cfp[i]);
00768 }
00769
00770 return;
00771 }
00772
00773 void SBPLCartPlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose,
00774 const std::vector<geometry_msgs::Point> &footprint,
00775 std::vector<geometry_msgs::Point> &out_footprint)
00776 {
00777 out_footprint.resize(2*footprint.size());
00778 double yaw = tf::getYaw(robot_pose.orientation);
00779 for(unsigned int i=0; i < footprint.size(); i++)
00780 {
00781 out_footprint[2*i].x = robot_pose.position.x + cos(yaw)*footprint[i].x - sin(yaw)*footprint[i].y;
00782 out_footprint[2*i].y = robot_pose.position.y + sin(yaw)*footprint[i].x + cos(yaw)*footprint[i].y;
00783 if(i == 0)
00784 {
00785 out_footprint.back().x = out_footprint[i].x;
00786 out_footprint.back().y = out_footprint[i].y;
00787 }
00788 else
00789 {
00790 out_footprint[2*i-1].x = out_footprint[2*i].x;
00791 out_footprint[2*i-1].y = out_footprint[2*i].y;
00792 }
00793 }
00794 return;
00795 }
00796
00797 bool SBPLCartPlanner::clearFootprint(const geometry_msgs::Pose &robot_pose,
00798 const std::vector<geometry_msgs::Point> &footprint)
00799 {
00800 vector<geometry_msgs::Point> oriented_footprint;
00801 getOrientedFootprint(robot_pose, footprint, oriented_footprint);
00802
00803
00804 if(!cost_map_.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
00805 {
00806 ROS_ERROR("Could not clear robot footprint: ");
00807 for (uint i = 0; i < oriented_footprint.size(); ++i)
00808 ROS_ERROR("oriented_footprint: %.2lf %.2lf %.2lf", oriented_footprint[i].x, oriented_footprint[i].y, oriented_footprint[i].z);
00809 return false;
00810 }
00811 return true;
00812 }
00813
00814 void SBPLCartPlanner::getOrientedFootprint(const geometry_msgs::Pose &robot_pose,
00815 const std::vector<geometry_msgs::Point> &footprint,
00816 std::vector<geometry_msgs::Point> &oriented_footprint)
00817 {
00818 oriented_footprint.resize(footprint.size());
00819 double yaw = tf::getYaw(robot_pose.orientation);
00820 for(unsigned int i=0; i < footprint.size(); i++)
00821 {
00822 oriented_footprint[i].x = robot_pose.position.x + cos(yaw)*footprint[i].x - sin(yaw)*footprint[i].y;
00823 oriented_footprint[i].y = robot_pose.position.y + sin(yaw)*footprint[i].x + cos(yaw)*footprint[i].y;
00824 }
00825 return;
00826 }
00827
00828 geometry_msgs::Pose SBPLCartPlanner::getGlobalCartPose(const geometry_msgs::Pose &robot_pose, const double &cart_angle)
00829 {
00830 EnvNAVXYTHETACARTLAT3Dpt_t sbpl_pose;
00831 sbpl_pose.x = robot_pose.position.x;
00832 sbpl_pose.y = robot_pose.position.y;
00833 sbpl_pose.theta = tf::getYaw(robot_pose.orientation);
00834 sbpl_pose.cartangle = cart_angle;
00835 return getGlobalCartPose(sbpl_pose);
00836 }
