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_dynamic_env_global_planner/sbpl_dynamic_env_global_planner.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <nav_msgs/Path.h>
00041 #include <sbpl_dynamic_planner/envTime.h>
00042 #include <sbpl_dynamic_planner/envDBubble.h>
00043 #include <sbpl_dynamic_planner/envInterval.h>
00044 #include <visualization_msgs/MarkerArray.h>
00045 #include <visualization_msgs/Marker.h>
00046 #include <geometry_msgs/PointStamped.h>
00047
00048 using namespace std;
00049 using namespace ros;
00050
00051
00052 PLUGINLIB_REGISTER_CLASS(SBPLDynEnvGlobalPlanner, SBPLDynEnvGlobalPlanner, nav_core::BaseGlobalPlanner);
00053
00054 SBPLDynEnvGlobalPlanner::SBPLDynEnvGlobalPlanner()
00055 : initialized_(false),
00056 tf_(ros::NodeHandle(),
00057 ros::Duration(10),
00058 true
00059 ),
00060 costmap_ros_(NULL)
00061 {
00062 }
00063
00064 void SBPLDynEnvGlobalPlanner::dynamicObstacleCallback(const dynamic_obs_msgs::DynamicObstaclesConstPtr& msg){
00065
00066 double obs_time = msg->header.stamp.toSec();
00067
00068 sensor_dynObs->clear();
00069 for(unsigned int i=0; i<msg->dyn_obs.size(); i++){
00070 SBPL_DynamicObstacle_t o;
00071 o.radius = msg->dyn_obs[i].radius;
00072 for(unsigned int j=0; j<msg->dyn_obs[i].trajectories.size(); j++){
00073 SBPL_Trajectory_t t;
00074 t.prob = msg->dyn_obs[i].trajectories[j].probability;
00075 t.existsAfter = msg->dyn_obs[i].trajectories[j].exists_after;
00076 for(unsigned int k=0; k<msg->dyn_obs[i].trajectories[j].points.size(); k++){
00077 geometry_msgs::PoseStamped pose_in;
00078 pose_in.header = msg->dyn_obs[i].trajectories[j].points[k].header;
00079 pose_in.header.stamp = ros::Time();
00080 pose_in.pose = msg->dyn_obs[i].trajectories[j].points[k].pose.pose;
00081 geometry_msgs::PoseStamped pose_out = pose_in;
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091 SBPL_Traj_Pt_t p;
00092 p.x = pose_out.pose.position.x;
00093 p.y = pose_out.pose.position.y;
00094
00095
00096
00097 p.t = CONTTIME2DISC(msg->dyn_obs[i].trajectories[j].points[k].header.stamp.toSec() - obs_time, time_resolution);
00098 p.std_dev = sqrt((msg->dyn_obs[i].trajectories[j].points[k].pose.covariance[0] + msg->dyn_obs[i].trajectories[j].points[k].pose.covariance[7])/2.0);
00099 t.points.push_back(p);
00100 }
00101 o.trajectories.push_back(t);
00102 }
00103 sensor_dynObs->push_back(o);
00104 }
00105
00106 pthread_mutex_lock(&m);
00107
00108 vector<SBPL_DynamicObstacle_t>* temp = current_dynObs;
00109 current_dynObs = sensor_dynObs;
00110 sensor_dynObs = temp;
00111 current_dynObs_timestamp = msg->header.stamp;
00112 pthread_mutex_unlock(&m);
00113 }
00114
00115 ros::Time SBPLDynEnvGlobalPlanner::getDynamicObstacles(){
00116 pthread_mutex_lock(&m);
00117
00118
00119 if(current_dynObs_timestamp > plan_dynObs_timestamp){
00120
00121 vector<SBPL_DynamicObstacle_t>* temp = plan_dynObs;
00122 plan_dynObs = current_dynObs;
00123 current_dynObs = temp;
00124
00125
00126 plan_dynObs_timestamp = current_dynObs_timestamp;
00127 }
00128 ros::Time t = plan_dynObs_timestamp;
00129 pthread_mutex_unlock(&m);
00130
00131 ROS_DEBUG("dynObs size = %d\n",(int)plan_dynObs->size());
00132 env->setDynamicObstacles(*plan_dynObs);
00133 return t;
00134 }
00135
00136 void SBPLDynEnvGlobalPlanner::moveBaseStatusCallback(const actionlib_msgs::GoalStatusArrayConstPtr& msg){
00137
00138 if(!msg->status_list.empty() && msg->status_list.back().status == msg->status_list.back().SUCCEEDED){
00139
00140 if(!pathDone){
00141 prevGoal.header.stamp = ros::Time::now();
00142 goal_pub.publish(prevGoal);
00143 ROS_DEBUG("sent new goal...\n");
00144 }
00145 }
00146 }
00147
00148
00149 SBPLDynEnvGlobalPlanner::SBPLDynEnvGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00150 : initialized_(false), costmap_ros_(NULL) {
00151
00152 initialize(name, costmap_ros);
00153 }
00154
00155
00156 void SBPLDynEnvGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00157 {
00158 if(!initialized_)
00159 {
00160 ros::NodeHandle private_nh("~/"+name);
00161 ros::NodeHandle nh(name);
00162 ros::NodeHandle global_nh;
00163
00164 ROS_DEBUG("Planner is %s", name.c_str());
00165
00166 private_nh.param("allocated_time", allocated_time_, 0.5);
00167 private_nh.param("initial_epsilon",initial_epsilon_,5.0);
00168 private_nh.param("decrease_epsilon",decrease_epsilon_,1.0);
00169 private_nh.param("primitive_filename",primitive_filename_,string(""));
00170 private_nh.param("time_resolution",time_resolution,0.001);
00171 private_nh.param("temporal_padding",temporal_padding,1.5);
00172
00173 double nominalvel_mpersecs, timetoturn45degsinplace_secs;
00174 private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
00175 private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
00176
00177 private_nh.param("remove_dynObs_from_costmap", remove_dynObs_from_costmap, true);
00178 private_nh.param("dyn_obs_pad_costmap_removal", dyn_obs_pad_costmap_removal, 0.2);
00179
00180 costmap_ros_ = costmap_ros;
00181 costmap_ros_->clearRobotFootprint();
00182 costmap_ros_->getCostmapCopy(cost_map_);
00183
00184 std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
00185
00186 env = new EnvIntervalLat();
00187 env->SetEnvParameter("cost_inscribed_thresh",costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00188 env->SetEnvParameter("cost_possibly_circumscribed_thresh", cost_map_.getCircumscribedCost());
00189 int obst_cost_thresh = costmap_2d::LETHAL_OBSTACLE;
00190 int dyn_obs_cost_thresh = costmap_2d::LETHAL_OBSTACLE;
00191 vector<sbpl_2Dpt_t> perimeterptsV;
00192 perimeterptsV.reserve(footprint.size());
00193 for (size_t ii(0); ii < footprint.size(); ++ii) {
00194 sbpl_2Dpt_t pt;
00195 pt.x = footprint[ii].x;
00196 pt.y = footprint[ii].y;
00197 perimeterptsV.push_back(pt);
00198 }
00199 vector<SBPL_DynamicObstacle_t> init_dynObs;
00200 env->InitializeEnv(costmap_ros_->getSizeInCellsX(),
00201 costmap_ros_->getSizeInCellsY(),
00202 0,
00203 0, 0, 0, 0,
00204 0, 0, 0,
00205 0, 0, 0,
00206 perimeterptsV, costmap_ros_->getResolution(), time_resolution, temporal_padding,
00207 nominalvel_mpersecs, timetoturn45degsinplace_secs, obst_cost_thresh, dyn_obs_cost_thresh,
00208 primitive_filename_.c_str(), init_dynObs);
00209 for (ssize_t ix(0); ix < costmap_ros_->getSizeInCellsX(); ++ix)
00210 for (ssize_t iy(0); iy < costmap_ros_->getSizeInCellsY(); ++iy)
00211 env->UpdateCost(ix, iy, cost_map_.getCost(ix,iy));
00212
00213 planner = new IntervalPlanner(env);
00214
00215 ROS_INFO("[sbpl_dynamic_env_global_planner] Initialized successfully");
00216 plan_pub_ = nh.advertise<nav_msgs::Path>("plan", 1);
00217 marker_pub = global_nh.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 1);
00218
00219 pthread_mutex_init(&m, NULL);
00220 sensor_dynObs = new vector<SBPL_DynamicObstacle_t>;
00221 current_dynObs = new vector<SBPL_DynamicObstacle_t>;
00222 plan_dynObs = new vector<SBPL_DynamicObstacle_t>;
00223 current_dynObs_timestamp = ros::Time::now();
00224 plan_dynObs_timestamp = ros::Time::now();
00225 dynObs_sub = ros::NodeHandle().subscribe("dynamic_obstacles", 1, &SBPLDynEnvGlobalPlanner::dynamicObstacleCallback, this);
00226 goal_pub = ros::NodeHandle().advertise<geometry_msgs::PoseStamped>("move_base_simple/goal",1);
00227 moveBaseStatus_sub = ros::NodeHandle().subscribe("move_base/status", 1, &SBPLDynEnvGlobalPlanner::moveBaseStatusCallback, this);
00228 }
00229 }
00230
00231 bool SBPLDynEnvGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
00232 const geometry_msgs::PoseStamped& goal,
00233 std::vector<geometry_msgs::PoseStamped>& plan)
00234 {
00235 ROS_DEBUG("\n\n\n\nstart wrapper\n");
00236
00237 plan.clear();
00238
00239 ROS_DEBUG("[sbpl_dynamic_env_global_planner] getting fresh copy of costmap");
00240 costmap_ros_->clearRobotFootprint();
00241 ROS_DEBUG("[sbpl_dynamic_env_global_planner] robot footprint cleared");
00242
00243 costmap_ros_->getCostmapCopy(cost_map_);
00244
00245 ROS_DEBUG("[sbpl_dynamic_env_global_planner] getting dynamic obstacles");
00246 ros::Time start_time_offset = getDynamicObstacles();
00247 ROS_DEBUG("[sbpl_dynamic_env_global_planner] done getting dynamic obstacles");
00248
00249 ROS_DEBUG("[sbpl_dynamic_env_global_planner] getting start point (%g,%g) goal point (%g,%g)",
00250 start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
00251 double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
00252 double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
00253
00254 if(planner->set_start(env->SetStart(start.pose.position.x - cost_map_.getOriginX(), start.pose.position.y - cost_map_.getOriginY(), theta_start, (start.header.stamp - start_time_offset).toSec())) == 0){
00255 ROS_ERROR("ERROR: failed to set start state\n");
00256 return false;
00257 }
00258 if(planner->set_goal(env->SetGoal(goal.pose.position.x - cost_map_.getOriginX(), goal.pose.position.y - cost_map_.getOriginY(), theta_goal)) == 0){
00259 ROS_ERROR("ERROR: failed to set goal state\n");
00260 return false;
00261 }
00262
00263 for(unsigned int ix = 0; ix < cost_map_.getSizeInCellsX(); ix++) {
00264 for(unsigned int iy = 0; iy < cost_map_.getSizeInCellsY(); iy++) {
00265
00266 unsigned char oldCost = env->GetMapCost(ix,iy);
00267 unsigned char newCost = cost_map_.getCost(ix,iy);
00268
00269
00270
00271
00272
00273 if(remove_dynObs_from_costmap){
00274
00275 bool isDynObs = false;
00276 double res = costmap_ros_->getResolution();
00277 for(unsigned int i=0; i < plan_dynObs->size(); i++){
00278 if(plan_dynObs->at(i).trajectories[0].points.empty())
00279 continue;
00280 double dx = plan_dynObs->at(i).trajectories[0].points[0].x - ix*res;
00281 double dy = plan_dynObs->at(i).trajectories[0].points[0].y - iy*res;
00282 double d = sqrt(dx*dx+dy*dy);
00283 if(d < plan_dynObs->at(i).radius + costmap_ros_->getInflationRadius() + dyn_obs_pad_costmap_removal){
00284 isDynObs = true;
00285 break;
00286 }
00287 }
00288 if(isDynObs){
00289
00290
00291
00292
00293 env->UpdateCost(ix, iy, 0);
00294 continue;
00295 }
00296
00297 }
00298
00299 env->UpdateCost(ix, iy, cost_map_.getCost(ix,iy));
00300 }
00301 }
00302
00303 planner->force_planning_from_scratch();
00304
00305
00306 ROS_DEBUG("allocated:%f, init eps:%f\n",allocated_time_,initial_epsilon_);
00307 planner->set_initialsolution_eps(initial_epsilon_);
00308 planner->set_decrease_eps_step(decrease_epsilon_);
00309 planner->set_search_mode(false);
00310
00311 vector<int> solution_stateIDs;
00312 ROS_DEBUG("start planner\n");
00313 if(planner->replan(allocated_time_, &solution_stateIDs))
00314 ROS_DEBUG("Solution is found");
00315 else{
00316 ROS_DEBUG("Solution not found");
00317 return false;
00318 }
00319 ROS_DEBUG("size of solution=%d", (int)solution_stateIDs.size());
00320
00321 vector<SBPL_4Dpt_t> sbpl_path;
00322 env->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
00323 ROS_DEBUG("Plan has %d points.\n", (int)sbpl_path.size());
00324 ros::Time plan_time = ros::Time::now();
00325
00326
00327 nav_msgs::Path gui_path;
00328 gui_path.set_poses_size(sbpl_path.size());
00329 gui_path.header.frame_id = costmap_ros_->getGlobalFrameID();
00330 gui_path.header.stamp = plan_time;
00331 visualization_msgs::MarkerArray ma;
00332 int waitCount = 0;
00333 pathDone = true;
00334 for(unsigned int i=0; i<sbpl_path.size(); i++){
00335 geometry_msgs::PoseStamped pose;
00336 pose.header.stamp = plan_time;
00337 pose.header.frame_id = costmap_ros_->getGlobalFrameID();
00338
00339 pose.pose.position.x = sbpl_path[i].x + cost_map_.getOriginX();
00340 pose.pose.position.y = sbpl_path[i].y + cost_map_.getOriginY();
00341 pose.pose.position.z = start.pose.position.z;
00342
00343 btQuaternion temp;
00344 temp.setEulerZYX(sbpl_path[i].theta,0,0);
00345 pose.pose.orientation.x = temp.getX();
00346 pose.pose.orientation.y = temp.getY();
00347 pose.pose.orientation.z = temp.getZ();
00348 pose.pose.orientation.w = temp.getW();
00349
00350
00351
00352 if(i>0 && pathDone &&
00353 sbpl_path[i].x == sbpl_path[i-1].x &&
00354 sbpl_path[i].y == sbpl_path[i-1].y &&
00355 sbpl_path[i].theta == sbpl_path[i-1].theta){
00356 ROS_DEBUG("we have a wait!\n");
00357 pathDone = false;
00358 prevGoal.header.frame_id = goal.header.frame_id;
00359 prevGoal.pose.position.x = goal.pose.position.x;
00360 prevGoal.pose.position.y = goal.pose.position.y;
00361 prevGoal.pose.position.z = goal.pose.position.z;
00362 prevGoal.pose.orientation.x = goal.pose.orientation.x;
00363 prevGoal.pose.orientation.y = goal.pose.orientation.y;
00364 prevGoal.pose.orientation.z = goal.pose.orientation.z;
00365 prevGoal.pose.orientation.w = goal.pose.orientation.w;
00366
00367 visualization_msgs::Marker m;
00368 m.header.frame_id = costmap_ros_->getGlobalFrameID();
00369 m.header.stamp = ros::Time::now();
00370 m.ns = "waits";
00371 m.id = waitCount;
00372 waitCount++;
00373 m.text = "WAIT";
00374 m.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00375 m.action = visualization_msgs::Marker::ADD;
00376 m.pose.position.x = pose.pose.position.x;
00377 m.pose.position.y = pose.pose.position.y;
00378 m.pose.position.z = 0.01;
00379
00380 m.scale.z = 0.5;
00381 m.color.r = 0.0;
00382 m.color.g = 0.0;
00383 m.color.b = 1.0;
00384 m.color.a = 1.0;
00385 m.lifetime = ros::Duration();
00386
00387 ma.markers.push_back(m);
00388
00389
00390 }
00391
00392 gui_path.poses[i].pose.position.x = pose.pose.position.x;
00393 gui_path.poses[i].pose.position.y = pose.pose.position.y;
00394 gui_path.poses[i].pose.position.z = pose.pose.position.z;
00395
00396 if(pathDone)
00397 plan.push_back(pose);
00398 }
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408 plan_pub_.publish(gui_path);
00409 marker_pub.publish(ma);
00410
00411
00412 ROS_DEBUG("done wrapper\n\n\n\n");
00413 return true;
00414 }
00415
00416 void SBPLDynEnvGlobalPlanner::visualizeExpansions(){
00417 vector<SBPL_4Dpt_t> p;
00418 env->getExpansions(&p);
00419 ROS_DEBUG("size=%d\n",p.size());
00420
00421 visualization_msgs::MarkerArray ma;
00422 for(int i=0; i<(int)p.size(); i++){
00423 visualization_msgs::Marker m;
00424 m.header.frame_id = costmap_ros_->getGlobalFrameID();
00425 m.header.stamp = ros::Time::now();
00426 m.ns = "expands";
00427 m.id = i;
00428 m.type = visualization_msgs::Marker::CUBE;
00429 m.action = visualization_msgs::Marker::ADD;
00430 m.pose.position.x = p[i].x + cost_map_.getOriginX();
00431 m.pose.position.y = p[i].y + cost_map_.getOriginY();
00432 m.pose.position.z = 0.001;
00433
00434 btQuaternion temp;
00435 temp.setEulerZYX(p[i].theta,0,0);
00436 m.pose.orientation.x = temp.getX();
00437 m.pose.orientation.y = temp.getY();
00438 m.pose.orientation.z = temp.getZ();
00439 m.pose.orientation.w = temp.getW();
00440
00441 m.scale.x = 0.025;
00442 m.scale.y = 0.025;
00443 m.scale.z = 0.025;
00444 m.color.r = 1.0;
00445 m.color.g = 0.0;
00446 m.color.b = 0.0;
00447 m.color.a = 0.5;
00448 m.lifetime = ros::Duration();
00449
00450 ma.markers.push_back(m);
00451 }
00452 marker_pub.publish(ma);
00453 }
00454