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 #include <algorithm>
00031 #include <limits>
00032 #include <list>
00033 #include <string>
00034 #include <utility>
00035 #include <vector>
00036
00037 #include <ros/ros.h>
00038
00039 #include <costmap_cspace_msgs/CSpace3D.h>
00040 #include <costmap_cspace_msgs/CSpace3DUpdate.h>
00041 #include <diagnostic_updater/diagnostic_updater.h>
00042 #include <nav_msgs/GetPlan.h>
00043 #include <nav_msgs/Path.h>
00044 #include <geometry_msgs/PoseArray.h>
00045 #include <planner_cspace_msgs/PlannerStatus.h>
00046 #include <sensor_msgs/PointCloud.h>
00047 #include <std_srvs/Empty.h>
00048 #include <trajectory_tracker_msgs/PathWithVelocity.h>
00049 #include <trajectory_tracker_msgs/converter.h>
00050
00051 #include <ros/console.h>
00052 #include <tf2/utils.h>
00053 #include <tf2_ros/transform_listener.h>
00054 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00055
00056 #include <actionlib/server/simple_action_server.h>
00057 #include <move_base_msgs/MoveBaseAction.h>
00058
00059 #include <neonavigation_common/compatibility.h>
00060
00061 #include <planner_cspace/bbf.h>
00062 #include <planner_cspace/grid_astar.h>
00063 #include <planner_cspace/planner_3d/grid_metric_converter.h>
00064 #include <planner_cspace/planner_3d/jump_detector.h>
00065 #include <planner_cspace/planner_3d/motion_cache.h>
00066 #include <planner_cspace/planner_3d/rotation_cache.h>
00067 #include <planner_cspace/planner_3d/path_interpolator.h>
00068
00069 #include <omp.h>
00070
00071 class Planner3dNode
00072 {
00073 public:
00074 using Astar = GridAstar<3, 2>;
00075
00076 protected:
00077 using Planner3DActionServer = actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>;
00078
00079 ros::NodeHandle nh_;
00080 ros::NodeHandle pnh_;
00081 ros::Subscriber sub_map_;
00082 ros::Subscriber sub_map_update_;
00083 ros::Subscriber sub_goal_;
00084 ros::Publisher pub_path_;
00085 ros::Publisher pub_path_velocity_;
00086 ros::Publisher pub_path_poses_;
00087 ros::Publisher pub_debug_;
00088 ros::Publisher pub_hist_;
00089 ros::Publisher pub_start_;
00090 ros::Publisher pub_end_;
00091 ros::Publisher pub_status_;
00092 ros::ServiceServer srs_forget_;
00093 ros::ServiceServer srs_make_plan_;
00094
00095 std::shared_ptr<Planner3DActionServer> act_;
00096 tf2_ros::Buffer tfbuf_;
00097 tf2_ros::TransformListener tfl_;
00098
00099 Astar as_;
00100 Astar::Gridmap<char, 0x40> cm_;
00101 Astar::Gridmap<bbf::BinaryBayesFilter, 0x20> cm_hist_bbf_;
00102 Astar::Gridmap<char, 0x40> cm_hist_;
00103 Astar::Gridmap<char, 0x80> cm_rough_;
00104 Astar::Gridmap<char, 0x40> cm_base_;
00105 Astar::Gridmap<char, 0x80> cm_rough_base_;
00106 Astar::Gridmap<char, 0x80> cm_hyst_;
00107 Astar::Gridmap<float> cost_estim_cache_;
00108
00109 Astar::Vecf euclid_cost_coef_;
00110
00111 float euclidCost(const Astar::Vec& v, const Astar::Vecf coef) const
00112 {
00113 Astar::Vec vc = v;
00114 float cost = 0;
00115 for (int i = 0; i < as_.getNoncyclic(); i++)
00116 {
00117 cost += powf(coef[i] * vc[i], 2.0);
00118 }
00119 cost = sqrtf(cost);
00120 vc.cycle(cm_.size());
00121 for (int i = as_.getNoncyclic(); i < as_.getDim(); i++)
00122 {
00123 cost += fabs(coef[i] * vc[i]);
00124 }
00125 return cost;
00126 }
00127 float euclidCost(const Astar::Vec& v) const
00128 {
00129 return euclidCost(v, euclid_cost_coef_);
00130 }
00131
00132 RotationCache rot_cache_;
00133 PathInterpolator path_interpolator_;
00134
00135 costmap_cspace_msgs::MapMetaData3D map_info_;
00136 std_msgs::Header map_header_;
00137 float max_vel_;
00138 float max_ang_vel_;
00139 float freq_;
00140 float freq_min_;
00141 float search_range_;
00142 int range_;
00143 int local_range_;
00144 double local_range_f_;
00145 int longcut_range_;
00146 double longcut_range_f_;
00147 int esc_range_;
00148 int esc_angle_;
00149 double esc_range_f_;
00150 int tolerance_range_;
00151 int tolerance_angle_;
00152 double tolerance_range_f_;
00153 double tolerance_angle_f_;
00154 int unknown_cost_;
00155 bool overwrite_cost_;
00156 bool has_map_;
00157 bool has_goal_;
00158 bool has_start_;
00159 bool goal_updated_;
00160 bool remember_updates_;
00161 bool fast_map_update_;
00162 std::vector<Astar::Vec> search_list_;
00163 std::vector<Astar::Vec> search_list_rough_;
00164 double hist_ignore_range_f_;
00165 int hist_ignore_range_;
00166 double hist_ignore_range_max_f_;
00167 int hist_ignore_range_max_;
00168 bool temporary_escape_;
00169 float remember_hit_odds_;
00170 float remember_miss_odds_;
00171 bool use_path_with_velocity_;
00172 float min_curve_raduis_;
00173
00174 JumpDetector jump_;
00175 std::string robot_frame_;
00176
00177 int max_retry_num_;
00178
00179 int num_task_;
00180
00181
00182 class CostCoeff
00183 {
00184 public:
00185 float weight_decel_;
00186 float weight_backward_;
00187 float weight_ang_vel_;
00188 float weight_costmap_;
00189 float weight_costmap_turn_;
00190 float weight_remembered_;
00191 float weight_hysteresis_;
00192 float in_place_turn_;
00193 float hysteresis_max_dist_;
00194 float hysteresis_expand_;
00195 };
00196 CostCoeff cc_;
00197
00198 geometry_msgs::PoseStamped start_;
00199 geometry_msgs::PoseStamped goal_;
00200 geometry_msgs::PoseStamped goal_raw_;
00201 Astar::Vecf ec_;
00202 Astar::Vecf ec_rough_;
00203 Astar::Vecf resolution_;
00204 double goal_tolerance_lin_f_;
00205 double goal_tolerance_ang_f_;
00206 double goal_tolerance_ang_finish_;
00207 int goal_tolerance_lin_;
00208 int goal_tolerance_ang_;
00209 float angle_resolution_aspect_;
00210
00211 enum DebugMode
00212 {
00213 DEBUG_HYSTERESIS,
00214 DEBUG_HISTORY,
00215 DEBUG_COST_ESTIM
00216 };
00217 DebugMode debug_out_;
00218
00219 planner_cspace_msgs::PlannerStatus status_;
00220
00221 bool find_best_;
00222 float sw_wait_;
00223
00224 float rough_cost_max_;
00225 bool rough_;
00226
00227 bool force_goal_orientation_;
00228
00229 bool escaping_;
00230
00231 int cnt_stuck_;
00232
00233 MotionCache motion_cache_;
00234 MotionCache motion_cache_linear_;
00235
00236 diagnostic_updater::Updater diag_updater_;
00237 ros::Duration costmap_watchdog_;
00238 ros::Time last_costmap_;
00239
00240 bool cbForget(std_srvs::EmptyRequest& req,
00241 std_srvs::EmptyResponse& res)
00242 {
00243 ROS_WARN("Forgetting remembered costmap.");
00244 if (has_map_)
00245 cm_hist_bbf_.clear(bbf::BinaryBayesFilter(bbf::MIN_ODDS));
00246
00247 return true;
00248 }
00249 bool cbMakePlan(nav_msgs::GetPlan::Request& req,
00250 nav_msgs::GetPlan::Response& res)
00251 {
00252 if (!has_map_)
00253 return false;
00254
00255 Astar::Vec s, e;
00256 grid_metric_converter::metric2Grid(
00257 map_info_, s[0], s[1], s[2],
00258 req.start.pose.position.x, req.start.pose.position.y, tf2::getYaw(req.start.pose.orientation));
00259 s[2] = 0;
00260 grid_metric_converter::metric2Grid(
00261 map_info_, e[0], e[1], e[2],
00262 req.goal.pose.position.x, req.goal.pose.position.y, tf2::getYaw(req.goal.pose.orientation));
00263 e[2] = 0;
00264
00265 if (!(cm_rough_.validate(s, range_) && cm_rough_.validate(e, range_)))
00266 {
00267 ROS_ERROR("Given start or goal is not on the map.");
00268 return false;
00269 }
00270 else if (cm_rough_[s] == 100 || cm_rough_[e] == 100)
00271 {
00272 ROS_ERROR("Given start or goal is in Rock.");
00273 return false;
00274 }
00275
00276 const Astar::Vecf euclid_cost_coef = ec_rough_;
00277
00278 const auto cb_cost = [this, &euclid_cost_coef](
00279 const Astar::Vec& s, Astar::Vec& e,
00280 const Astar::Vec& v_goal, const Astar::Vec& v_start,
00281 const bool hyst) -> float
00282 {
00283 const Astar::Vec d = e - s;
00284 float cost = euclidCost(d, euclid_cost_coef);
00285
00286 int sum = 0;
00287 const auto cache_page = motion_cache_linear_.find(0, d);
00288 if (cache_page == motion_cache_linear_.end(0))
00289 return -1;
00290 const int num = cache_page->second.getMotion().size();
00291 for (const auto& pos_diff : cache_page->second.getMotion())
00292 {
00293 const Astar::Vec pos(s[0] + pos_diff[0], s[1] + pos_diff[1], 0);
00294 const auto c = cm_rough_[pos];
00295 if (c > 99)
00296 return -1;
00297 sum += c;
00298 }
00299 const float distf = cache_page->second.getDistance();
00300 cost += sum * map_info_.linear_resolution * distf * cc_.weight_costmap_ / (100.0 * num);
00301
00302 return cost;
00303 };
00304 const auto cb_cost_estim = [this, &euclid_cost_coef](
00305 const Astar::Vec& s, const Astar::Vec& e)
00306 {
00307 const Astar::Vec d = e - s;
00308 const float cost = euclidCost(d, euclid_cost_coef);
00309
00310 return cost;
00311 };
00312 const auto cb_search = [this](
00313 const Astar::Vec& p,
00314 const Astar::Vec& s, const Astar::Vec& e) -> std::vector<Astar::Vec>&
00315 {
00316 return search_list_rough_;
00317 };
00318 const auto cb_progress = [](const std::list<Astar::Vec>& path_grid)
00319 {
00320 return true;
00321 };
00322
00323 const auto ts = boost::chrono::high_resolution_clock::now();
00324
00325
00326 std::list<Astar::Vec> path_grid;
00327 if (!as_.search(s, e, path_grid,
00328 std::bind(cb_cost,
00329 std::placeholders::_1, std::placeholders::_2,
00330 std::placeholders::_3, std::placeholders::_4, false),
00331 cb_cost_estim, cb_search, cb_progress,
00332 0,
00333 1.0f / freq_min_,
00334 find_best_))
00335 {
00336 ROS_WARN("Path plan failed (goal unreachable)");
00337 return false;
00338 }
00339 const auto tnow = boost::chrono::high_resolution_clock::now();
00340 ROS_INFO("Path found (%0.4f sec.)",
00341 boost::chrono::duration<float>(tnow - ts).count());
00342
00343 nav_msgs::Path path;
00344 path.header = map_header_;
00345 path.header.stamp = ros::Time::now();
00346
00347 const std::list<Astar::Vecf> path_interpolated =
00348 path_interpolator_.interpolate(path_grid, 0.5, 0.0);
00349 grid_metric_converter::grid2MetricPath(map_info_, path_interpolated, path);
00350
00351 res.plan.header = map_header_;
00352 res.plan.poses.resize(path.poses.size());
00353 for (size_t i = 0; i < path.poses.size(); ++i)
00354 {
00355 res.plan.poses[i] = path.poses[i];
00356 }
00357
00358 return true;
00359 }
00360
00361 void cbGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
00362 {
00363 if (act_->isActive())
00364 {
00365 ROS_ERROR("Setting new goal is ignored since planner_3d is proceeding the action.");
00366 return;
00367 }
00368 setGoal(*msg);
00369 }
00370 void cbPreempt()
00371 {
00372 ROS_WARN("Preempting the current goal.");
00373 act_->setPreempted(move_base_msgs::MoveBaseResult(), "Preempted.");
00374 has_goal_ = false;
00375 status_.status = planner_cspace_msgs::PlannerStatus::DONE;
00376 }
00377 bool setGoal(const geometry_msgs::PoseStamped& msg)
00378 {
00379 goal_raw_ = goal_ = msg;
00380
00381 const double len2 =
00382 goal_.pose.orientation.x * goal_.pose.orientation.x +
00383 goal_.pose.orientation.y * goal_.pose.orientation.y +
00384 goal_.pose.orientation.z * goal_.pose.orientation.z +
00385 goal_.pose.orientation.w * goal_.pose.orientation.w;
00386 if (fabs(len2 - 1.0) < 0.1)
00387 {
00388 escaping_ = false;
00389 has_goal_ = true;
00390 cnt_stuck_ = 0;
00391 if (!updateGoal())
00392 {
00393 has_goal_ = false;
00394 return false;
00395 }
00396 status_.status = planner_cspace_msgs::PlannerStatus::DOING;
00397 pub_status_.publish(status_);
00398 diag_updater_.update();
00399 }
00400 else
00401 {
00402 has_goal_ = false;
00403 if (act_->isActive())
00404 act_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal cleared.");
00405 }
00406 return true;
00407 }
00408 void fillCostmap(
00409 reservable_priority_queue<Astar::PriorityVec>& open,
00410 Astar::Gridmap<float>& g,
00411 const Astar::Vec& s, const Astar::Vec& e)
00412 {
00413 const Astar::Vec s_rough(s[0], s[1], 0);
00414
00415 while (true)
00416 {
00417 if (open.size() < 1)
00418 break;
00419
00420 std::vector<Astar::PriorityVec> centers;
00421 for (size_t i = 0; i < static_cast<size_t>(num_task_); ++i)
00422 {
00423 if (open.size() < 1)
00424 break;
00425 const auto center = open.top();
00426 open.pop();
00427 if (center.p_raw_ > g[center.v_])
00428 continue;
00429 if (center.p_raw_ - ec_rough_[0] * (range_ + local_range_ + longcut_range_) > g[s_rough])
00430 continue;
00431 centers.push_back(center);
00432 }
00433 #pragma omp parallel for schedule(static)
00434 for (auto it = centers.begin(); it < centers.end(); ++it)
00435 {
00436 const Astar::Vec p = it->v_;
00437 const float c = it->p_raw_;
00438
00439 Astar::Vec d;
00440 d[2] = 0;
00441
00442 const int range_rough = 4;
00443 for (d[0] = -range_rough; d[0] <= range_rough; d[0]++)
00444 {
00445 for (d[1] = -range_rough; d[1] <= range_rough; d[1]++)
00446 {
00447 if (d[0] == 0 && d[1] == 0)
00448 continue;
00449 if (d.sqlen() > range_rough * range_rough)
00450 continue;
00451
00452 const Astar::Vec next = p + d;
00453 if ((unsigned int)next[0] >= (unsigned int)map_info_.width ||
00454 (unsigned int)next[1] >= (unsigned int)map_info_.height)
00455 continue;
00456 float& gnext = g[next];
00457 if (gnext < 0)
00458 continue;
00459
00460 float cost = 0;
00461
00462 {
00463 float sum = 0, sum_hist = 0;
00464 const float grid_to_len = d.gridToLenFactor();
00465 const int dist = d.len();
00466 const float dpx = static_cast<float>(d[0]) / dist;
00467 const float dpy = static_cast<float>(d[1]) / dist;
00468 Astar::Vec pos(static_cast<int>(p[0]), static_cast<int>(p[1]), 0);
00469 int i = 0;
00470 for (; i < dist; i++)
00471 {
00472 const char c = cm_rough_[pos];
00473 if (c > 99)
00474 break;
00475 sum += c;
00476
00477 sum_hist += cm_hist_[pos];
00478 pos[0] += dpx;
00479 pos[1] += dpy;
00480 }
00481 if (i != dist)
00482 continue;
00483 cost +=
00484 (map_info_.linear_resolution * grid_to_len / 100.0) *
00485 (sum * cc_.weight_costmap_ + sum_hist * cc_.weight_remembered_);
00486
00487 if (cost < 0)
00488 {
00489 cost = 0;
00490 ROS_WARN_THROTTLE(1.0, "Negative cost value is detected. Limited to zero.");
00491 }
00492 }
00493 cost += euclidCost(d, ec_rough_);
00494
00495 const float gp = c + cost;
00496 if (gnext > gp)
00497 {
00498 gnext = gp;
00499 #pragma omp critical
00500 {
00501 open.push(Astar::PriorityVec(gp, gp, next));
00502 }
00503 }
00504 }
00505 }
00506 }
00507 }
00508 rough_cost_max_ = g[s_rough] + ec_rough_[0] * (range_ + local_range_);
00509 }
00510 bool searchAvailablePos(Astar::Vec& s, const int xy_range, const int angle_range,
00511 const int cost_acceptable = 50, const int min_xy_range = 0)
00512 {
00513 ROS_DEBUG("%d, %d (%d,%d,%d)", xy_range, angle_range, s[0], s[1], s[2]);
00514
00515 float range_min = FLT_MAX;
00516 Astar::Vec s_out;
00517 Astar::Vec d;
00518 for (d[2] = -angle_range; d[2] <= angle_range; d[2]++)
00519 {
00520 for (d[0] = -xy_range; d[0] <= xy_range; d[0]++)
00521 {
00522 for (d[1] = -xy_range; d[1] <= xy_range; d[1]++)
00523 {
00524 if (d[0] == 0 && d[1] == 0 && d[2] == 0)
00525 continue;
00526 if (d.sqlen() > xy_range * xy_range)
00527 continue;
00528 if (d.sqlen() < min_xy_range * min_xy_range)
00529 continue;
00530
00531 Astar::Vec s2 = s + d;
00532 if ((unsigned int)s2[0] >= (unsigned int)map_info_.width ||
00533 (unsigned int)s2[1] >= (unsigned int)map_info_.height)
00534 continue;
00535 s2.cycleUnsigned(map_info_.angle);
00536 if (cm_[s2] >= cost_acceptable)
00537 continue;
00538 const auto cost = euclidCost(d, ec_);
00539 if (cost < range_min)
00540 {
00541 range_min = cost;
00542 s_out = s2;
00543 }
00544 }
00545 }
00546 }
00547
00548 if (range_min == FLT_MAX)
00549 {
00550 if (cost_acceptable != 100)
00551 {
00552 return searchAvailablePos(s, xy_range, angle_range, 100);
00553 }
00554 return false;
00555 }
00556 s = s_out;
00557 s.cycleUnsigned(map_info_.angle);
00558 ROS_DEBUG(" (%d,%d,%d)", s[0], s[1], s[2]);
00559 return true;
00560 }
00561 bool updateGoal(const bool goal_changed = true)
00562 {
00563 if (!has_goal_)
00564 return true;
00565
00566 if (!has_map_ || !has_start_)
00567 {
00568 ROS_ERROR("Goal received, however map/goal/start are not ready. (%d/%d/%d)",
00569 static_cast<int>(has_map_), static_cast<int>(has_goal_), static_cast<int>(has_start_));
00570 return true;
00571 }
00572
00573 Astar::Vec s, e;
00574 grid_metric_converter::metric2Grid(
00575 map_info_, s[0], s[1], s[2],
00576 start_.pose.position.x, start_.pose.position.y,
00577 tf2::getYaw(start_.pose.orientation));
00578 s.cycleUnsigned(map_info_.angle);
00579 grid_metric_converter::metric2Grid(
00580 map_info_, e[0], e[1], e[2],
00581 goal_.pose.position.x, goal_.pose.position.y,
00582 tf2::getYaw(goal_.pose.orientation));
00583 e.cycleUnsigned(map_info_.angle);
00584 if (goal_changed)
00585 {
00586 ROS_INFO(
00587 "New goal received (%d, %d, %d)",
00588 e[0], e[1], e[2]);
00589 }
00590
00591 if (!cm_.validate(e, range_))
00592 {
00593 ROS_ERROR("Given goal is not on the map.");
00594 return false;
00595 }
00596 if (!cm_.validate(s, range_))
00597 {
00598 ROS_ERROR("You are on the edge of the world.");
00599 return false;
00600 }
00601
00602 const auto ts = boost::chrono::high_resolution_clock::now();
00603 reservable_priority_queue<Astar::PriorityVec> open;
00604
00605 cost_estim_cache_.clear(FLT_MAX);
00606 if (cm_[e] == 100)
00607 {
00608 if (!searchAvailablePos(e, tolerance_range_, tolerance_angle_))
00609 {
00610 ROS_WARN("Oops! Goal is in Rock!");
00611 ++cnt_stuck_;
00612 return true;
00613 }
00614 ROS_INFO("Goal moved (%d, %d, %d)",
00615 e[0], e[1], e[2]);
00616 float x, y, yaw;
00617 grid_metric_converter::grid2Metric(map_info_, e[0], e[1], e[2], x, y, yaw);
00618 goal_.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
00619 goal_.pose.position.x = x;
00620 goal_.pose.position.y = y;
00621 }
00622 if (cm_[s] == 100)
00623 {
00624 if (!searchAvailablePos(s, tolerance_range_, tolerance_angle_))
00625 {
00626 ROS_WARN("Oops! You are in Rock!");
00627 return true;
00628 }
00629 }
00630
00631 e[2] = 0;
00632 cost_estim_cache_[e] = -ec_rough_[0] * 0.5;
00633 open.push(Astar::PriorityVec(cost_estim_cache_[e], cost_estim_cache_[e], e));
00634 fillCostmap(open, cost_estim_cache_, s, e);
00635 const auto tnow = boost::chrono::high_resolution_clock::now();
00636 ROS_DEBUG("Cost estimation cache generated (%0.4f sec.)",
00637 boost::chrono::duration<float>(tnow - ts).count());
00638 cost_estim_cache_[e] = 0;
00639
00640 if (goal_changed)
00641 cm_hyst_.clear(100);
00642
00643 publishCostmap();
00644
00645 goal_updated_ = true;
00646
00647 return true;
00648 }
00649 void publishCostmap()
00650 {
00651 if (pub_debug_.getNumSubscribers() == 0)
00652 return;
00653
00654 sensor_msgs::PointCloud debug;
00655 debug.header = map_header_;
00656 debug.header.stamp = ros::Time::now();
00657 {
00658 Astar::Vec p;
00659 for (p[1] = 0; p[1] < cost_estim_cache_.size()[1]; p[1]++)
00660 {
00661 for (p[0] = 0; p[0] < cost_estim_cache_.size()[0]; p[0]++)
00662 {
00663 p[2] = 0;
00664 float x, y, yaw;
00665 grid_metric_converter::grid2Metric(map_info_, p[0], p[1], p[2], x, y, yaw);
00666 geometry_msgs::Point32 point;
00667 point.x = x;
00668 point.y = y;
00669 switch (debug_out_)
00670 {
00671 case DEBUG_HYSTERESIS:
00672 if (cost_estim_cache_[p] == FLT_MAX)
00673 continue;
00674
00675 point.z = 100;
00676 for (p[2] = 0; p[2] < static_cast<int>(map_info_.angle); ++p[2])
00677 point.z = std::min(static_cast<float>(cm_hyst_[p]), point.z);
00678
00679 point.z *= 0.01;
00680 break;
00681 case DEBUG_HISTORY:
00682 if (cm_rough_base_[p] != 0)
00683 continue;
00684 point.z = cm_hist_bbf_[p].getProbability();
00685 break;
00686 case DEBUG_COST_ESTIM:
00687 if (cost_estim_cache_[p] == FLT_MAX)
00688 continue;
00689 point.z = cost_estim_cache_[p] / 500;
00690 break;
00691 }
00692 debug.points.push_back(point);
00693 }
00694 }
00695 }
00696 pub_debug_.publish(debug);
00697 }
00698 void publishEmptyPath()
00699 {
00700 nav_msgs::Path path;
00701 path.header.frame_id = robot_frame_;
00702 path.header.stamp = ros::Time::now();
00703 if (use_path_with_velocity_)
00704 {
00705 pub_path_velocity_.publish(
00706 trajectory_tracker_msgs::toPathWithVelocity(
00707 path, std::numeric_limits<double>::quiet_NaN()));
00708 }
00709 else
00710 {
00711 pub_path_.publish(path);
00712 }
00713 }
00714
00715 void cbMapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr& msg)
00716 {
00717 if (!has_map_)
00718 return;
00719 ROS_DEBUG("Map updated");
00720
00721 const ros::Time now = ros::Time::now();
00722 last_costmap_ = now;
00723
00724 cm_ = cm_base_;
00725 cm_rough_ = cm_rough_base_;
00726 if (remember_updates_)
00727 {
00728 if (pub_hist_.getNumSubscribers() > 0)
00729 {
00730 sensor_msgs::PointCloud pc;
00731 pc.header = map_header_;
00732 pc.header.stamp = now;
00733 Astar::Vec p;
00734 for (p[1] = 0; p[1] < cm_hist_bbf_.size()[1]; p[1]++)
00735 {
00736 for (p[0] = 0; p[0] < cm_hist_bbf_.size()[0]; p[0]++)
00737 {
00738 if (cm_hist_bbf_[p].get() > bbf::probabilityToOdds(0.1))
00739 {
00740 float x, y, yaw;
00741 grid_metric_converter::grid2Metric(map_info_, p[0], p[1], p[2], x, y, yaw);
00742 geometry_msgs::Point32 point;
00743 point.x = x;
00744 point.y = y;
00745 point.z = cm_hist_bbf_[p].getProbability();
00746 pc.points.push_back(point);
00747 }
00748 }
00749 }
00750 pub_hist_.publish(pc);
00751 }
00752 Astar::Vec p;
00753 cm_hist_.clear(0);
00754 for (p[1] = 0; p[1] < cm_hist_bbf_.size()[1]; p[1]++)
00755 {
00756 for (p[0] = 0; p[0] < cm_hist_bbf_.size()[0]; p[0]++)
00757 {
00758 p[2] = 0;
00759 cm_hist_[p] = lroundf(cm_hist_bbf_[p].getNormalizedProbability() * 100.0);
00760 }
00761 }
00762 }
00763
00764 {
00765 const Astar::Vec center(
00766 static_cast<int>(msg->width / 2), static_cast<int>(msg->height / 2), 0);
00767 const Astar::Vec gp(
00768 static_cast<int>(msg->x), static_cast<int>(msg->y), static_cast<int>(msg->yaw));
00769 const Astar::Vec gp_rough(gp[0], gp[1], 0);
00770 const int hist_ignore_range_sq = hist_ignore_range_ * hist_ignore_range_;
00771 const int hist_ignore_range_max_sq =
00772 hist_ignore_range_max_ * hist_ignore_range_max_;
00773 Astar::Vec p;
00774 for (p[0] = 0; p[0] < static_cast<int>(msg->width); p[0]++)
00775 {
00776 for (p[1] = 0; p[1] < static_cast<int>(msg->height); p[1]++)
00777 {
00778 int cost_min = 100;
00779 for (p[2] = 0; p[2] < static_cast<int>(msg->angle); p[2]++)
00780 {
00781 const size_t addr = ((p[2] * msg->height) + p[1]) * msg->width + p[0];
00782 const char c = msg->data[addr];
00783 if (c < cost_min)
00784 cost_min = c;
00785 }
00786 p[2] = 0;
00787 if (cost_min > cm_rough_[gp_rough + p])
00788 {
00789 cm_rough_[gp_rough + p] = cost_min;
00790 }
00791
00792 Astar::Vec pos = gp + p;
00793 pos[2] = 0;
00794 if (cost_min == 100)
00795 {
00796 const Astar::Vec p2 = p - center;
00797 const float sqlen = p2.sqlen();
00798 if (sqlen > hist_ignore_range_sq &&
00799 sqlen < hist_ignore_range_max_sq)
00800 {
00801 cm_hist_bbf_[pos].update(remember_hit_odds_);
00802 }
00803 }
00804 else if (cost_min >= 0)
00805 {
00806 const Astar::Vec p2 = p - center;
00807 const float sqlen = p2.sqlen();
00808 if (sqlen < hist_ignore_range_max_sq)
00809 {
00810 cm_hist_bbf_[pos].update(remember_miss_odds_);
00811 }
00812 }
00813
00814 for (p[2] = 0; p[2] < static_cast<int>(msg->angle); p[2]++)
00815 {
00816 const size_t addr = ((p[2] * msg->height) + p[1]) * msg->width + p[0];
00817 const char c = msg->data[addr];
00818 if (overwrite_cost_)
00819 {
00820 if (c >= 0)
00821 cm_[gp + p] = c;
00822 }
00823 else
00824 {
00825 if (cm_[gp + p] < c)
00826 cm_[gp + p] = c;
00827 }
00828 }
00829 }
00830 }
00831 }
00832 if (!has_goal_ || !has_start_)
00833 return;
00834
00835 if (!fast_map_update_)
00836 {
00837 updateGoal(false);
00838 return;
00839 }
00840
00841 Astar::Vec s, e;
00842 grid_metric_converter::metric2Grid(
00843 map_info_, s[0], s[1], s[2],
00844 start_.pose.position.x, start_.pose.position.y,
00845 tf2::getYaw(start_.pose.orientation));
00846 s.cycleUnsigned(map_info_.angle);
00847 grid_metric_converter::metric2Grid(
00848 map_info_, e[0], e[1], e[2],
00849 goal_.pose.position.x, goal_.pose.position.y,
00850 tf2::getYaw(goal_.pose.orientation));
00851 e.cycleUnsigned(map_info_.angle);
00852
00853 if (cm_[e] == 100)
00854 {
00855 updateGoal(false);
00856 return;
00857 }
00858
00859 e[2] = 0;
00860
00861 const auto ts = boost::chrono::high_resolution_clock::now();
00862
00863 Astar::Vec p, p_cost_min;
00864 p[2] = 0;
00865 float cost_min = FLT_MAX;
00866 for (p[1] = static_cast<int>(msg->y); p[1] < static_cast<int>(msg->y + msg->height); p[1]++)
00867 {
00868 for (p[0] = static_cast<int>(msg->x); p[0] < static_cast<int>(msg->x + msg->width); p[0]++)
00869 {
00870 if (cost_min > cost_estim_cache_[p])
00871 {
00872 p_cost_min = p;
00873 cost_min = cost_estim_cache_[p];
00874 }
00875 }
00876 }
00877
00878 reservable_priority_queue<Astar::PriorityVec> open;
00879 reservable_priority_queue<Astar::PriorityVec> erase;
00880 open.reserve(map_info_.width * map_info_.height / 2);
00881 erase.reserve(map_info_.width * map_info_.height / 2);
00882
00883 if (cost_min != FLT_MAX)
00884 erase.push(Astar::PriorityVec(cost_min, cost_min, p_cost_min));
00885 while (true)
00886 {
00887 if (erase.size() < 1)
00888 break;
00889 const Astar::PriorityVec center = erase.top();
00890 const Astar::Vec p = center.v_;
00891 erase.pop();
00892
00893 if (cost_estim_cache_[p] == FLT_MAX)
00894 continue;
00895 cost_estim_cache_[p] = FLT_MAX;
00896
00897 Astar::Vec d;
00898 d[2] = 0;
00899 for (d[0] = -1; d[0] <= 1; d[0]++)
00900 {
00901 for (d[1] = -1; d[1] <= 1; d[1]++)
00902 {
00903 if (!((d[0] == 0) ^ (d[1] == 0)))
00904 continue;
00905 const Astar::Vec next = p + d;
00906 if ((unsigned int)next[0] >= (unsigned int)map_info_.width ||
00907 (unsigned int)next[1] >= (unsigned int)map_info_.height)
00908 continue;
00909 const float& gn = cost_estim_cache_[next];
00910 if (gn == FLT_MAX)
00911 continue;
00912 if (gn < cost_min)
00913 {
00914 open.push(Astar::PriorityVec(gn, gn, next));
00915 continue;
00916 }
00917 erase.push(Astar::PriorityVec(gn, gn, next));
00918 }
00919 }
00920 }
00921 if (open.size() == 0)
00922 {
00923 open.push(Astar::PriorityVec(-ec_rough_[0] * 0.5, -ec_rough_[0] * 0.5, e));
00924 }
00925 {
00926 Astar::Vec p;
00927 p[2] = 0;
00928 for (p[0] = 0; p[0] < static_cast<int>(map_info_.width); p[0]++)
00929 {
00930 for (p[1] = 0; p[1] < static_cast<int>(map_info_.height); p[1]++)
00931 {
00932 const auto& gp = cost_estim_cache_[p];
00933 if (gp > rough_cost_max_)
00934 {
00935 open.push(Astar::PriorityVec(gp, gp, p));
00936 }
00937 }
00938 }
00939 }
00940
00941 fillCostmap(open, cost_estim_cache_, s, e);
00942 const auto tnow = boost::chrono::high_resolution_clock::now();
00943 ROS_DEBUG("Cost estimation cache updated (%0.4f sec.)",
00944 boost::chrono::duration<float>(tnow - ts).count());
00945 publishCostmap();
00946 }
00947 void cbMap(const costmap_cspace_msgs::CSpace3D::ConstPtr& msg)
00948 {
00949 ROS_INFO("Map received");
00950 ROS_INFO(" linear_resolution %0.2f x (%dx%d) px", msg->info.linear_resolution,
00951 msg->info.width, msg->info.height);
00952 ROS_INFO(" angular_resolution %0.2f x %d px", msg->info.angular_resolution,
00953 msg->info.angle);
00954 ROS_INFO(" origin %0.3f m, %0.3f m, %0.3f rad",
00955 msg->info.origin.position.x,
00956 msg->info.origin.position.y,
00957 tf2::getYaw(msg->info.origin.orientation));
00958
00959
00960 publishEmptyPath();
00961
00962 const float ec_val[3] =
00963 {
00964 1.0f / max_vel_,
00965 1.0f / max_vel_,
00966 1.0f * cc_.weight_ang_vel_ / max_ang_vel_
00967 };
00968 ec_ = Astar::Vecf(ec_val[0], ec_val[1], ec_val[2]);
00969 ec_rough_ = Astar::Vecf(ec_val[0], ec_val[1], 0);
00970
00971 if (map_info_.linear_resolution != msg->info.linear_resolution ||
00972 map_info_.angular_resolution != msg->info.angular_resolution)
00973 {
00974 map_info_ = msg->info;
00975 Astar::Vec d;
00976 range_ = static_cast<int>(search_range_ / map_info_.linear_resolution);
00977
00978 costmap_cspace_msgs::MapMetaData3D map_info_linear(map_info_);
00979 map_info_linear.angle = 1;
00980 motion_cache_linear_.reset(
00981 map_info_linear.linear_resolution,
00982 map_info_linear.angular_resolution,
00983 range_,
00984 cm_rough_.getAddressor());
00985 motion_cache_.reset(
00986 map_info_.linear_resolution,
00987 map_info_.angular_resolution,
00988 range_,
00989 cm_.getAddressor());
00990
00991 search_list_.clear();
00992 for (d[0] = -range_; d[0] <= range_; d[0]++)
00993 {
00994 for (d[1] = -range_; d[1] <= range_; d[1]++)
00995 {
00996 if (d.sqlen() > range_ * range_)
00997 continue;
00998 for (d[2] = 0; d[2] < static_cast<int>(map_info_.angle); d[2]++)
00999 {
01000 search_list_.push_back(d);
01001 }
01002 }
01003 }
01004 search_list_rough_.clear();
01005 for (d[0] = -range_; d[0] <= range_; d[0]++)
01006 {
01007 for (d[1] = -range_; d[1] <= range_; d[1]++)
01008 {
01009 if (d.sqlen() > range_ * range_)
01010 continue;
01011 d[2] = 0;
01012 search_list_rough_.push_back(d);
01013 }
01014 }
01015 ROS_DEBUG("Search list updated (range: ang %d, lin %d) %d",
01016 map_info_.angle, range_, static_cast<int>(search_list_.size()));
01017
01018 rot_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, range_);
01019 path_interpolator_.reset(map_info_.angular_resolution, range_);
01020 ROS_DEBUG("Rotation cache generated");
01021 }
01022 else
01023 {
01024 map_info_ = msg->info;
01025 }
01026 map_header_ = msg->header;
01027 jump_.setMapFrame(map_header_.frame_id);
01028
01029 resolution_[0] = 1.0 / map_info_.linear_resolution;
01030 resolution_[1] = 1.0 / map_info_.linear_resolution;
01031 resolution_[2] = 1.0 / map_info_.angular_resolution;
01032
01033 hist_ignore_range_ = lroundf(hist_ignore_range_f_ / map_info_.linear_resolution);
01034 hist_ignore_range_max_ = lroundf(hist_ignore_range_max_f_ / map_info_.linear_resolution);
01035 local_range_ = lroundf(local_range_f_ / map_info_.linear_resolution);
01036 longcut_range_ = lroundf(longcut_range_f_ / map_info_.linear_resolution);
01037 esc_range_ = lroundf(esc_range_f_ / map_info_.linear_resolution);
01038 esc_angle_ = map_info_.angle / 8;
01039 tolerance_range_ = lroundf(tolerance_range_f_ / map_info_.linear_resolution);
01040 tolerance_angle_ = lroundf(tolerance_angle_f_ / map_info_.angular_resolution);
01041 goal_tolerance_lin_ = lroundf(goal_tolerance_lin_f_ / map_info_.linear_resolution);
01042 goal_tolerance_ang_ = lroundf(goal_tolerance_ang_f_ / map_info_.angular_resolution);
01043
01044 const int size[3] =
01045 {
01046 static_cast<int>(map_info_.width),
01047 static_cast<int>(map_info_.height),
01048 static_cast<int>(map_info_.angle)
01049 };
01050 as_.reset(Astar::Vec(size[0], size[1], size[2]));
01051 cm_.reset(Astar::Vec(size[0], size[1], size[2]));
01052 cm_hyst_.reset(Astar::Vec(size[0], size[1], size[2]));
01053
01054 cost_estim_cache_.reset(Astar::Vec(size[0], size[1], 1));
01055 cm_rough_.reset(Astar::Vec(size[0], size[1], 1));
01056 cm_hist_.reset(Astar::Vec(size[0], size[1], 1));
01057 cm_hist_bbf_.reset(Astar::Vec(size[0], size[1], 1));
01058
01059 Astar::Vec p;
01060 for (p[0] = 0; p[0] < static_cast<int>(map_info_.width); p[0]++)
01061 {
01062 for (p[1] = 0; p[1] < static_cast<int>(map_info_.height); p[1]++)
01063 {
01064 int cost_min = 100;
01065 for (p[2] = 0; p[2] < static_cast<int>(map_info_.angle); p[2]++)
01066 {
01067 const size_t addr = ((p[2] * size[1]) + p[1]) * size[0] + p[0];
01068 char c = msg->data[addr];
01069 if (c < 0)
01070 c = unknown_cost_;
01071 cm_[p] = c;
01072 if (c < cost_min)
01073 cost_min = c;
01074 }
01075 p[2] = 0;
01076 cm_rough_[p] = cost_min;
01077 }
01078 }
01079 ROS_DEBUG("Map copied");
01080 cm_hyst_.clear(100);
01081
01082 has_map_ = true;
01083
01084 cm_rough_base_ = cm_rough_;
01085 cm_base_ = cm_;
01086 cm_hist_bbf_.clear(bbf::BinaryBayesFilter(bbf::MIN_ODDS));
01087 cm_hist_.clear(0);
01088
01089 updateGoal();
01090 }
01091 void cbAction()
01092 {
01093 move_base_msgs::MoveBaseGoalConstPtr goal = act_->acceptNewGoal();
01094 if (!setGoal(goal->target_pose))
01095 act_->setAborted(move_base_msgs::MoveBaseResult(), "Given goal is invalid.");
01096 }
01097
01098 void updateStart()
01099 {
01100 geometry_msgs::PoseStamped start;
01101 start.header.frame_id = robot_frame_;
01102 start.header.stamp = ros::Time(0);
01103 start.pose.orientation.x = 0.0;
01104 start.pose.orientation.y = 0.0;
01105 start.pose.orientation.z = 0.0;
01106 start.pose.orientation.w = 1.0;
01107 start.pose.position.x = 0;
01108 start.pose.position.y = 0;
01109 start.pose.position.z = 0;
01110 try
01111 {
01112 geometry_msgs::TransformStamped trans =
01113 tfbuf_.lookupTransform(map_header_.frame_id, robot_frame_, ros::Time(), ros::Duration(0.1));
01114 tf2::doTransform(start, start, trans);
01115 }
01116 catch (tf2::TransformException& e)
01117 {
01118 has_start_ = false;
01119 return;
01120 }
01121 start_ = start;
01122 has_start_ = true;
01123 }
01124
01125 public:
01126 Planner3dNode()
01127 : nh_()
01128 , pnh_("~")
01129 , tfl_(tfbuf_)
01130 , jump_(tfbuf_)
01131 {
01132 neonavigation_common::compat::checkCompatMode();
01133 sub_map_ = neonavigation_common::compat::subscribe(
01134 nh_, "costmap",
01135 pnh_, "costmap", 1, &Planner3dNode::cbMap, this);
01136 sub_map_update_ = neonavigation_common::compat::subscribe(
01137 nh_, "costmap_update",
01138 pnh_, "costmap_update", 1, &Planner3dNode::cbMapUpdate, this);
01139 sub_goal_ = neonavigation_common::compat::subscribe(
01140 nh_, "move_base_simple/goal",
01141 pnh_, "goal", 1, &Planner3dNode::cbGoal, this);
01142 pub_debug_ = pnh_.advertise<sensor_msgs::PointCloud>("debug", 1, true);
01143 pub_hist_ = pnh_.advertise<sensor_msgs::PointCloud>("remembered", 1, true);
01144 pub_start_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_start", 1, true);
01145 pub_end_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_end", 1, true);
01146 pub_status_ = pnh_.advertise<planner_cspace_msgs::PlannerStatus>("status", 1, true);
01147 srs_forget_ = neonavigation_common::compat::advertiseService(
01148 nh_, "forget_planning_cost",
01149 pnh_, "forget", &Planner3dNode::cbForget, this);
01150 srs_make_plan_ = pnh_.advertiseService("make_plan", &Planner3dNode::cbMakePlan, this);
01151
01152 act_.reset(new Planner3DActionServer(ros::NodeHandle(), "move_base", false));
01153 act_->registerGoalCallback(boost::bind(&Planner3dNode::cbAction, this));
01154 act_->registerPreemptCallback(boost::bind(&Planner3dNode::cbPreempt, this));
01155
01156 pnh_.param("use_path_with_velocity", use_path_with_velocity_, false);
01157 if (use_path_with_velocity_)
01158 {
01159 pub_path_velocity_ = nh_.advertise<trajectory_tracker_msgs::PathWithVelocity>(
01160 "path_velocity", 1, true);
01161 }
01162 else
01163 {
01164 pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
01165 nh_, "path",
01166 pnh_, "path", 1, true);
01167 }
01168 pub_path_poses_ = pnh_.advertise<geometry_msgs::PoseArray>(
01169 "path_poses", 1, true);
01170
01171 pnh_.param("freq", freq_, 4.0f);
01172 pnh_.param("freq_min", freq_min_, 2.0f);
01173 pnh_.param("search_range", search_range_, 0.4f);
01174
01175 double costmap_watchdog;
01176 pnh_.param("costmap_watchdog", costmap_watchdog, 0.0);
01177 costmap_watchdog_ = ros::Duration(costmap_watchdog);
01178
01179 pnh_.param("max_vel", max_vel_, 0.3f);
01180 pnh_.param("max_ang_vel", max_ang_vel_, 0.6f);
01181 pnh_.param("min_curve_raduis", min_curve_raduis_, 0.1f);
01182
01183 pnh_.param("weight_decel", cc_.weight_decel_, 50.0f);
01184 pnh_.param("weight_backward", cc_.weight_backward_, 0.9f);
01185 pnh_.param("weight_ang_vel", cc_.weight_ang_vel_, 1.0f);
01186 pnh_.param("weight_costmap", cc_.weight_costmap_, 50.0f);
01187 pnh_.param("weight_costmap_turn", cc_.weight_costmap_turn_, 0.0f);
01188 pnh_.param("weight_remembered", cc_.weight_remembered_, 1000.0f);
01189 pnh_.param("cost_in_place_turn", cc_.in_place_turn_, 30.0f);
01190 pnh_.param("hysteresis_max_dist", cc_.hysteresis_max_dist_, 0.1f);
01191 pnh_.param("hysteresis_expand", cc_.hysteresis_expand_, 0.1f);
01192 pnh_.param("weight_hysteresis", cc_.weight_hysteresis_, 5.0f);
01193
01194 pnh_.param("goal_tolerance_lin", goal_tolerance_lin_f_, 0.05);
01195 pnh_.param("goal_tolerance_ang", goal_tolerance_ang_f_, 0.1);
01196 pnh_.param("goal_tolerance_ang_finish", goal_tolerance_ang_finish_, 0.05);
01197
01198 pnh_.param("unknown_cost", unknown_cost_, 100);
01199 pnh_.param("overwrite_cost", overwrite_cost_, false);
01200
01201 pnh_.param("hist_ignore_range", hist_ignore_range_f_, 0.6);
01202 pnh_.param("hist_ignore_range_max", hist_ignore_range_max_f_, 1.25);
01203 pnh_.param("remember_updates", remember_updates_, false);
01204 double remember_hit_prob, remember_miss_prob;
01205 pnh_.param("remember_hit_prob", remember_hit_prob, 0.6);
01206 pnh_.param("remember_miss_prob", remember_miss_prob, 0.3);
01207 remember_hit_odds_ = bbf::probabilityToOdds(remember_hit_prob);
01208 remember_miss_odds_ = bbf::probabilityToOdds(remember_miss_prob);
01209
01210 pnh_.param("local_range", local_range_f_, 2.5);
01211 pnh_.param("longcut_range", longcut_range_f_, 0.0);
01212 pnh_.param("esc_range", esc_range_f_, 0.25);
01213 pnh_.param("tolerance_range", tolerance_range_f_, 0.25);
01214 pnh_.param("tolerance_angle", tolerance_angle_f_, 0.0);
01215
01216 pnh_.param("sw_wait", sw_wait_, 0.0f);
01217 pnh_.param("find_best", find_best_, true);
01218
01219 pnh_.param("robot_frame", robot_frame_, std::string("base_link"));
01220
01221 double pos_jump, yaw_jump;
01222 std::string jump_detect_frame;
01223 pnh_.param("pos_jump", pos_jump, 1.0);
01224 pnh_.param("yaw_jump", yaw_jump, 1.5);
01225 pnh_.param("jump_detect_frame", jump_detect_frame, std::string("base_link"));
01226 jump_.setBaseFrame(jump_detect_frame);
01227 jump_.setThresholds(pos_jump, yaw_jump);
01228
01229 pnh_.param("force_goal_orientation", force_goal_orientation_, true);
01230
01231 pnh_.param("temporary_escape", temporary_escape_, true);
01232
01233 pnh_.param("fast_map_update", fast_map_update_, false);
01234 if (fast_map_update_)
01235 {
01236 ROS_WARN("planner_3d: Experimental fast_map_update is enabled. ");
01237 }
01238 std::string debug_mode;
01239 pnh_.param("debug_mode", debug_mode, std::string("cost_estim"));
01240 if (debug_mode == "hyst")
01241 debug_out_ = DEBUG_HYSTERESIS;
01242 else if (debug_mode == "hist")
01243 debug_out_ = DEBUG_HISTORY;
01244 else if (debug_mode == "cost_estim")
01245 debug_out_ = DEBUG_COST_ESTIM;
01246
01247 bool print_planning_duration;
01248 pnh_.param("print_planning_duration", print_planning_duration, false);
01249 if (print_planning_duration)
01250 {
01251 if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
01252 {
01253 ros::console::notifyLoggerLevelsChanged();
01254 }
01255 }
01256
01257 pnh_.param("max_retry_num", max_retry_num_, -1);
01258
01259 int queue_size_limit;
01260 pnh_.param("queue_size_limit", queue_size_limit, 0);
01261 as_.setQueueSizeLimit(queue_size_limit);
01262
01263 int num_threads;
01264 pnh_.param("num_threads", num_threads, 1);
01265 omp_set_num_threads(num_threads);
01266
01267 pnh_.param("num_search_task", num_task_, num_threads * 16);
01268 as_.setSearchTaskNum(num_task_);
01269
01270 status_.status = planner_cspace_msgs::PlannerStatus::DONE;
01271
01272 has_map_ = false;
01273 has_goal_ = false;
01274 has_start_ = false;
01275 goal_updated_ = false;
01276
01277 escaping_ = false;
01278 cnt_stuck_ = 0;
01279
01280 diag_updater_.setHardwareID("none");
01281 diag_updater_.add("Path Planner Status", this, &Planner3dNode::diagnoseStatus);
01282
01283 act_->start();
01284 }
01285 void spin()
01286 {
01287 ros::Rate wait(freq_);
01288 ROS_DEBUG("Initialized");
01289
01290 while (ros::ok())
01291 {
01292 wait.sleep();
01293 ros::spinOnce();
01294
01295 const ros::Time now = ros::Time::now();
01296
01297 if (has_map_)
01298 {
01299 updateStart();
01300 if (jump_.detectJump())
01301 {
01302 cm_hist_bbf_.clear(bbf::BinaryBayesFilter(bbf::MIN_ODDS));
01303 }
01304
01305 if (!goal_updated_ && has_goal_)
01306 updateGoal();
01307 }
01308
01309 bool has_costmap(false);
01310 if (costmap_watchdog_ > ros::Duration(0))
01311 {
01312 if (last_costmap_ + costmap_watchdog_ < now)
01313 {
01314 ROS_WARN(
01315 "Navigation is stopping since the costmap is too old (costmap: %0.3f)",
01316 last_costmap_.toSec());
01317 status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING;
01318 publishEmptyPath();
01319 }
01320 else
01321 {
01322 has_costmap = true;
01323 }
01324 }
01325 else
01326 {
01327 has_costmap = true;
01328 }
01329
01330 if (has_map_ && has_goal_ && has_start_ && has_costmap)
01331 {
01332 if (act_->isActive())
01333 {
01334 move_base_msgs::MoveBaseFeedback feedback;
01335 feedback.base_position = start_;
01336 act_->publishFeedback(feedback);
01337 }
01338
01339 if (status_.status == planner_cspace_msgs::PlannerStatus::FINISHING)
01340 {
01341 const float yaw_s = tf2::getYaw(start_.pose.orientation);
01342 float yaw_g = tf2::getYaw(goal_.pose.orientation);
01343 if (force_goal_orientation_)
01344 yaw_g = tf2::getYaw(goal_raw_.pose.orientation);
01345
01346 float yaw_diff = yaw_s - yaw_g;
01347 if (yaw_diff > M_PI)
01348 yaw_diff -= M_PI * 2.0;
01349 else if (yaw_diff < -M_PI)
01350 yaw_diff += M_PI * 2.0;
01351 if (fabs(yaw_diff) < goal_tolerance_ang_finish_)
01352 {
01353 status_.status = planner_cspace_msgs::PlannerStatus::DONE;
01354 has_goal_ = false;
01355 ROS_INFO("Path plan finished");
01356 if (act_->isActive())
01357 act_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
01358 }
01359 }
01360 else
01361 {
01362 if (escaping_)
01363 {
01364 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
01365 }
01366 else if (max_retry_num_ != -1 && cnt_stuck_ > max_retry_num_)
01367 {
01368 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
01369 status_.status = planner_cspace_msgs::PlannerStatus::DONE;
01370 has_goal_ = false;
01371 if (act_->isActive())
01372 act_->setAborted(
01373 move_base_msgs::MoveBaseResult(), "Goal is in Rock");
01374
01375 ROS_ERROR("Exceeded max_retry_num:%d", max_retry_num_);
01376 continue;
01377 }
01378 else
01379 {
01380 status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
01381 }
01382 nav_msgs::Path path;
01383 path.header = map_header_;
01384 path.header.stamp = now;
01385 makePlan(start_.pose, goal_.pose, path, true);
01386 if (use_path_with_velocity_)
01387 {
01388
01389 pub_path_velocity_.publish(
01390 trajectory_tracker_msgs::toPathWithVelocity(path, std::numeric_limits<double>::quiet_NaN()));
01391 }
01392 else
01393 {
01394 pub_path_.publish(path);
01395 }
01396
01397 if (sw_wait_ > 0.0)
01398 {
01399 if (switchDetect(path))
01400 {
01401 ROS_INFO("Planned path has switchback");
01402 ros::Duration(sw_wait_).sleep();
01403 }
01404 }
01405 }
01406 }
01407 else if (!has_goal_)
01408 {
01409 publishEmptyPath();
01410 }
01411 pub_status_.publish(status_);
01412 diag_updater_.force_update();
01413 }
01414 }
01415
01416 protected:
01417 bool makePlan(const geometry_msgs::Pose& gs, const geometry_msgs::Pose& ge,
01418 nav_msgs::Path& path, bool hyst)
01419 {
01420 Astar::Vec s, e;
01421 grid_metric_converter::metric2Grid(
01422 map_info_, s[0], s[1], s[2],
01423 gs.position.x, gs.position.y, tf2::getYaw(gs.orientation));
01424 s.cycleUnsigned(map_info_.angle);
01425 grid_metric_converter::metric2Grid(
01426 map_info_, e[0], e[1], e[2],
01427 ge.position.x, ge.position.y, tf2::getYaw(ge.orientation));
01428 e.cycleUnsigned(map_info_.angle);
01429
01430 geometry_msgs::PoseStamped p;
01431 p.header = map_header_;
01432 float x, y, yaw;
01433 grid_metric_converter::grid2Metric(map_info_, e[0], e[1], e[2], x, y, yaw);
01434 p.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
01435 p.pose.position.x = x;
01436 p.pose.position.y = y;
01437 pub_end_.publish(p);
01438
01439 if (cm_[s] == 100)
01440 {
01441 if (!searchAvailablePos(s, tolerance_range_, tolerance_angle_))
01442 {
01443 ROS_WARN("Oops! You are in Rock!");
01444 status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
01445 return false;
01446 }
01447 ROS_INFO("Start moved");
01448 }
01449 const Astar::Vec s_rough(s[0], s[1], 0);
01450
01451 if (cost_estim_cache_[s_rough] == FLT_MAX)
01452 {
01453 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
01454 ROS_WARN("Goal unreachable.");
01455 if (!escaping_ && temporary_escape_)
01456 {
01457 e = s;
01458 if (searchAvailablePos(e, esc_range_, esc_angle_, 50, esc_range_ / 2))
01459 {
01460 escaping_ = true;
01461 ROS_INFO("Temporary goal (%d, %d, %d)",
01462 e[0], e[1], e[2]);
01463 float x, y, yaw;
01464 grid_metric_converter::grid2Metric(map_info_, e[0], e[1], e[2], x, y, yaw);
01465 goal_.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
01466 goal_.pose.position.x = x;
01467 goal_.pose.position.y = y;
01468
01469 updateGoal();
01470 return false;
01471 }
01472 }
01473 return false;
01474 }
01475
01476 grid_metric_converter::grid2Metric(map_info_, s[0], s[1], s[2], x, y, yaw);
01477 p.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
01478 p.pose.position.x = x;
01479 p.pose.position.y = y;
01480 pub_start_.publish(p);
01481
01482 Astar::Vec diff = s - e;
01483 diff.cycle(map_info_.angle);
01484 if (diff.sqlen() <= goal_tolerance_lin_ * goal_tolerance_lin_ &&
01485 abs(diff[2]) <= goal_tolerance_ang_)
01486 {
01487 path.poses.resize(1);
01488 path.poses[0].header = path.header;
01489 if (force_goal_orientation_)
01490 path.poses[0].pose = goal_raw_.pose;
01491 else
01492 path.poses[0].pose = ge;
01493
01494 if (escaping_)
01495 {
01496 goal_ = goal_raw_;
01497 escaping_ = false;
01498 updateGoal();
01499 ROS_INFO("Escaped");
01500 }
01501 else
01502 {
01503 status_.status = planner_cspace_msgs::PlannerStatus::FINISHING;
01504 ROS_INFO("Path plan finishing");
01505 }
01506 return true;
01507 }
01508
01509 const float range_limit = cost_estim_cache_[s_rough] - (local_range_ + range_) * ec_[0];
01510 angle_resolution_aspect_ = 2.0 / tanf(map_info_.angular_resolution);
01511
01512 const auto ts = boost::chrono::high_resolution_clock::now();
01513
01514
01515 std::list<Astar::Vec> path_grid;
01516 if (!as_.search(s, e, path_grid,
01517 std::bind(&Planner3dNode::cbCost,
01518 this, std::placeholders::_1, std::placeholders::_2,
01519 std::placeholders::_3, std::placeholders::_4, hyst),
01520 std::bind(&Planner3dNode::cbCostEstim,
01521 this, std::placeholders::_1, std::placeholders::_2),
01522 std::bind(&Planner3dNode::cbSearch,
01523 this, std::placeholders::_1,
01524 std::placeholders::_2, std::placeholders::_3),
01525 std::bind(&Planner3dNode::cbProgress,
01526 this, std::placeholders::_1),
01527 range_limit,
01528 1.0f / freq_min_,
01529 true))
01530 {
01531 ROS_WARN("Path plan failed (goal unreachable)");
01532 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
01533 if (!find_best_)
01534 return false;
01535 }
01536 const auto tnow = boost::chrono::high_resolution_clock::now();
01537 ROS_DEBUG("Path found (%0.4f sec.)",
01538 boost::chrono::duration<float>(tnow - ts).count());
01539
01540 geometry_msgs::PoseArray poses;
01541 poses.header = path.header;
01542 for (const auto& p : path_grid)
01543 {
01544 geometry_msgs::Pose pose;
01545 float x, y, yaw;
01546 grid_metric_converter::grid2Metric(map_info_, p[0], p[1], p[2], x, y, yaw);
01547 pose.position.x = x;
01548 pose.position.y = y;
01549 pose.orientation =
01550 tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
01551 poses.poses.push_back(pose);
01552 }
01553 pub_path_poses_.publish(poses);
01554
01555 const std::list<Astar::Vecf> path_interpolated =
01556 path_interpolator_.interpolate(path_grid, 0.5, local_range_);
01557 grid_metric_converter::grid2MetricPath(map_info_, path_interpolated, path);
01558
01559 if (hyst)
01560 {
01561 const std::list<Astar::Vecf> path_interpolated =
01562 path_interpolator_.interpolate(path_grid, 0.5, local_range_);
01563
01564 std::unordered_map<Astar::Vec, bool, Astar::Vec> path_points;
01565 const float max_dist = cc_.hysteresis_max_dist_ / map_info_.linear_resolution;
01566 const float expand_dist = cc_.hysteresis_expand_ / map_info_.linear_resolution;
01567 const int path_range = range_ + max_dist + expand_dist + 5;
01568 for (const Astar::Vecf& p : path_interpolated)
01569 {
01570 Astar::Vec d;
01571 for (d[0] = -path_range; d[0] <= path_range; d[0]++)
01572 {
01573 for (d[1] = -path_range; d[1] <= path_range; d[1]++)
01574 {
01575 Astar::Vec point = p + d;
01576 point.cycleUnsigned(map_info_.angle);
01577 if ((unsigned int)point[0] >= (unsigned int)map_info_.width ||
01578 (unsigned int)point[1] >= (unsigned int)map_info_.height)
01579 continue;
01580 path_points[point] = true;
01581 }
01582 }
01583 }
01584
01585 cm_hyst_.clear(100);
01586 const auto ts = boost::chrono::high_resolution_clock::now();
01587 for (auto& ps : path_points)
01588 {
01589 const Astar::Vec& p = ps.first;
01590 float d_min = FLT_MAX;
01591 auto it_prev = path_interpolated.begin();
01592 for (auto it = path_interpolated.begin(); it != path_interpolated.end(); it++)
01593 {
01594 if (it != it_prev)
01595 {
01596 const float d =
01597 CyclicVecFloat<3, 2>(p).distLinestrip2d(*it_prev, *it);
01598 if (d < d_min)
01599 d_min = d;
01600 }
01601 it_prev = it;
01602 }
01603 d_min = std::max(expand_dist, std::min(expand_dist + max_dist, d_min));
01604 cm_hyst_[p] = lroundf((d_min - expand_dist) * 100.0 / max_dist);
01605 }
01606 const auto tnow = boost::chrono::high_resolution_clock::now();
01607 ROS_DEBUG("Hysteresis map generated (%0.4f sec.)",
01608 boost::chrono::duration<float>(tnow - ts).count());
01609 publishCostmap();
01610 }
01611
01612 return true;
01613 }
01614 std::vector<Astar::Vec>& cbSearch(
01615 const Astar::Vec& p,
01616 const Astar::Vec& s, const Astar::Vec& e)
01617 {
01618 const Astar::Vec ds = s - p;
01619
01620 if (ds.sqlen() < local_range_ * local_range_)
01621 {
01622 rough_ = false;
01623 euclid_cost_coef_ = ec_;
01624 return search_list_;
01625 }
01626 rough_ = true;
01627 euclid_cost_coef_ = ec_rough_;
01628 return search_list_rough_;
01629 }
01630 bool cbProgress(const std::list<Astar::Vec>& path_grid)
01631 {
01632 publishEmptyPath();
01633 ROS_WARN("Search timed out");
01634 return true;
01635 }
01636
01637 float cbCostEstim(const Astar::Vec& s, const Astar::Vec& e)
01638 {
01639 Astar::Vec s2(s[0], s[1], 0);
01640 float cost = cost_estim_cache_[s2];
01641 if (cost == FLT_MAX)
01642 return FLT_MAX;
01643 if (!rough_)
01644 {
01645 if (s2[2] > static_cast<int>(map_info_.angle) / 2)
01646 s2[2] -= map_info_.angle;
01647 cost += ec_rough_[2] * fabs(s[2]);
01648 }
01649 return cost;
01650 }
01651 bool switchDetect(const nav_msgs::Path& path)
01652 {
01653 geometry_msgs::Pose p_prev;
01654 bool first(true);
01655 bool dir_set(false);
01656 bool dir_prev(false);
01657 for (const auto& p : path.poses)
01658 {
01659 if (!first)
01660 {
01661 const float len = hypotf(
01662 p.pose.position.y - p_prev.position.y,
01663 p.pose.position.x - p_prev.position.x);
01664 if (len > 0.001)
01665 {
01666 const float yaw = tf2::getYaw(p.pose.orientation);
01667 const float vel_yaw = atan2f(
01668 p.pose.position.y - p_prev.position.y,
01669 p.pose.position.x - p_prev.position.x);
01670 const bool dir = (cosf(yaw) * cosf(vel_yaw) + sinf(yaw) * sinf(vel_yaw) < 0);
01671
01672 if (dir_set && (dir_prev ^ dir))
01673 {
01674 return true;
01675 }
01676 dir_prev = dir;
01677 dir_set = true;
01678 }
01679 }
01680 first = false;
01681 p_prev = p.pose;
01682 }
01683 return false;
01684 }
01685 float cbCost(const Astar::Vec& s, Astar::Vec& e,
01686 const Astar::Vec& v_goal,
01687 const Astar::Vec& v_start,
01688 const bool hyst)
01689 {
01690 Astar::Vec d_raw = e - s;
01691 d_raw.cycle(map_info_.angle);
01692 const Astar::Vec d = d_raw;
01693 float cost = euclidCost(d);
01694
01695 if (d[0] == 0 && d[1] == 0)
01696 {
01697
01698 int sum = 0;
01699 const int dir = d[2] < 0 ? -1 : 1;
01700 Astar::Vec pos = s;
01701 for (int i = 0; i < abs(d[2]); i++)
01702 {
01703 pos[2] += dir;
01704 if (pos[2] < 0)
01705 pos[2] += map_info_.angle;
01706 else if (pos[2] >= static_cast<int>(map_info_.angle))
01707 pos[2] -= map_info_.angle;
01708 const auto c = cm_[pos];
01709 if (c > 99)
01710 return -1;
01711 sum += c;
01712 }
01713
01714 const float cost =
01715 sum * map_info_.angular_resolution * ec_[2] / ec_[0] +
01716 sum * map_info_.angular_resolution * cc_.weight_costmap_turn_ / 100.0;
01717
01718 return cc_.in_place_turn_ + cost;
01719 }
01720
01721 Astar::Vec d2;
01722 d2[0] = d[0] + range_;
01723 d2[1] = d[1] + range_;
01724 d2[2] = e[2];
01725
01726 const Astar::Vecf motion = rot_cache_.getMotion(s[2], d2);
01727 const Astar::Vecf motion_grid = motion * resolution_;
01728
01729 if (lroundf(motion_grid[0]) == 0 && lroundf(motion_grid[1]) != 0)
01730 {
01731
01732 return -1;
01733 }
01734
01735 if (fabs(motion[2]) >= 2.0 * M_PI / 4.0)
01736 {
01737
01738
01739 return -1;
01740 }
01741
01742 const float dist = motion.len();
01743
01744 if (motion[0] < 0)
01745 {
01746
01747 cost *= 1.0 + cc_.weight_backward_;
01748 }
01749
01750 if (d[2] == 0)
01751 {
01752 if (lroundf(motion_grid[0]) == 0)
01753 return -1;
01754 const float aspect = motion[0] / motion[1];
01755 if (fabs(aspect) < angle_resolution_aspect_)
01756 return -1;
01757
01758
01759 int sum = 0, sum_hyst = 0;
01760 Astar::Vec d_index(d[0], d[1], e[2]);
01761 d_index.cycleUnsigned(map_info_.angle);
01762
01763 const auto cache_page = motion_cache_.find(s[2], d_index);
01764 if (cache_page == motion_cache_.end(s[2]))
01765 return -1;
01766 const int num = cache_page->second.getMotion().size();
01767 for (const auto& pos_diff : cache_page->second.getMotion())
01768 {
01769 const Astar::Vec pos(
01770 s[0] + pos_diff[0], s[1] + pos_diff[1], pos_diff[2]);
01771 const auto c = cm_[pos];
01772 if (c > 99)
01773 return -1;
01774 sum += c;
01775
01776 if (hyst)
01777 sum_hyst += cm_hyst_[pos];
01778 }
01779 const float distf = cache_page->second.getDistance();
01780 cost += sum * map_info_.linear_resolution * distf * cc_.weight_costmap_ / (100.0 * num);
01781 cost += sum_hyst * map_info_.linear_resolution * distf * cc_.weight_hysteresis_ / (100.0 * num);
01782 }
01783 else
01784 {
01785
01786 if (motion[0] * motion[1] * motion[2] < 0)
01787 return -1;
01788
01789 if (d.sqlen() < 3 * 3)
01790 return -1;
01791
01792 const std::pair<float, float>& radiuses = rot_cache_.getRadiuses(s[2], d2);
01793 const float r1 = radiuses.first;
01794 const float r2 = radiuses.second;
01795
01796
01797 if (fabs(r1 - r2) >= map_info_.linear_resolution * 1.5)
01798 {
01799
01800 return -1;
01801 }
01802
01803 const float curv_radius = (r1 + r2) / 2;
01804 if (std::abs(curv_radius) < min_curve_raduis_)
01805 return -1;
01806
01807 if (fabs(max_vel_ / r1) > max_ang_vel_)
01808 {
01809 const float vel = fabs(curv_radius) * max_ang_vel_;
01810
01811
01812 cost += dist * fabs(vel / max_vel_) * cc_.weight_decel_;
01813 }
01814
01815 {
01816 int sum = 0, sum_hyst = 0;
01817 Astar::Vec d_index(d[0], d[1], e[2]);
01818 d_index.cycleUnsigned(map_info_.angle);
01819
01820 const auto cache_page = motion_cache_.find(s[2], d_index);
01821 if (cache_page == motion_cache_.end(s[2]))
01822 return -1;
01823 const int num = cache_page->second.getMotion().size();
01824 for (const auto& pos_diff : cache_page->second.getMotion())
01825 {
01826 const Astar::Vec pos(
01827 s[0] + pos_diff[0], s[1] + pos_diff[1], pos_diff[2]);
01828 const auto c = cm_[pos];
01829 if (c > 99)
01830 return -1;
01831 sum += c;
01832
01833 if (hyst)
01834 sum_hyst += cm_hyst_[pos];
01835 }
01836 const float distf = cache_page->second.getDistance();
01837 cost += sum * map_info_.linear_resolution * distf * cc_.weight_costmap_ / (100.0 * num);
01838 cost += sum * map_info_.angular_resolution * abs(d[2]) * cc_.weight_costmap_turn_ / (100.0 * num);
01839 cost += sum_hyst * map_info_.linear_resolution * distf * cc_.weight_hysteresis_ / (100.0 * num);
01840 }
01841 }
01842
01843 return cost;
01844 }
01845
01846 void diagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper& stat)
01847 {
01848 switch (status_.error)
01849 {
01850 case planner_cspace_msgs::PlannerStatus::GOING_WELL:
01851 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Going well.");
01852 break;
01853 case planner_cspace_msgs::PlannerStatus::IN_ROCK:
01854 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The robot is in rock.");
01855 break;
01856 case planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND:
01857 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Path not found.");
01858 break;
01859 case planner_cspace_msgs::PlannerStatus::DATA_MISSING:
01860 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Required data is missing.");
01861 break;
01862 case planner_cspace_msgs::PlannerStatus::INTERNAL_ERROR:
01863 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Planner internal error.");
01864 break;
01865 default:
01866 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Unknown error.");
01867 break;
01868 }
01869 stat.addf("status", "%u", status_.status);
01870 stat.addf("error", "%u", status_.error);
01871 }
01872 };
01873
01874 int main(int argc, char* argv[])
01875 {
01876 ros::init(argc, argv, "planner_3d");
01877
01878 Planner3dNode jy;
01879 jy.spin();
01880
01881 return 0;
01882 }