planner_3d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2023, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Make DEBUG flag taking precedence over NDEBUG flag for boundary test
31 #ifdef DEBUG
32 #ifdef NDEBUG
33 #undef NDEBUG
34 #endif
35 #endif
36 
37 #include <algorithm>
38 #include <cmath>
39 #include <limits>
40 #include <list>
41 #include <memory>
42 #include <string>
43 #include <unordered_map>
44 #include <utility>
45 #include <vector>
46 
47 #include <omp.h>
48 
49 #include <ros/ros.h>
50 
51 #include <costmap_cspace_msgs/CSpace3D.h>
52 #include <costmap_cspace_msgs/CSpace3DUpdate.h>
53 #include <diagnostic_updater/diagnostic_updater.h>
54 #include <dynamic_reconfigure/server.h>
55 #include <geometry_msgs/PoseArray.h>
56 #include <nav_msgs/GetPlan.h>
57 #include <nav_msgs/OccupancyGrid.h>
58 #include <nav_msgs/Path.h>
59 #include <neonavigation_metrics_msgs/Metrics.h>
60 #include <neonavigation_metrics_msgs/helper.h>
61 #include <planner_cspace_msgs/PlannerStatus.h>
62 #include <sensor_msgs/PointCloud.h>
63 #include <std_srvs/Empty.h>
64 #include <trajectory_tracker_msgs/PathWithVelocity.h>
65 #include <trajectory_tracker_msgs/converter.h>
66 
67 #include <ros/console.h>
68 #include <tf2/utils.h>
69 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
70 #include <tf2_ros/transform_listener.h>
71 
72 #include <actionlib/server/simple_action_server.h>
73 #include <move_base_msgs/MoveBaseAction.h>
74 #include <planner_cspace_msgs/MoveWithToleranceAction.h>
75 
76 #include <neonavigation_common/compatibility.h>
77 
78 #include <planner_cspace/Planner3DConfig.h>
79 #include <planner_cspace/bbf.h>
80 #include <planner_cspace/grid_astar.h>
81 #include <planner_cspace/jump_detector.h>
82 #include <planner_cspace/planner_3d/costmap_bbf.h>
83 #include <planner_cspace/planner_3d/distance_map.h>
84 #include <planner_cspace/planner_3d/grid_astar_model.h>
85 #include <planner_cspace/planner_3d/grid_metric_converter.h>
86 #include <planner_cspace/planner_3d/motion_cache.h>
87 #include <planner_cspace/planner_3d/rotation_cache.h>
88 #include <planner_cspace/planner_3d/start_pose_predictor.h>
89 
90 namespace planner_cspace
91 {
92 namespace planner_3d
93 {
94 class Planner3dNode
95 {
96 public:
97  using Astar = GridAstar<3, 2>;
98 
99 protected:
100  using Planner3DActionServer = actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>;
101  using Planner3DTolerantActionServer = actionlib::SimpleActionServer<planner_cspace_msgs::MoveWithToleranceAction>;
102 
103  ros::NodeHandle nh_;
104  ros::NodeHandle pnh_;
105  ros::Subscriber sub_map_;
106  ros::Subscriber sub_map_update_;
107  ros::Subscriber sub_goal_;
108  ros::Subscriber sub_temporary_escape_trigger_;
109  ros::Publisher pub_path_;
110  ros::Publisher pub_path_velocity_;
111  ros::Publisher pub_path_poses_;
112  ros::Publisher pub_preserved_path_poses_;
113  ros::Publisher pub_hysteresis_map_;
114  ros::Publisher pub_distance_map_;
115  ros::Publisher pub_remembered_map_;
116  ros::Publisher pub_start_;
117  ros::Publisher pub_end_;
118  ros::Publisher pub_goal_;
119  ros::Publisher pub_status_;
120  ros::Publisher pub_metrics_;
121  ros::ServiceServer srs_forget_;
122  ros::ServiceServer srs_make_plan_;
123 
124  std::shared_ptr<Planner3DActionServer> act_;
125  std::shared_ptr<Planner3DTolerantActionServer> act_tolerant_;
126  planner_cspace_msgs::MoveWithToleranceGoalConstPtr goal_tolerant_;
127  tf2_ros::Buffer tfbuf_;
128  tf2_ros::TransformListener tfl_;
129  dynamic_reconfigure::Server<Planner3DConfig> parameter_server_;
130 
131  Astar as_;
132  Astar::Gridmap<char, 0x40> cm_;
133  Astar::Gridmap<char, 0x80> cm_rough_;
134  Astar::Gridmap<char, 0x40> cm_base_;
135  Astar::Gridmap<char, 0x80> cm_rough_base_;
136  Astar::Gridmap<char, 0x80> cm_hyst_;
137  Astar::Gridmap<char, 0x80> cm_updates_;
138  Astar::Gridmap<char, 0x80> cm_local_esc_;
139  CostmapBBF::Ptr bbf_costmap_;
140  DistanceMap cost_estim_cache_;
141  DistanceMap cost_estim_cache_static_;
142  DistanceMap arrivable_map_;
143 
144  GridAstarModel3D::Ptr model_;
145 
146  costmap_cspace_msgs::MapMetaData3D map_info_;
147  costmap_cspace_msgs::CSpace3DUpdate::ConstPtr map_update_retained_;
148  std_msgs::Header map_header_;
149  float freq_;
150  float freq_min_;
151  float search_timeout_abort_;
152  float search_range_;
153  bool antialias_start_;
154  int range_;
155  int local_range_;
156  double local_range_f_;
157  double longcut_range_f_;
158  int esc_range_;
159  int esc_range_min_;
160  int esc_angle_;
161  double esc_range_f_;
162  double esc_range_min_ratio_;
163  int tolerance_range_;
164  int tolerance_angle_;
165  double tolerance_range_f_;
166  double tolerance_angle_f_;
167  double path_interpolation_resolution_;
168  double grid_enumeration_resolution_;
169  int unknown_cost_;
170  bool overwrite_cost_;
171  int relocation_acceptable_cost_;
172  bool has_map_;
173  bool has_goal_;
174  bool has_start_;
175  bool has_hysteresis_map_;
176  std::vector<Astar::Vec> hyst_updated_cells_;
177  bool cost_estim_cache_created_;
178  bool remember_updates_;
179  bool fast_map_update_;
180  std::vector<Astar::Vec> search_list_;
181  std::vector<Astar::Vec> search_list_rough_;
182  double hist_ignore_range_f_;
183  int hist_ignore_range_;
184  double hist_ignore_range_max_f_;
185  int hist_ignore_range_max_;
186  bool temporary_escape_;
187  float remember_hit_odds_;
188  float remember_miss_odds_;
189  bool use_path_with_velocity_;
190  bool retain_last_error_status_;
191  int num_cost_estim_task_;
192  bool keep_a_part_of_previous_path_;
193  bool trigger_plan_by_costmap_update_;
194  bool enable_crowd_mode_;
195 
196  JumpDetector jump_;
197  std::string robot_frame_;
198 
199  int max_retry_num_;
200 
201  int num_task_;
202 
203  // Cost weights
204  CostCoeff cc_;
205 
206  geometry_msgs::PoseStamped start_;
207  geometry_msgs::PoseStamped goal_;
208  geometry_msgs::PoseStamped goal_raw_;
209  geometry_msgs::PoseStamped goal_original_;
210  Astar::Vecf ec_;
211  double goal_tolerance_lin_f_;
212  double goal_tolerance_ang_f_;
213  double goal_tolerance_ang_finish_;
214  int goal_tolerance_lin_;
215  int goal_tolerance_ang_;
216  double temporary_escape_tolerance_lin_f_;
217  double temporary_escape_tolerance_ang_f_;
218  int temporary_escape_tolerance_lin_;
219  int temporary_escape_tolerance_ang_;
220 
221  planner_cspace_msgs::PlannerStatus status_;
222  neonavigation_metrics_msgs::Metrics metrics_;
223 
224  bool find_best_;
225  float sw_wait_;
226  geometry_msgs::PoseStamped sw_pos_;
227  bool is_path_switchback_;
228 
229  bool rough_;
230 
231  bool force_goal_orientation_;
232 
233  bool escaping_;
234 
235  int cnt_stuck_;
236  bool is_start_occupied_;
237 
238  diagnostic_updater::Updater diag_updater_;
239  ros::Duration costmap_watchdog_;
240  ros::Time last_costmap_;
241 
242  int prev_map_update_x_min_;
243  int prev_map_update_x_max_;
244  int prev_map_update_y_min_;
245  int prev_map_update_y_max_;
246  nav_msgs::Path previous_path_;
247  StartPosePredictor start_pose_predictor_;
248  ros::Timer no_map_update_timer_;
249 
250  bool cbForget(std_srvs::EmptyRequest& req,
251  std_srvs::EmptyResponse& res)
252  {
253  ROS_WARN("Forgetting remembered costmap.");
254  if (has_map_)
255  bbf_costmap_->clear();
256 
257  return true;
258  }
259  void cbTemporaryEscape(const std_msgs::Empty::ConstPtr&)
260  {
261  if (!has_map_)
262  {
263  // metric2Grid requires map_info_
264  return;
265  }
266  updateTemporaryEscapeGoal(metric2Grid(start_.pose), false);
267  }
268  enum class DiscretePoseStatus
269  {
270  OK,
271  RELOCATED,
272  IN_ROCK,
273  OUT_OF_MAP,
274  };
275  template <class T>
276  DiscretePoseStatus relocateDiscretePoseIfNeededImpl(const T& cm,
277  const int tolerance_range,
278  const int tolerance_angle,
279  Astar::Vec& pose_discrete) const
280  {
281  if (!cm.validate(pose_discrete, range_))
282  {
283  return DiscretePoseStatus::OUT_OF_MAP;
284  }
285  if (cm[pose_discrete] == 100)
286  {
287  if (searchAvailablePos(cm, pose_discrete, tolerance_range, tolerance_angle))
288  return DiscretePoseStatus::RELOCATED;
289  else
290  return DiscretePoseStatus::IN_ROCK;
291  }
292  return DiscretePoseStatus::OK;
293  }
294  DiscretePoseStatus relocateDiscretePoseIfNeeded(Astar::Vec& pose_discrete,
295  const int tolerance_range,
296  const int tolerance_angle,
297  bool use_cm_rough = false) const
298  {
299  if (use_cm_rough)
300  {
301  pose_discrete[2] = 0;
302  return relocateDiscretePoseIfNeededImpl(cm_rough_, tolerance_range, tolerance_angle, pose_discrete);
303  }
304  else
305  {
306  return relocateDiscretePoseIfNeededImpl(cm_, tolerance_range, tolerance_angle, pose_discrete);
307  }
308  }
309 
310  Astar::Vec metric2Grid(const geometry_msgs::Pose& pose) const
311  {
312  Astar::Vec grid;
313  grid_metric_converter::metric2Grid(
314  map_info_, grid[0], grid[1], grid[2],
315  pose.position.x, pose.position.y,
316  tf2::getYaw(pose.orientation));
317  grid.cycleUnsigned(map_info_.angle);
318  return grid;
319  }
320  geometry_msgs::Pose grid2MetricPose(const Astar::Vec& grid) const
321  {
322  float x, y, yaw;
323  grid_metric_converter::grid2Metric(map_info_, grid[0], grid[1], grid[2], x, y, yaw);
324  geometry_msgs::Pose pose;
325  pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
326  pose.position.x = x;
327  pose.position.y = y;
328  return pose;
329  }
330  geometry_msgs::Point32 grid2MetricPoint(const Astar::Vec& grid) const
331  {
332  float x, y, yaw;
333  grid_metric_converter::grid2Metric(map_info_, grid[0], grid[1], grid[2], x, y, yaw);
334  geometry_msgs::Point32 point;
335  point.x = x;
336  point.y = y;
337  return point;
338  }
339 
340  bool cbMakePlan(nav_msgs::GetPlan::Request& req,
341  nav_msgs::GetPlan::Response& res)
342  {
343  if (!has_map_)
344  {
345  ROS_ERROR("make_plan service is called without map.");
346  return false;
347  }
348 
349  if (req.start.header.frame_id != map_header_.frame_id ||
350  req.goal.header.frame_id != map_header_.frame_id)
351  {
352  ROS_ERROR("Start [%s] and Goal [%s] poses must be in the map frame [%s].",
353  req.start.header.frame_id.c_str(),
354  req.goal.header.frame_id.c_str(),
355  map_header_.frame_id.c_str());
356  return false;
357  }
358 
359  Astar::Vec s = metric2Grid(req.start.pose);
360  Astar::Vec e = metric2Grid(req.goal.pose);
361  ROS_INFO("Path plan from (%d, %d) to (%d, %d)", s[0], s[1], e[0], e[1]);
362 
363  const int tolerance_range = std::lround(req.tolerance / map_info_.linear_resolution);
364  const DiscretePoseStatus start_status = relocateDiscretePoseIfNeeded(s, tolerance_range, tolerance_angle_, true);
365  const DiscretePoseStatus goal_status = relocateDiscretePoseIfNeeded(e, tolerance_range, tolerance_angle_, true);
366  switch (start_status)
367  {
368  case DiscretePoseStatus::OUT_OF_MAP:
369  ROS_ERROR("Given start is not on the map.");
370  return false;
371  case DiscretePoseStatus::IN_ROCK:
372  ROS_ERROR("Given start is in Rock.");
373  return false;
374  case DiscretePoseStatus::RELOCATED:
375  ROS_INFO("Given start is moved (%d, %d)", s[0], s[1]);
376  break;
377  default:
378  break;
379  }
380  switch (goal_status)
381  {
382  case DiscretePoseStatus::OUT_OF_MAP:
383  ROS_ERROR("Given goal is not on the map.");
384  return false;
385  case DiscretePoseStatus::IN_ROCK:
386  ROS_ERROR("Given goal is in Rock.");
387  return false;
388  case DiscretePoseStatus::RELOCATED:
389  ROS_INFO("Given goal is moved (%d, %d)", e[0], e[1]);
390  break;
391  default:
392  break;
393  }
394 
395  const auto cb_progress = [](const std::list<Astar::Vec>& /* path_grid */, const SearchStats& /* stats */)
396  {
397  return true;
398  };
399 
400  const auto ts = boost::chrono::high_resolution_clock::now();
401 
402  GridAstarModel2D::Ptr model_2d(new GridAstarModel2D(model_));
403 
404  std::list<Astar::Vec> path_grid;
405  std::vector<GridAstarModel3D::VecWithCost> starts;
406  starts.emplace_back(s);
407  if (!as_.search(
408  starts, e, path_grid,
409  model_2d,
410  cb_progress,
411  0, 1.0f / freq_min_, find_best_))
412  {
413  ROS_WARN("Path plan failed (goal unreachable)");
414  return false;
415  }
416  const auto tnow = boost::chrono::high_resolution_clock::now();
417  ROS_INFO("Path found (%0.4f sec.)",
418  boost::chrono::duration<float>(tnow - ts).count());
419 
420  nav_msgs::Path path;
421  path.header = map_header_;
422  path.header.stamp = ros::Time::now();
423 
424  const std::list<Astar::Vecf> path_interpolated = model_->interpolatePath(path_grid);
425  grid_metric_converter::appendGridPath2MetricPath(map_info_, path_interpolated, path);
426 
427  res.plan.header = map_header_;
428  res.plan.poses.resize(path.poses.size());
429  for (size_t i = 0; i < path.poses.size(); ++i)
430  {
431  res.plan.poses[i] = path.poses[i];
432  }
433  return true;
434  }
435 
436  void cbGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
437  {
438  if (act_->isActive() || act_tolerant_->isActive())
439  {
440  ROS_ERROR("Setting new goal is ignored since planner_3d is proceeding the action.");
441  return;
442  }
443  setGoal(*msg);
444  }
445  void cbPreempt()
446  {
447  ROS_WARN("Preempting the current goal.");
448  if (act_->isActive())
449  act_->setPreempted(move_base_msgs::MoveBaseResult(), "Preempted.");
450 
451  if (act_tolerant_->isActive())
452  act_tolerant_->setPreempted(planner_cspace_msgs::MoveWithToleranceResult(), "Preempted.");
453 
454  has_goal_ = false;
455  status_.status = planner_cspace_msgs::PlannerStatus::DONE;
456  }
457 
458  bool setGoal(const geometry_msgs::PoseStamped& msg)
459  {
460  if (msg.header.frame_id != map_header_.frame_id)
461  {
462  ROS_ERROR("Goal [%s] pose must be in the map frame [%s].",
463  msg.header.frame_id.c_str(), map_header_.frame_id.c_str());
464  return false;
465  }
466 
467  goal_original_ = goal_raw_ = goal_ = msg;
468 
469  const double len2 =
470  goal_.pose.orientation.x * goal_.pose.orientation.x +
471  goal_.pose.orientation.y * goal_.pose.orientation.y +
472  goal_.pose.orientation.z * goal_.pose.orientation.z +
473  goal_.pose.orientation.w * goal_.pose.orientation.w;
474  if (std::abs(len2 - 1.0) < 0.1)
475  {
476  escaping_ = false;
477  has_goal_ = true;
478  cnt_stuck_ = 0;
479  if (!createCostEstimCache())
480  {
481  has_goal_ = false;
482  return false;
483  }
484  status_.status = planner_cspace_msgs::PlannerStatus::DOING;
485  status_.header.stamp = ros::Time::now();
486  pub_status_.publish(status_);
487  diag_updater_.update();
488  }
489  else
490  {
491  has_goal_ = false;
492  if (act_->isActive())
493  act_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal cleared.");
494  if (act_tolerant_->isActive())
495  act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(), "Goal cleared.");
496  }
497  return true;
498  }
499 
500  template <class T>
501  bool searchAvailablePos(const T& cm, Astar::Vec& s, const int xy_range, const int angle_range,
502  int cost_acceptable = -1, const int min_xy_range = 0) const
503  {
504  if (cost_acceptable == -1)
505  {
506  cost_acceptable = relocation_acceptable_cost_;
507  }
508  ROS_DEBUG("%d, %d (%d,%d,%d)", xy_range, angle_range, s[0], s[1], s[2]);
509 
510  float range_min = std::numeric_limits<float>::max();
511  Astar::Vec s_out;
512  Astar::Vec d;
513  for (d[2] = -angle_range; d[2] <= angle_range; d[2]++)
514  {
515  for (d[0] = -xy_range; d[0] <= xy_range; d[0]++)
516  {
517  for (d[1] = -xy_range; d[1] <= xy_range; d[1]++)
518  {
519  if (d[0] == 0 && d[1] == 0 && d[2] == 0)
520  continue;
521  if (d.sqlen() > xy_range * xy_range)
522  continue;
523  if (d.sqlen() < min_xy_range * min_xy_range)
524  continue;
525 
526  Astar::Vec s2 = s + d;
527  if ((unsigned int)s2[0] >= (unsigned int)map_info_.width ||
528  (unsigned int)s2[1] >= (unsigned int)map_info_.height)
529  continue;
530  s2.cycleUnsigned(map_info_.angle);
531  if (!cm_.validate(s2, range_))
532  continue;
533 
534  if (cm_[s2] >= cost_acceptable)
535  continue;
536  const auto cost = model_->euclidCost(d);
537  if (cost < range_min)
538  {
539  range_min = cost;
540  s_out = s2;
541  }
542  }
543  }
544  }
545 
546  if (range_min == std::numeric_limits<float>::max())
547  {
548  if (cost_acceptable != 100)
549  {
550  return searchAvailablePos(cm, s, xy_range, angle_range, 100);
551  }
552  return false;
553  }
554  s = s_out;
555  s.cycleUnsigned(map_info_.angle);
556  ROS_DEBUG(" (%d,%d,%d)", s[0], s[1], s[2]);
557  return true;
558  }
559 
560  bool createCostEstimCache(const bool goal_changed = true)
561  {
562  if (!has_goal_)
563  return true;
564 
565  if (!has_map_ || !has_start_)
566  {
567  ROS_ERROR("Goal received, however map/goal/start are not ready. (%d/%d/%d)",
568  static_cast<int>(has_map_), static_cast<int>(has_goal_), static_cast<int>(has_start_));
569  return true;
570  }
571 
572  cost_estim_cache_created_ = false;
573  is_start_occupied_ = false;
574 
575  Astar::Vec s = metric2Grid(start_.pose);
576  Astar::Vec e = metric2Grid(goal_raw_.pose);
577  if (goal_changed)
578  {
579  ROS_INFO("New goal received. Metric: (%.3f, %.3f, %.3f), Grid: (%d, %d, %d)",
580  goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation),
581  e[0], e[1], e[2]);
582  clearHysteresis();
583  has_hysteresis_map_ = false;
584  }
585  const DiscretePoseStatus start_pose_status = relocateDiscretePoseIfNeeded(s, tolerance_range_, tolerance_angle_);
586  const DiscretePoseStatus goal_pose_status = relocateDiscretePoseIfNeeded(e, tolerance_range_, tolerance_angle_);
587  switch (start_pose_status)
588  {
589  case DiscretePoseStatus::OUT_OF_MAP:
590  ROS_ERROR("You are on the edge of the world.");
591  return false;
592  case DiscretePoseStatus::IN_ROCK:
593  ROS_WARN("Oops! You are in Rock!");
594  ++cnt_stuck_;
595  is_start_occupied_ = true;
596  return true;
597  default:
598  break;
599  }
600  switch (goal_pose_status)
601  {
602  case DiscretePoseStatus::OUT_OF_MAP:
603  ROS_ERROR("Given goal is not on the map.");
604  return false;
605  case DiscretePoseStatus::IN_ROCK:
606  if (escaping_)
607  {
608  ROS_WARN("Oops! Temporary goal is in Rock! Reverting the temporary goal.");
609  goal_raw_ = goal_ = goal_original_;
610  escaping_ = false;
611  return true;
612  }
613  ROS_WARN("Oops! Goal is in Rock!");
614  ++cnt_stuck_;
615  if (!escaping_ && temporary_escape_ && enable_crowd_mode_)
616  {
617  updateTemporaryEscapeGoal(s);
618  }
619  return true;
620  case DiscretePoseStatus::RELOCATED:
621  goal_.pose = grid2MetricPose(e);
622  ROS_INFO("Goal moved. Metric: (%.3f, %.3f, %.3f), Grid: (%d, %d, %d)",
623  goal_.pose.position.x, goal_.pose.position.y, tf2::getYaw(goal_.pose.orientation),
624  e[0], e[1], e[2]);
625  break;
626  default:
627  const Astar::Vec e_prev = metric2Grid(goal_.pose);
628  if (e[0] != e_prev[0] || e[1] != e_prev[1] || e[2] != e_prev[2])
629  {
630  ROS_INFO("Goal reverted. Metric: (%.3f, %.3f, %.3f), Grid: (%d, %d, %d)",
631  goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation),
632  e[0], e[1], e[2]);
633  }
634  goal_ = goal_raw_;
635  break;
636  }
637 
638  {
639  const auto ts = boost::chrono::high_resolution_clock::now();
640  cost_estim_cache_.create(s, e);
641  const auto tnow = boost::chrono::high_resolution_clock::now();
642  const float dur = boost::chrono::duration<float>(tnow - ts).count();
643  ROS_DEBUG("Cost estimation cache generated (%0.4f sec.)", dur);
644 
645  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
646  "distance_map_init_dur", dur, "second"));
647  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
648  "distance_map_update_dur", 0.0, "second"));
649  }
650 
651  publishDebug();
652 
653  cost_estim_cache_created_ = true;
654 
655  return true;
656  }
657  void clearHysteresis()
658  {
659  for (const Astar::Vec& p : hyst_updated_cells_)
660  {
661  cm_hyst_[p] = 100;
662  }
663  hyst_updated_cells_.clear();
664  }
665  void publishDebug()
666  {
667  if (pub_distance_map_.getNumSubscribers() > 0)
668  {
669  sensor_msgs::PointCloud distance_map;
670  distance_map.header = map_header_;
671  distance_map.header.stamp = ros::Time::now();
672  distance_map.channels.resize(1);
673  distance_map.channels[0].name = "distance";
674  distance_map.points.reserve(1024);
675  distance_map.channels[0].values.reserve(1024);
676  const float k_dist = map_info_.linear_resolution * cc_.max_vel_;
677  for (Astar::Vec p(0, 0, 0); p[1] < cost_estim_cache_.size()[1]; p[1]++)
678  {
679  for (p[0] = 0; p[0] < cost_estim_cache_.size()[0]; p[0]++)
680  {
681  p[2] = 0;
682  const float cost = cost_estim_cache_[p];
683  if (cost == std::numeric_limits<float>::max())
684  continue;
685 
686  geometry_msgs::Point32 point = grid2MetricPoint(p);
687  point.z = cost / 500;
688  distance_map.points.push_back(point);
689  distance_map.channels[0].values.push_back(cost * k_dist);
690  }
691  }
692  pub_distance_map_.publish(distance_map);
693  }
694 
695  if (pub_hysteresis_map_.getNumSubscribers() > 0)
696  {
697  nav_msgs::OccupancyGrid hysteresis_map;
698  hysteresis_map.header.frame_id = map_header_.frame_id;
699  hysteresis_map.info.resolution = map_info_.linear_resolution;
700  hysteresis_map.info.width = map_info_.width;
701  hysteresis_map.info.height = map_info_.height;
702  hysteresis_map.info.origin = map_info_.origin;
703  hysteresis_map.data.resize(map_info_.width * map_info_.height, 100);
704 
705  for (Astar::Vec p(0, 0, 0); p[1] < cost_estim_cache_.size()[1]; p[1]++)
706  {
707  for (p[0] = 0; p[0] < cost_estim_cache_.size()[0]; p[0]++)
708  {
709  if (cost_estim_cache_[p] == std::numeric_limits<float>::max())
710  continue;
711 
712  char cost = 100;
713  for (Astar::Vec p2 = p; p2[2] < static_cast<int>(map_info_.angle); ++p2[2])
714  {
715  cost = std::min(cm_hyst_[p2], cost);
716  }
717  hysteresis_map.data[p[0] + p[1] * map_info_.width] = cost;
718  }
719  }
720  pub_hysteresis_map_.publish(hysteresis_map);
721  }
722  }
723  void publishRememberedMap()
724  {
725  if (pub_remembered_map_.getNumSubscribers() > 0 && remember_updates_)
726  {
727  nav_msgs::OccupancyGrid remembered_map;
728  remembered_map.header.frame_id = map_header_.frame_id;
729  remembered_map.info.resolution = map_info_.linear_resolution;
730  remembered_map.info.width = map_info_.width;
731  remembered_map.info.height = map_info_.height;
732  remembered_map.info.origin = map_info_.origin;
733  remembered_map.data.resize(map_info_.width * map_info_.height);
734 
735  const auto generate_pointcloud = [this, &remembered_map](const Astar::Vec& p, bbf::BinaryBayesFilter& bbf)
736  {
737  remembered_map.data[p[0] + p[1] * map_info_.width] =
738  (bbf.getProbability() - bbf::MIN_PROBABILITY) * 100 / (bbf::MAX_PROBABILITY - bbf::MIN_PROBABILITY);
739  };
740  bbf_costmap_->forEach(generate_pointcloud);
741  pub_remembered_map_.publish(remembered_map);
742  }
743  }
744  void publishEmptyPath()
745  {
746  nav_msgs::Path path;
747  path.header.frame_id = robot_frame_;
748  path.header.stamp = ros::Time::now();
749  publishPath(path);
750  }
751  void publishFinishPath()
752  {
753  nav_msgs::Path path;
754  path.header.frame_id = map_header_.frame_id;
755  path.header.stamp = ros::Time::now();
756  // Specify single pose to control only orientation
757  path.poses.resize(1);
758  path.poses[0].header = path.header;
759  if (force_goal_orientation_)
760  path.poses[0].pose = goal_raw_.pose;
761  else
762  path.poses[0].pose = goal_.pose;
763  publishPath(path);
764  }
765  void publishPath(const nav_msgs::Path& path)
766  {
767  if (use_path_with_velocity_)
768  {
769  pub_path_velocity_.publish(
770  trajectory_tracker_msgs::toPathWithVelocity(
771  path, std::numeric_limits<double>::quiet_NaN()));
772  }
773  else
774  {
775  pub_path_.publish(path);
776  }
777  previous_path_ = path;
778  }
779 
780  void applyCostmapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr& msg)
781  {
782  const auto ts_cm_init_start = boost::chrono::high_resolution_clock::now();
783  const ros::Time now = ros::Time::now();
784 
785  const int map_update_x_min = static_cast<int>(msg->x);
786  const int map_update_x_max = std::max(static_cast<int>(msg->x + msg->width) - 1, 0);
787  const int map_update_y_min = static_cast<int>(msg->y);
788  const int map_update_y_max = std::max(static_cast<int>(msg->y + msg->height) - 1, 0);
789 
790  if (static_cast<size_t>(map_update_x_max) >= map_info_.width ||
791  static_cast<size_t>(map_update_y_max) >= map_info_.height ||
792  msg->angle > map_info_.angle)
793  {
794  ROS_WARN(
795  "Map update out of range (update range: %dx%dx%d, map range: %dx%dx%d)",
796  map_update_x_max, map_update_y_max, msg->angle,
797  map_info_.width, map_info_.height, map_info_.angle);
798  map_update_retained_ = msg;
799  return;
800  }
801 
802  last_costmap_ = now;
803 
804  cm_.copy_partially(
805  cm_base_,
806  Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
807  Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, static_cast<int>(map_info_.angle) - 1));
808  cm_rough_.copy_partially(
809  cm_rough_base_,
810  Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
811  Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 0));
812  cm_updates_.clear_partially(
813  -1,
814  Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
815  Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 0));
816 
817  // Should search 1px around the region to
818  // update the costmap even if the edge of the local map is obstacle
819  const int search_range_x_min = std::max(
820  0,
821  std::min(prev_map_update_x_min_, map_update_x_min) - 1);
822  const int search_range_x_max = std::min(
823  static_cast<int>(map_info_.width - 1),
824  std::max(prev_map_update_x_max_, map_update_x_max) + 1);
825  const int search_range_y_min = std::max(
826  0,
827  std::min(prev_map_update_y_min_, map_update_y_min) - 1);
828  const int search_range_y_max = std::min(
829  static_cast<int>(map_info_.height - 1),
830  std::max(prev_map_update_y_max_, map_update_y_max) + 1);
831 
832  prev_map_update_x_min_ = map_update_x_min;
833  prev_map_update_x_max_ = map_update_x_max;
834  prev_map_update_y_min_ = map_update_y_min;
835  prev_map_update_y_max_ = map_update_y_max;
836 
837  bool clear_hysteresis(false);
838 
839  {
840  const Astar::Vec gp(
841  static_cast<int>(msg->x), static_cast<int>(msg->y), static_cast<int>(msg->yaw));
842  const Astar::Vec gp_rough(gp[0], gp[1], 0);
843  for (Astar::Vec p(0, 0, 0); p[0] < static_cast<int>(msg->width); p[0]++)
844  {
845  for (p[1] = 0; p[1] < static_cast<int>(msg->height); p[1]++)
846  {
847  int cost_min = 100;
848  for (p[2] = 0; p[2] < static_cast<int>(msg->angle); p[2]++)
849  {
850  const size_t addr = ((p[2] * msg->height) + p[1]) * msg->width + p[0];
851  const char c = msg->data[addr];
852  if (c < cost_min)
853  cost_min = c;
854  if (c == 100 && !clear_hysteresis && cm_hyst_[gp + p] == 0)
855  clear_hysteresis = true;
856  }
857  p[2] = 0;
858  cm_updates_[gp_rough + p] = cost_min;
859  if (cost_min > cm_rough_[gp_rough + p])
860  cm_rough_[gp_rough + p] = cost_min;
861 
862  for (p[2] = 0; p[2] < static_cast<int>(msg->angle); p[2]++)
863  {
864  const size_t addr = ((p[2] * msg->height) + p[1]) * msg->width + p[0];
865  const char c = msg->data[addr];
866  if (overwrite_cost_)
867  {
868  if (c >= 0)
869  cm_[gp + p] = c;
870  }
871  else
872  {
873  if (cm_[gp + p] < c)
874  cm_[gp + p] = c;
875  }
876  }
877  }
878  }
879  }
880  map_update_retained_ = nullptr;
881  const auto ts_cm_init_end = boost::chrono::high_resolution_clock::now();
882  const float ts_cm_init_dur = boost::chrono::duration<float>(ts_cm_init_end - ts_cm_init_start).count();
883  ROS_DEBUG("Costmaps updated (%.4f)", ts_cm_init_dur);
884  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
885  "costmap_dur",
886  ts_cm_init_dur,
887  "second"));
888 
889  if (clear_hysteresis && has_hysteresis_map_)
890  {
891  ROS_INFO("The previous path collides to the obstacle. Clearing hysteresis map.");
892  clearHysteresis();
893  has_hysteresis_map_ = false;
894  }
895 
896  if (!has_start_)
897  return;
898 
899  const Astar::Vec s = metric2Grid(start_.pose);
900 
901  if (remember_updates_)
902  {
903  const auto ts = boost::chrono::high_resolution_clock::now();
904  bbf_costmap_->remember(
905  &cm_updates_, s,
906  remember_hit_odds_, remember_miss_odds_,
907  hist_ignore_range_, hist_ignore_range_max_);
908  publishRememberedMap();
909  bbf_costmap_->updateCostmap();
910  const auto tnow = boost::chrono::high_resolution_clock::now();
911  const float dur = boost::chrono::duration<float>(tnow - ts).count();
912  ROS_DEBUG("Remembered costmap updated (%0.4f sec.)", dur);
913  }
914  if (!has_goal_)
915  return;
916 
917  if (!fast_map_update_)
918  {
919  createCostEstimCache(false);
920  return;
921  }
922 
923  const Astar::Vec e = metric2Grid(goal_.pose);
924 
925  if (cm_[e] == 100)
926  {
927  createCostEstimCache(false);
928  return;
929  }
930 
931  {
932  const auto ts = boost::chrono::high_resolution_clock::now();
933  cost_estim_cache_.update(
934  s, e,
935  DistanceMap::Rect(
936  Astar::Vec(search_range_x_min, search_range_y_min, 0),
937  Astar::Vec(search_range_x_max, search_range_y_max, 0)));
938  const auto tnow = boost::chrono::high_resolution_clock::now();
939  const float dur = boost::chrono::duration<float>(tnow - ts).count();
940  ROS_DEBUG("Cost estimation cache updated (%0.4f sec.)", dur);
941  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
942  "distance_map_update_dur", dur, "second"));
943  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
944  "distance_map_init_dur", 0.0, "second"));
945  }
946 
947  const DistanceMap::DebugData dm_debug = cost_estim_cache_.getDebugData();
948  if (dm_debug.has_negative_cost)
949  {
950  ROS_WARN("Negative cost value is detected. Limited to zero.");
951  }
952  ROS_DEBUG("Cost estimation cache search queue initial size: %lu, capacity: %lu",
953  dm_debug.search_queue_size, dm_debug.search_queue_cap);
954  publishDebug();
955  return;
956  }
957 
958  void cbNoMapUpdateTimer(const ros::TimerEvent& e)
959  {
960  planPath(e.current_real);
961  no_map_update_timer_ =
962  nh_.createTimer(costmap_watchdog_, &Planner3dNode::cbNoMapUpdateTimer, this, true);
963  }
964  void cbMapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr& msg)
965  {
966  if (!has_map_)
967  return;
968  ROS_DEBUG("Map updated");
969  if (trigger_plan_by_costmap_update_)
970  {
971  no_map_update_timer_.stop();
972  updateStart();
973  applyCostmapUpdate(msg);
974  planPath(last_costmap_);
975  if (costmap_watchdog_ > ros::Duration(0))
976  {
977  no_map_update_timer_ =
978  nh_.createTimer(costmap_watchdog_, &Planner3dNode::cbNoMapUpdateTimer, this, true);
979  }
980  }
981  else
982  {
983  applyCostmapUpdate(msg);
984  }
985  }
986  void cbMap(const costmap_cspace_msgs::CSpace3D::ConstPtr& msg)
987  {
988  ROS_INFO("Map received");
989  ROS_INFO(" linear_resolution %0.2f x (%dx%d) px", msg->info.linear_resolution,
990  msg->info.width, msg->info.height);
991  ROS_INFO(" angular_resolution %0.2f x %d px", msg->info.angular_resolution,
992  msg->info.angle);
993  ROS_INFO(" origin %0.3f m, %0.3f m, %0.3f rad",
994  msg->info.origin.position.x,
995  msg->info.origin.position.y,
996  tf2::getYaw(msg->info.origin.orientation));
997 
998  // Stop robot motion until next planning step
999  publishEmptyPath();
1000 
1001  ec_ = Astar::Vecf(
1002  1.0f / cc_.max_vel_,
1003  1.0f / cc_.max_vel_,
1004  1.0f * cc_.weight_ang_vel_ / cc_.max_ang_vel_);
1005 
1006  if (map_info_.linear_resolution != msg->info.linear_resolution ||
1007  map_info_.angular_resolution != msg->info.angular_resolution)
1008  {
1009  map_info_ = msg->info;
1010  resetGridAstarModel(true);
1011  ROS_DEBUG("Search model updated");
1012  }
1013  else
1014  {
1015  map_info_ = msg->info;
1016  }
1017  map_header_ = msg->header;
1018  jump_.setMapFrame(map_header_.frame_id);
1019 
1020  const int size[3] =
1021  {
1022  static_cast<int>(map_info_.width),
1023  static_cast<int>(map_info_.height),
1024  static_cast<int>(map_info_.angle),
1025  };
1026 
1027  const Astar::Vec size3d(size[0], size[1], size[2]);
1028  const Astar::Vec size2d(size[0], size[1], 1);
1029 
1030  as_.reset(size3d);
1031  cm_.reset(size3d);
1032  cm_hyst_.reset(size3d);
1033 
1034  const DistanceMap::Params dmp =
1035  {
1036  .euclid_cost = ec_,
1037  .range = range_,
1038  .local_range = local_range_,
1039  .longcut_range = static_cast<int>(std::lround(longcut_range_f_ / map_info_.linear_resolution)),
1040  .size = size2d,
1041  .resolution = map_info_.linear_resolution,
1042  };
1043  cost_estim_cache_.init(model_, dmp);
1044  if (enable_crowd_mode_)
1045  {
1046  cost_estim_cache_static_.init(model_, dmp);
1047  }
1048  cm_rough_.reset(size2d);
1049  cm_updates_.reset(size2d);
1050  bbf_costmap_->reset(size2d);
1051 
1052  Astar::Vec p;
1053  for (p[0] = 0; p[0] < static_cast<int>(map_info_.width); p[0]++)
1054  {
1055  for (p[1] = 0; p[1] < static_cast<int>(map_info_.height); p[1]++)
1056  {
1057  int cost_min = 100;
1058  for (p[2] = 0; p[2] < static_cast<int>(map_info_.angle); p[2]++)
1059  {
1060  const size_t addr = ((p[2] * size[1]) + p[1]) * size[0] + p[0];
1061  char c = msg->data[addr];
1062  if (c < 0)
1063  c = unknown_cost_;
1064  cm_[p] = c;
1065  if (c < cost_min)
1066  cost_min = c;
1067  }
1068  p[2] = 0;
1069  cm_rough_[p] = cost_min;
1070  }
1071  }
1072  ROS_DEBUG("Map copied");
1073 
1074  cm_hyst_.clear(100);
1075  hyst_updated_cells_.clear();
1076  has_hysteresis_map_ = false;
1077 
1078  cm_updates_.clear(0);
1079 
1080  has_map_ = true;
1081 
1082  cm_rough_base_ = cm_rough_;
1083  cm_base_ = cm_;
1084  bbf_costmap_->clear();
1085 
1086  prev_map_update_x_min_ = map_info_.width;
1087  prev_map_update_x_max_ = 0;
1088  prev_map_update_y_min_ = map_info_.height;
1089  prev_map_update_y_max_ = 0;
1090 
1091  createCostEstimCache();
1092 
1093  if (map_update_retained_ && map_update_retained_->header.stamp >= msg->header.stamp)
1094  {
1095  ROS_INFO("Applying retained map update");
1096  cbMapUpdate(map_update_retained_);
1097  }
1098  map_update_retained_ = nullptr;
1099  }
1100  void cbAction()
1101  {
1102  if (act_tolerant_->isActive())
1103  {
1104  ROS_ERROR("Setting new goal is ignored since planner_3d is proceeding by tolerant_move action.");
1105  return;
1106  }
1107 
1108  move_base_msgs::MoveBaseGoalConstPtr goal = act_->acceptNewGoal();
1109  if (!setGoal(goal->target_pose))
1110  act_->setAborted(move_base_msgs::MoveBaseResult(), "Given goal is invalid.");
1111  }
1112 
1113  void cbTolerantAction()
1114  {
1115  if (act_->isActive())
1116  {
1117  ROS_ERROR("Setting new goal is ignored since planner_3d is proceeding by move_base action.");
1118  return;
1119  }
1120 
1121  goal_tolerant_ = act_tolerant_->acceptNewGoal();
1122  if (!setGoal(goal_tolerant_->target_pose))
1123  act_tolerant_->setAborted(planner_cspace_msgs::MoveWithToleranceResult(), "Given goal is invalid.");
1124  }
1125 
1126  void updateStart()
1127  {
1128  geometry_msgs::PoseStamped start;
1129  start.header.frame_id = robot_frame_;
1130  start.header.stamp = ros::Time(0);
1131  start.pose.orientation.x = 0.0;
1132  start.pose.orientation.y = 0.0;
1133  start.pose.orientation.z = 0.0;
1134  start.pose.orientation.w = 1.0;
1135  start.pose.position.x = 0;
1136  start.pose.position.y = 0;
1137  start.pose.position.z = 0;
1138  try
1139  {
1140  geometry_msgs::TransformStamped trans =
1141  tfbuf_.lookupTransform(map_header_.frame_id, robot_frame_, ros::Time(), ros::Duration(0.1));
1142  tf2::doTransform(start, start, trans);
1143  }
1144  catch (tf2::TransformException& e)
1145  {
1146  has_start_ = false;
1147  return;
1148  }
1149  start_ = start;
1150  has_start_ = true;
1151  }
1152 
1153 public:
1154  Planner3dNode()
1155  : nh_()
1156  , pnh_("~")
1157  , tfl_(tfbuf_)
1158  , parameter_server_(pnh_)
1159  , bbf_costmap_(new CostmapBBFImpl())
1160  , cost_estim_cache_(cm_rough_, bbf_costmap_)
1161  , cost_estim_cache_static_(cm_rough_base_, CostmapBBF::Ptr(new CostmapBBFNoOp()))
1162  , arrivable_map_(cm_local_esc_, CostmapBBF::Ptr(new CostmapBBFNoOp()))
1163  , jump_(tfbuf_)
1164  {
1165  neonavigation_common::compat::checkCompatMode();
1166  sub_map_ = neonavigation_common::compat::subscribe(
1167  nh_, "costmap",
1168  pnh_, "costmap", 1, &Planner3dNode::cbMap, this);
1169  sub_map_update_ = neonavigation_common::compat::subscribe(
1170  nh_, "costmap_update",
1171  pnh_, "costmap_update", 1, &Planner3dNode::cbMapUpdate, this);
1172  sub_goal_ = neonavigation_common::compat::subscribe(
1173  nh_, "move_base_simple/goal",
1174  pnh_, "goal", 1, &Planner3dNode::cbGoal, this);
1175  sub_temporary_escape_trigger_ = pnh_.subscribe(
1176  "temporary_escape", 1, &Planner3dNode::cbTemporaryEscape, this);
1177  pub_start_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_start", 1, true);
1178  pub_end_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_end", 1, true);
1179  pub_goal_ = pnh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1, true);
1180  pub_status_ = pnh_.advertise<planner_cspace_msgs::PlannerStatus>("status", 1, true);
1181  pub_metrics_ = pnh_.advertise<neonavigation_metrics_msgs::Metrics>("metrics", 1, false);
1182  srs_forget_ = neonavigation_common::compat::advertiseService(
1183  nh_, "forget_planning_cost",
1184  pnh_, "forget", &Planner3dNode::cbForget, this);
1185  srs_make_plan_ = pnh_.advertiseService("make_plan", &Planner3dNode::cbMakePlan, this);
1186 
1187  // Debug outputs
1188  pub_distance_map_ = pnh_.advertise<sensor_msgs::PointCloud>("distance_map", 1, true);
1189  pub_hysteresis_map_ = pnh_.advertise<nav_msgs::OccupancyGrid>("hysteresis_map", 1, true);
1190  pub_remembered_map_ = pnh_.advertise<nav_msgs::OccupancyGrid>("remembered_map", 1, true);
1191 
1192  act_.reset(new Planner3DActionServer(ros::NodeHandle(), "move_base", false));
1193  act_->registerGoalCallback(boost::bind(&Planner3dNode::cbAction, this));
1194  act_->registerPreemptCallback(boost::bind(&Planner3dNode::cbPreempt, this));
1195 
1196  act_tolerant_.reset(new Planner3DTolerantActionServer(ros::NodeHandle(), "tolerant_move", false));
1197  act_tolerant_->registerGoalCallback(boost::bind(&Planner3dNode::cbTolerantAction, this));
1198  act_tolerant_->registerPreemptCallback(boost::bind(&Planner3dNode::cbPreempt, this));
1199  goal_tolerant_ = nullptr;
1200 
1201  pnh_.param("use_path_with_velocity", use_path_with_velocity_, false);
1202  if (use_path_with_velocity_)
1203  {
1204  pub_path_velocity_ = nh_.advertise<trajectory_tracker_msgs::PathWithVelocity>(
1205  "path_velocity", 1, true);
1206  }
1207  else
1208  {
1209  pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
1210  nh_, "path",
1211  pnh_, "path", 1, true);
1212  }
1213  pub_path_poses_ = pnh_.advertise<geometry_msgs::PoseArray>("path_poses", 1, true);
1214  pub_preserved_path_poses_ = pnh_.advertise<nav_msgs::Path>("preserved_path_poses", 1, true);
1215 
1216  pnh_.param("freq", freq_, 4.0f);
1217  pnh_.param("freq_min", freq_min_, 2.0f);
1218  pnh_.param("search_timeout_abort", search_timeout_abort_, 30.0f);
1219  pnh_.param("search_range", search_range_, 0.4f);
1220  pnh_.param("antialias_start", antialias_start_, false);
1221 
1222  double costmap_watchdog;
1223  pnh_.param("costmap_watchdog", costmap_watchdog, 0.0);
1224  costmap_watchdog_ = ros::Duration(costmap_watchdog);
1225 
1226  pnh_.param("max_vel", cc_.max_vel_, 0.3f);
1227  pnh_.param("max_ang_vel", cc_.max_ang_vel_, 0.6f);
1228  pnh_.param("min_curve_radius", cc_.min_curve_radius_, 0.1f);
1229 
1230  pnh_.param("weight_decel", cc_.weight_decel_, 50.0f);
1231  pnh_.param("weight_backward", cc_.weight_backward_, 0.9f);
1232  pnh_.param("weight_ang_vel", cc_.weight_ang_vel_, 1.0f);
1233  pnh_.param("weight_costmap", cc_.weight_costmap_, 50.0f);
1234  pnh_.param("weight_costmap_turn", cc_.weight_costmap_turn_, 0.0f);
1235  pnh_.param("weight_remembered", cc_.weight_remembered_, 1000.0f);
1236  pnh_.param("cost_in_place_turn", cc_.in_place_turn_, 30.0f);
1237  pnh_.param("hysteresis_max_dist", cc_.hysteresis_max_dist_, 0.1f);
1238  pnh_.param("hysteresis_expand", cc_.hysteresis_expand_, 0.1f);
1239  pnh_.param("weight_hysteresis", cc_.weight_hysteresis_, 5.0f);
1240 
1241  pnh_.param("goal_tolerance_lin", goal_tolerance_lin_f_, 0.05);
1242  pnh_.param("goal_tolerance_ang", goal_tolerance_ang_f_, 0.1);
1243  pnh_.param("goal_tolerance_ang_finish", goal_tolerance_ang_finish_, 0.05);
1244  pnh_.param("temporary_escape_tolerance_lin", temporary_escape_tolerance_lin_f_, 0.1);
1245  pnh_.param("temporary_escape_tolerance_ang", temporary_escape_tolerance_ang_f_, 1.57);
1246 
1247  pnh_.param("unknown_cost", unknown_cost_, 100);
1248  pnh_.param("overwrite_cost", overwrite_cost_, false);
1249  pnh_.param("relocation_acceptable_cost", relocation_acceptable_cost_, 50);
1250 
1251  pnh_.param("hist_ignore_range", hist_ignore_range_f_, 0.6);
1252  pnh_.param("hist_ignore_range_max", hist_ignore_range_max_f_, 1.25);
1253  pnh_.param("remember_updates", remember_updates_, false);
1254  double remember_hit_prob, remember_miss_prob;
1255  pnh_.param("remember_hit_prob", remember_hit_prob, 0.6);
1256  pnh_.param("remember_miss_prob", remember_miss_prob, 0.3);
1257  remember_hit_odds_ = bbf::probabilityToOdds(remember_hit_prob);
1258  remember_miss_odds_ = bbf::probabilityToOdds(remember_miss_prob);
1259 
1260  pnh_.param("local_range", local_range_f_, 2.5);
1261  pnh_.param("longcut_range", longcut_range_f_, 0.0);
1262  pnh_.param("esc_range", esc_range_f_, 0.25);
1263  pnh_.param("esc_range_min_ratio", esc_range_min_ratio_, 0.5);
1264  pnh_.param("tolerance_range", tolerance_range_f_, 0.25);
1265  pnh_.param("tolerance_angle", tolerance_angle_f_, 0.0);
1266  pnh_.param("path_interpolation_resolution", path_interpolation_resolution_, 0.5);
1267  pnh_.param("grid_enumeration_resolution", grid_enumeration_resolution_, 0.1);
1268  if (path_interpolation_resolution_ < grid_enumeration_resolution_)
1269  {
1270  ROS_ERROR("path_interpolation_resolution must be greater than or equal to grid_enumeration_resolution.");
1271  path_interpolation_resolution_ = grid_enumeration_resolution_;
1272  }
1273 
1274  pnh_.param("sw_wait", sw_wait_, 2.0f);
1275  pnh_.param("find_best", find_best_, true);
1276 
1277  pnh_.param("robot_frame", robot_frame_, std::string("base_link"));
1278 
1279  double pos_jump, yaw_jump;
1280  std::string jump_detect_frame;
1281  pnh_.param("pos_jump", pos_jump, 1.0);
1282  pnh_.param("yaw_jump", yaw_jump, 1.5);
1283  pnh_.param("jump_detect_frame", jump_detect_frame, std::string("base_link"));
1284  jump_.setBaseFrame(jump_detect_frame);
1285  jump_.setThresholds(pos_jump, yaw_jump);
1286 
1287  pnh_.param("force_goal_orientation", force_goal_orientation_, true);
1288 
1289  pnh_.param("temporary_escape", temporary_escape_, true);
1290  pnh_.param("enable_crowd_mode", enable_crowd_mode_, false);
1291 
1292  pnh_.param("fast_map_update", fast_map_update_, false);
1293  if (fast_map_update_)
1294  {
1295  ROS_WARN("planner_3d: Experimental fast_map_update is enabled. ");
1296  }
1297  if (pnh_.hasParam("debug_mode"))
1298  {
1299  ROS_ERROR(
1300  "planner_3d: ~/debug_mode parameter and ~/debug topic are deprecated. "
1301  "Use ~/distance_map, ~/hysteresis_map, and ~/remembered_map topics instead.");
1302  }
1303 
1304  bool print_planning_duration;
1305  pnh_.param("print_planning_duration", print_planning_duration, false);
1306  if (print_planning_duration)
1307  {
1308  if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
1309  {
1310  ros::console::notifyLoggerLevelsChanged();
1311  }
1312  }
1313 
1314  pnh_.param("max_retry_num", max_retry_num_, -1);
1315 
1316  int queue_size_limit;
1317  pnh_.param("queue_size_limit", queue_size_limit, 0);
1318  as_.setQueueSizeLimit(queue_size_limit);
1319 
1320  int num_threads;
1321  pnh_.param("num_threads", num_threads, 1);
1322  omp_set_num_threads(num_threads);
1323 
1324  int num_task;
1325  pnh_.param("num_search_task", num_task, num_threads * 16);
1326  as_.setSearchTaskNum(num_task);
1327  pnh_.param("num_cost_estim_task", num_cost_estim_task_, num_threads * 16);
1328  cost_estim_cache_.setParams(cc_, num_cost_estim_task_);
1329  cost_estim_cache_static_.setParams(cc_, num_cost_estim_task_);
1330 
1331  pnh_.param("retain_last_error_status", retain_last_error_status_, true);
1332  status_.status = planner_cspace_msgs::PlannerStatus::DONE;
1333 
1334  has_map_ = false;
1335  has_goal_ = false;
1336  has_start_ = false;
1337  cost_estim_cache_created_ = false;
1338 
1339  escaping_ = false;
1340  cnt_stuck_ = 0;
1341  is_path_switchback_ = false;
1342 
1343  diag_updater_.setHardwareID("none");
1344  diag_updater_.add("Path Planner Status", this, &Planner3dNode::diagnoseStatus);
1345 
1346  act_->start();
1347  act_tolerant_->start();
1348 
1349  // cbParameter() with the inital parameters will be called within setCallback().
1350  parameter_server_.setCallback(boost::bind(&Planner3dNode::cbParameter, this, _1, _2));
1351  }
1352 
1353  void resetGridAstarModel(const bool force_reset)
1354  {
1355  const int previous_range = range_;
1356  range_ = static_cast<int>(search_range_ / map_info_.linear_resolution);
1357  hist_ignore_range_ = std::lround(hist_ignore_range_f_ / map_info_.linear_resolution);
1358  hist_ignore_range_max_ = std::lround(hist_ignore_range_max_f_ / map_info_.linear_resolution);
1359  local_range_ = std::lround(local_range_f_ / map_info_.linear_resolution);
1360  esc_range_ = std::lround(esc_range_f_ / map_info_.linear_resolution);
1361  esc_range_min_ = std::lround(esc_range_f_ * esc_range_min_ratio_ / map_info_.linear_resolution);
1362  esc_angle_ = map_info_.angle / 8;
1363  tolerance_range_ = std::lround(tolerance_range_f_ / map_info_.linear_resolution);
1364  tolerance_angle_ = std::lround(tolerance_angle_f_ / map_info_.angular_resolution);
1365  goal_tolerance_lin_ = std::lround(goal_tolerance_lin_f_ / map_info_.linear_resolution);
1366  goal_tolerance_ang_ = std::lround(goal_tolerance_ang_f_ / map_info_.angular_resolution);
1367  temporary_escape_tolerance_lin_ = std::lround(temporary_escape_tolerance_lin_f_ / map_info_.linear_resolution);
1368  temporary_escape_tolerance_ang_ = std::lround(temporary_escape_tolerance_ang_f_ / map_info_.angular_resolution);
1369  cc_.angle_resolution_aspect_ = 2.0 / tanf(map_info_.angular_resolution);
1370 
1371  const bool reset_required = force_reset || (previous_range != range_);
1372  if (reset_required)
1373  {
1374  model_.reset(
1375  new GridAstarModel3D(
1376  map_info_,
1377  ec_,
1378  local_range_,
1379  cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_,
1380  cc_, range_, path_interpolation_resolution_, grid_enumeration_resolution_));
1381  }
1382  else
1383  {
1384  model_->updateCostParameters(ec_, cc_, local_range_);
1385  }
1386  }
1387 
1388  void cbParameter(const Planner3DConfig& config, const uint32_t /* level */)
1389  {
1390  freq_ = config.freq;
1391  freq_min_ = config.freq_min;
1392  search_timeout_abort_ = config.search_timeout_abort;
1393  search_range_ = config.search_range;
1394  antialias_start_ = config.antialias_start;
1395  costmap_watchdog_ = ros::Duration(config.costmap_watchdog);
1396 
1397  cc_.max_vel_ = config.max_vel;
1398  cc_.max_ang_vel_ = config.max_ang_vel;
1399  cc_.min_curve_radius_ = config.min_curve_radius;
1400  cc_.weight_decel_ = config.weight_decel;
1401  cc_.weight_backward_ = config.weight_backward;
1402  cc_.weight_ang_vel_ = config.weight_ang_vel;
1403  cc_.weight_costmap_ = config.weight_costmap;
1404  cc_.weight_costmap_turn_ = config.weight_costmap_turn;
1405  cc_.weight_remembered_ = config.weight_remembered;
1406  cc_.in_place_turn_ = config.cost_in_place_turn;
1407  cc_.hysteresis_max_dist_ = config.hysteresis_max_dist;
1408  cc_.hysteresis_expand_ = config.hysteresis_expand;
1409  cc_.weight_hysteresis_ = config.weight_hysteresis;
1410  cc_.weight_costmap_turn_heuristics_ = config.weight_costmap_turn_heuristics;
1411  cc_.turn_penalty_cost_threshold_ = config.turn_penalty_cost_threshold;
1412 
1413  goal_tolerance_lin_f_ = config.goal_tolerance_lin;
1414  goal_tolerance_ang_f_ = config.goal_tolerance_ang;
1415  goal_tolerance_ang_finish_ = config.goal_tolerance_ang_finish;
1416  temporary_escape_tolerance_lin_f_ = config.temporary_escape_tolerance_lin;
1417  temporary_escape_tolerance_ang_f_ = config.temporary_escape_tolerance_ang;
1418 
1419  overwrite_cost_ = config.overwrite_cost;
1420  relocation_acceptable_cost_ = config.relocation_acceptable_cost;
1421  hist_ignore_range_f_ = config.hist_ignore_range;
1422  hist_ignore_range_max_f_ = config.hist_ignore_range_max;
1423 
1424  remember_updates_ = config.remember_updates;
1425  remember_hit_odds_ = bbf::probabilityToOdds(config.remember_hit_prob);
1426  remember_miss_odds_ = bbf::probabilityToOdds(config.remember_miss_prob);
1427 
1428  local_range_f_ = config.local_range;
1429  longcut_range_f_ = config.longcut_range;
1430  esc_range_f_ = config.esc_range;
1431  esc_range_min_ratio_ = config.esc_range_min_ratio;
1432  tolerance_range_f_ = config.tolerance_range;
1433  tolerance_angle_f_ = config.tolerance_angle;
1434  find_best_ = config.find_best;
1435  force_goal_orientation_ = config.force_goal_orientation;
1436  temporary_escape_ = config.temporary_escape;
1437  fast_map_update_ = config.fast_map_update;
1438  max_retry_num_ = config.max_retry_num;
1439  sw_wait_ = config.sw_wait;
1440 
1441  cost_estim_cache_.setParams(cc_, num_cost_estim_task_);
1442  cost_estim_cache_static_.setParams(cc_, num_cost_estim_task_);
1443  ec_ = Astar::Vecf(
1444  1.0f / cc_.max_vel_,
1445  1.0f / cc_.max_vel_,
1446  1.0f * cc_.weight_ang_vel_ / cc_.max_ang_vel_);
1447 
1448  if (map_info_.linear_resolution != 0.0 && map_info_.angular_resolution != 0.0)
1449  {
1450  resetGridAstarModel(false);
1451  const Astar::Vec size2d(static_cast<int>(map_info_.width), static_cast<int>(map_info_.height), 1);
1452  const DistanceMap::Params dmp =
1453  {
1454  .euclid_cost = ec_,
1455  .range = range_,
1456  .local_range = local_range_,
1457  .longcut_range = static_cast<int>(std::lround(longcut_range_f_ / map_info_.linear_resolution)),
1458  .size = size2d,
1459  .resolution = map_info_.linear_resolution,
1460  };
1461  cost_estim_cache_.init(model_, dmp);
1462  if (enable_crowd_mode_)
1463  {
1464  cost_estim_cache_static_.init(model_, dmp);
1465  }
1466  }
1467 
1468  keep_a_part_of_previous_path_ = config.keep_a_part_of_previous_path;
1469  StartPosePredictor::Config start_pose_predictor_config;
1470  start_pose_predictor_config.lin_vel_ = config.max_vel;
1471  start_pose_predictor_config.ang_vel_ = config.max_ang_vel;
1472  start_pose_predictor_config.dist_stop_ = config.dist_stop_to_previous_path;
1473  start_pose_predictor_config.prediction_sec_ = 1.0 / freq_;
1474  start_pose_predictor_config.switch_back_prediction_sec_ = config.sw_wait;
1475  if (keep_a_part_of_previous_path_)
1476  {
1477  // No need to wait additional times
1478  sw_wait_ = 1.0 / freq_;
1479  }
1480  else
1481  {
1482  sw_wait_ = config.sw_wait;
1483  }
1484  start_pose_predictor_.setConfig(start_pose_predictor_config);
1485  trigger_plan_by_costmap_update_ = config.trigger_plan_by_costmap_update;
1486  no_map_update_timer_.stop();
1487  }
1488 
1489  void waitUntil(const ros::Time& next_replan_time)
1490  {
1491  while (ros::ok())
1492  {
1493  const ros::Time prev_map_update_stamp = last_costmap_;
1494  ros::spinOnce();
1495  const bool costmap_updated = last_costmap_ != prev_map_update_stamp;
1496 
1497  if (has_map_)
1498  {
1499  updateStart();
1500 
1501  if (jump_.detectJump())
1502  {
1503  bbf_costmap_->clear();
1504  // Robot pose jumped.
1505  return;
1506  }
1507 
1508  if (costmap_updated && previous_path_.poses.size() > 1)
1509  {
1510  for (const auto& path_pose : previous_path_.poses)
1511  {
1512  if (cm_[metric2Grid(path_pose.pose)] == 100)
1513  {
1514  // Obstacle on the path.
1515  return;
1516  }
1517  }
1518  }
1519 
1520  if (is_path_switchback_)
1521  {
1522  const float len = std::hypot(
1523  start_.pose.position.y - sw_pos_.pose.position.y,
1524  start_.pose.position.x - sw_pos_.pose.position.x);
1525  const float yaw = tf2::getYaw(start_.pose.orientation);
1526  const float sw_yaw = tf2::getYaw(sw_pos_.pose.orientation);
1527  float yaw_diff = yaw - sw_yaw;
1528  yaw_diff = std::atan2(std::sin(yaw_diff), std::cos(yaw_diff));
1529  if (len < goal_tolerance_lin_f_ && std::fabs(yaw_diff) < goal_tolerance_ang_f_)
1530  {
1531  // robot has arrived at the switchback point
1532  is_path_switchback_ = false;
1533  return;
1534  }
1535  }
1536  }
1537  if (ros::Time::now() > next_replan_time)
1538  {
1539  return;
1540  }
1541  ros::Duration(0.01).sleep();
1542  }
1543  }
1544 
1545  void planPath(const ros::Time& now)
1546  {
1547  if (has_map_ && !cost_estim_cache_created_ && has_goal_)
1548  {
1549  createCostEstimCache();
1550  }
1551  bool has_costmap(false);
1552  if (costmap_watchdog_ > ros::Duration(0))
1553  {
1554  const ros::Duration costmap_delay = now - last_costmap_;
1555  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
1556  "costmap_delay",
1557  costmap_delay.toSec(),
1558  "second"));
1559  if (costmap_delay > costmap_watchdog_)
1560  {
1561  ROS_WARN_THROTTLE(1.0,
1562  "Navigation is stopping since the costmap is too old (costmap: %0.3f)",
1563  last_costmap_.toSec());
1564  status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING;
1565  publishEmptyPath();
1566  }
1567  else
1568  {
1569  has_costmap = true;
1570  }
1571  }
1572  else
1573  {
1574  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
1575  "costmap_delay",
1576  -1.0,
1577  "second"));
1578  has_costmap = true;
1579  }
1580 
1581  if (has_map_ && has_goal_ && has_start_ && has_costmap)
1582  {
1583  if (act_->isActive())
1584  {
1585  move_base_msgs::MoveBaseFeedback feedback;
1586  feedback.base_position = start_;
1587  act_->publishFeedback(feedback);
1588  }
1589 
1590  if (act_tolerant_->isActive())
1591  {
1592  planner_cspace_msgs::MoveWithToleranceFeedback feedback;
1593  feedback.base_position = start_;
1594  act_tolerant_->publishFeedback(feedback);
1595  }
1596 
1597  is_path_switchback_ = false;
1598  if (status_.status == planner_cspace_msgs::PlannerStatus::FINISHING)
1599  {
1600  const float yaw_s = tf2::getYaw(start_.pose.orientation);
1601  float yaw_g = tf2::getYaw(goal_.pose.orientation);
1602  if (force_goal_orientation_)
1603  yaw_g = tf2::getYaw(goal_raw_.pose.orientation);
1604 
1605  float yaw_diff = yaw_s - yaw_g;
1606  if (yaw_diff > M_PI)
1607  yaw_diff -= M_PI * 2.0;
1608  else if (yaw_diff < -M_PI)
1609  yaw_diff += M_PI * 2.0;
1610  if (std::abs(yaw_diff) <
1611  (act_tolerant_->isActive() ? goal_tolerant_->goal_tolerance_ang_finish : goal_tolerance_ang_finish_))
1612  {
1613  status_.status = planner_cspace_msgs::PlannerStatus::DONE;
1614  has_goal_ = false;
1615  // Don't publish empty path here in order a path follower
1616  // to minimize the error to the desired final pose
1617  ROS_INFO("Path plan finished");
1618 
1619  if (act_->isActive())
1620  act_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
1621  if (act_tolerant_->isActive())
1622  act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(), "Goal reached.");
1623  }
1624  else
1625  {
1626  publishFinishPath();
1627  }
1628  }
1629  else
1630  {
1631  bool skip_path_planning = false;
1632  if (escaping_)
1633  {
1634  status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
1635  }
1636  else if (max_retry_num_ != -1 && cnt_stuck_ > max_retry_num_)
1637  {
1638  status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
1639  status_.status = planner_cspace_msgs::PlannerStatus::DONE;
1640  has_goal_ = false;
1641 
1642  publishEmptyPath();
1643  ROS_ERROR("Exceeded max_retry_num:%d", max_retry_num_);
1644 
1645  if (act_->isActive())
1646  act_->setAborted(
1647  move_base_msgs::MoveBaseResult(), "Goal is in Rock");
1648  if (act_tolerant_->isActive())
1649  act_tolerant_->setAborted(
1650  planner_cspace_msgs::MoveWithToleranceResult(), "Goal is in Rock");
1651  return;
1652  }
1653  else if (!cost_estim_cache_created_)
1654  {
1655  skip_path_planning = true;
1656  if (is_start_occupied_)
1657  {
1658  status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
1659  }
1660  else
1661  {
1662  status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
1663  }
1664  }
1665  else
1666  {
1667  status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
1668  }
1669 
1670  if (skip_path_planning)
1671  {
1672  publishEmptyPath();
1673  }
1674  else
1675  {
1676  nav_msgs::Path path;
1677  path.header = map_header_;
1678  path.header.stamp = now;
1679  makePlan(start_.pose, goal_.pose, path, true);
1680  publishPath(path);
1681  if ((sw_wait_ > 0.0) && !keep_a_part_of_previous_path_)
1682  {
1683  const int sw_index = getSwitchIndex(path);
1684  is_path_switchback_ = (sw_index >= 0);
1685  if (is_path_switchback_)
1686  sw_pos_ = path.poses[sw_index];
1687  }
1688  }
1689  }
1690  }
1691  else if (!has_goal_)
1692  {
1693  if (!retain_last_error_status_)
1694  status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
1695  publishEmptyPath();
1696  }
1697  publishCurrentGoal();
1698  status_.header.stamp = now;
1699  pub_status_.publish(status_);
1700  diag_updater_.force_update();
1701 
1702  metrics_.header.stamp = now;
1703  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
1704  "stuck_cnt",
1705  cnt_stuck_,
1706  "count"));
1707  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
1708  "error",
1709  status_.error,
1710  "enum"));
1711  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
1712  "status",
1713  status_.status,
1714  "enum"));
1715  pub_metrics_.publish(metrics_);
1716  metrics_.data.clear();
1717  }
1718 
1719  void spin()
1720  {
1721  ROS_DEBUG("Initialized");
1722 
1723  ros::Time next_replan_time = ros::Time::now();
1724  ros::Rate r(100);
1725  while (ros::ok())
1726  {
1727  if (trigger_plan_by_costmap_update_)
1728  {
1729  if (jump_.detectJump())
1730  {
1731  bbf_costmap_->clear();
1732  }
1733  ros::spinOnce();
1734  r.sleep();
1735  }
1736  else
1737  {
1738  waitUntil(next_replan_time);
1739  const ros::Time now = ros::Time::now();
1740  planPath(now);
1741  if (is_path_switchback_)
1742  {
1743  next_replan_time = now + ros::Duration(sw_wait_);
1744  ROS_INFO("Planned path has switchback. Planner will stop until: %f at the latest.", next_replan_time.toSec());
1745  }
1746  else
1747  {
1748  next_replan_time = now + ros::Duration(1.0 / freq_);
1749  }
1750  }
1751  }
1752  }
1753 
1754 protected:
1755  void publishStartAndGoalMarkers(const Astar::Vec& start_grid, const Astar::Vec& end_grid)
1756  {
1757  geometry_msgs::PoseStamped p;
1758  p.header = map_header_;
1759  p.pose = grid2MetricPose(end_grid);
1760  pub_end_.publish(p);
1761  p.pose = grid2MetricPose(start_grid);
1762  pub_start_.publish(p);
1763  }
1764 
1765  void publishCurrentGoal()
1766  {
1767  geometry_msgs::PoseStamped p(goal_);
1768  p.header = map_header_;
1769  pub_goal_.publish(p);
1770  }
1771 
1772  bool isPathFinishing(const Astar::Vec& start_grid, const Astar::Vec& end_grid) const
1773  {
1774  int g_tolerance_lin, g_tolerance_ang;
1775  if (escaping_)
1776  {
1777  g_tolerance_lin = temporary_escape_tolerance_lin_;
1778  g_tolerance_ang = temporary_escape_tolerance_ang_;
1779  }
1780  else if (act_tolerant_->isActive())
1781  {
1782  g_tolerance_lin = std::lround(goal_tolerant_->goal_tolerance_lin / map_info_.linear_resolution);
1783  g_tolerance_ang = std::lround(goal_tolerant_->goal_tolerance_ang / map_info_.angular_resolution);
1784  }
1785  else
1786  {
1787  g_tolerance_lin = goal_tolerance_lin_;
1788  g_tolerance_ang = goal_tolerance_ang_;
1789  }
1790  Astar::Vec remain = start_grid - end_grid;
1791  remain.cycle(map_info_.angle);
1792  return (remain.sqlen() <= g_tolerance_lin * g_tolerance_lin && std::abs(remain[2]) <= g_tolerance_ang);
1793  }
1794 
1795  enum class StartPoseStatus
1796  {
1797  START_OCCUPIED,
1798  FINISHING,
1799  NORMAL,
1800  };
1801  StartPoseStatus buildStartPoses(const geometry_msgs::Pose& start_metric, const geometry_msgs::Pose& end_metric,
1802  std::vector<Astar::VecWithCost>& result_start_poses)
1803  {
1804  result_start_poses.clear();
1805  start_pose_predictor_.clear();
1806 
1807  Astar::Vecf sf;
1808  grid_metric_converter::metric2Grid(
1809  map_info_, sf[0], sf[1], sf[2],
1810  start_metric.position.x, start_metric.position.y, tf2::getYaw(start_metric.orientation));
1811  Astar::Vec start_grid(static_cast<int>(std::floor(sf[0])), static_cast<int>(std::floor(sf[1])), std::lround(sf[2]));
1812  start_grid.cycleUnsigned(map_info_.angle);
1813 
1814  const Astar::Vec end_grid = metric2Grid(end_metric);
1815 
1816  if (!cm_.validate(start_grid, range_))
1817  {
1818  ROS_ERROR("You are on the edge of the world.");
1819  return StartPoseStatus::START_OCCUPIED;
1820  }
1821 
1822  if (keep_a_part_of_previous_path_ && !previous_path_.poses.empty())
1823  {
1824  Astar::Vec expected_start_grid;
1825  if (start_pose_predictor_.process(start_metric, cm_, map_info_, previous_path_, expected_start_grid))
1826  {
1827  ROS_DEBUG("Start grid is moved to (%d, %d, %d) from (%d, %d, %d) by start pose predictor.",
1828  expected_start_grid[0], expected_start_grid[1], expected_start_grid[2],
1829  start_grid[0], start_grid[1], start_grid[2]);
1830  result_start_poses.push_back(Astar::VecWithCost(expected_start_grid));
1831  return isPathFinishing(start_grid, end_grid) ? StartPoseStatus::FINISHING : StartPoseStatus::NORMAL;
1832  }
1833  else
1834  {
1835  return StartPoseStatus::START_OCCUPIED;
1836  }
1837  }
1838 
1839  if (antialias_start_)
1840  {
1841  const int x_cand[] = {0, ((sf[0] - start_grid[0]) < 0.5 ? -1 : 1)};
1842  const int y_cand[] = {0, ((sf[1] - start_grid[1]) < 0.5 ? -1 : 1)};
1843  for (const int x : x_cand)
1844  {
1845  for (const int y : y_cand)
1846  {
1847  const Astar::Vec p = start_grid + Astar::Vec(x, y, 0);
1848  if (!cm_.validate(p, range_))
1849  continue;
1850 
1851  const Astar::Vecf subpx = sf - Astar::Vecf(p[0] + 0.5f, p[1] + 0.5f, 0.0f);
1852  if (subpx.sqlen() > 1.0)
1853  continue;
1854  if (cm_[p] > 99)
1855  continue;
1856 
1857  const Astar::Vecf diff = Astar::Vecf(p[0] + 0.5f, p[1] + 0.5f, 0.0f) - sf;
1858  const double cost = std::hypot(diff[0] * ec_[0], diff[1] * ec_[0]) +
1859  cm_[p] * cc_.weight_costmap_ / 100.0;
1860  result_start_poses.push_back(Astar::VecWithCost(p, cost));
1861  }
1862  }
1863  }
1864  else if (cm_[start_grid] < 100)
1865  {
1866  result_start_poses.push_back(Astar::VecWithCost(start_grid));
1867  }
1868  if (result_start_poses.empty())
1869  {
1870  const Astar::Vec original_start_grid = start_grid;
1871  if (!searchAvailablePos(cm_, start_grid, tolerance_range_, tolerance_angle_))
1872  {
1873  ROS_WARN("Oops! You are in Rock!");
1874  return StartPoseStatus::START_OCCUPIED;
1875  }
1876  ROS_INFO("Start grid is moved to (%d, %d, %d) from (%d, %d, %d) by relocation.",
1877  start_grid[0], start_grid[1], start_grid[2],
1878  original_start_grid[0], original_start_grid[1], original_start_grid[2]);
1879  result_start_poses.push_back(Astar::VecWithCost(start_grid));
1880  }
1881  for (const Astar::VecWithCost& s : result_start_poses)
1882  {
1883  if (isPathFinishing(s.v_, end_grid))
1884  {
1885  return StartPoseStatus::FINISHING;
1886  }
1887  }
1888  return StartPoseStatus::NORMAL;
1889  }
1890 
1891  bool makePlan(const geometry_msgs::Pose& start_metric, const geometry_msgs::Pose& end_metric,
1892  nav_msgs::Path& path, bool hyst)
1893  {
1894  const Astar::Vec start_grid = metric2Grid(start_metric);
1895  const Astar::Vec end_grid = metric2Grid(end_metric);
1896  publishStartAndGoalMarkers(start_grid, end_grid);
1897 
1898  std::vector<Astar::VecWithCost> starts;
1899  switch (buildStartPoses(start_metric, end_metric, starts))
1900  {
1901  case StartPoseStatus::START_OCCUPIED:
1902  status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
1903  return false;
1904  case StartPoseStatus::FINISHING:
1905  if (escaping_)
1906  {
1907  goal_ = goal_raw_ = goal_original_;
1908  escaping_ = false;
1909  createCostEstimCache();
1910  ROS_INFO("Escaped");
1911  return true;
1912  }
1913  if (act_tolerant_->isActive() && goal_tolerant_->continuous_movement_mode)
1914  {
1915  ROS_INFO("Robot reached near the goal.");
1916  act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(),
1917  "Goal reached (Continuous movement mode).");
1918  goal_tolerant_ = nullptr;
1919  }
1920  else
1921  {
1922  status_.status = planner_cspace_msgs::PlannerStatus::FINISHING;
1923  publishFinishPath();
1924  ROS_INFO("Path plan finishing");
1925  return true;
1926  }
1927  break;
1928  default:
1929  break;
1930  }
1931 
1932  const float initial_2dof_cost = cost_estim_cache_[Astar::Vec(start_grid[0], start_grid[1], 0)];
1933  // If goal gets occupied, cost_estim_cache_ is not updated to reduce
1934  // computational cost for clearing huge map. In this case, cm_[e] is 100.
1935  if (initial_2dof_cost == std::numeric_limits<float>::max() || cm_[end_grid] >= 100)
1936  {
1937  status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
1938  ROS_WARN("Goal unreachable.");
1939  start_pose_predictor_.clear();
1940  if (temporary_escape_)
1941  {
1942  updateTemporaryEscapeGoal(start_grid);
1943  }
1944  return false;
1945  }
1946 
1947  const float range_limit = initial_2dof_cost - (local_range_ + range_) * ec_[0];
1948  const auto ts = boost::chrono::high_resolution_clock::now();
1949  const auto cb_progress =
1950  [this, ts, start_grid, end_grid](const std::list<Astar::Vec>& path_grid, const SearchStats& stats) -> bool
1951  {
1952  const auto tnow = boost::chrono::high_resolution_clock::now();
1953  const auto tdiff = boost::chrono::duration<float>(tnow - ts).count();
1954  publishEmptyPath();
1955  if (tdiff > search_timeout_abort_)
1956  {
1957  ROS_ERROR(
1958  "Search aborted due to timeout. "
1959  "search_timeout_abort may be too small or planner_3d may have a bug: "
1960  "s=(%d, %d, %d), g=(%d, %d, %d), tdiff=%0.4f, "
1961  "num_loop=%lu, "
1962  "num_search_queue=%lu, "
1963  "num_prev_updates=%lu, "
1964  "num_total_updates=%lu, ",
1965  start_grid[0], start_grid[1], start_grid[2], end_grid[0], end_grid[1], end_grid[2], tdiff,
1966  stats.num_loop, stats.num_search_queue, stats.num_prev_updates, stats.num_total_updates);
1967  return false;
1968  }
1969  ROS_WARN("Search timed out (%0.4f sec.)", tdiff);
1970  return true;
1971  };
1972 
1973  model_->enableHysteresis(hyst && has_hysteresis_map_);
1974  std::list<Astar::Vec> path_grid;
1975  bool is_goal_same_as_start = false;
1976  for (const auto s : starts)
1977  {
1978  if (s.v_ == end_grid)
1979  {
1980  ROS_DEBUG("The start grid is the same as the end grid. Path planning skipped.");
1981  path_grid.push_back(end_grid);
1982  is_goal_same_as_start = true;
1983  break;
1984  }
1985  }
1986  if (!is_goal_same_as_start && !as_.search(starts, end_grid, path_grid,
1987  model_,
1988  cb_progress,
1989  range_limit,
1990  1.0f / freq_min_,
1991  find_best_))
1992  {
1993  ROS_WARN("Path plan failed (goal unreachable)");
1994  status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
1995  if (!find_best_)
1996  return false;
1997  }
1998  const auto tnow = boost::chrono::high_resolution_clock::now();
1999  const float dur = boost::chrono::duration<float>(tnow - ts).count();
2000  ROS_DEBUG("Path found (%0.4f sec.)", dur);
2001  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
2002  "path_search_dur",
2003  dur,
2004  "second"));
2005 
2006  geometry_msgs::PoseArray poses;
2007  poses.header = path.header;
2008  for (const auto& p : path_grid)
2009  {
2010  poses.poses.push_back(grid2MetricPose(p));
2011  }
2012  pub_path_poses_.publish(poses);
2013  if (!start_pose_predictor_.getPreservedPath().poses.empty())
2014  {
2015  pub_preserved_path_poses_.publish(start_pose_predictor_.getPreservedPath());
2016  }
2017  const std::list<Astar::Vecf> path_interpolated = model_->interpolatePath(path_grid);
2018  path.poses = start_pose_predictor_.getPreservedPath().poses;
2019  grid_metric_converter::appendGridPath2MetricPath(map_info_, path_interpolated, path);
2020 
2021  if (hyst)
2022  {
2023  const auto ts = boost::chrono::high_resolution_clock::now();
2024  std::unordered_map<Astar::Vec, bool, Astar::Vec> path_points;
2025  const float max_dist = cc_.hysteresis_max_dist_ / map_info_.linear_resolution;
2026  const float expand_dist = cc_.hysteresis_expand_ / map_info_.linear_resolution;
2027  const int path_range = range_ + max_dist + expand_dist + 5;
2028  for (const Astar::Vecf& p : path_interpolated)
2029  {
2030  Astar::Vec d;
2031  for (d[0] = -path_range; d[0] <= path_range; d[0]++)
2032  {
2033  for (d[1] = -path_range; d[1] <= path_range; d[1]++)
2034  {
2035  Astar::Vec point = p + d;
2036  point.cycleUnsigned(map_info_.angle);
2037  if ((unsigned int)point[0] >= (unsigned int)map_info_.width ||
2038  (unsigned int)point[1] >= (unsigned int)map_info_.height)
2039  continue;
2040  path_points[point] = true;
2041  }
2042  }
2043  }
2044 
2045  clearHysteresis();
2046  for (auto& ps : path_points)
2047  {
2048  const Astar::Vec& p = ps.first;
2049  float d_min = std::numeric_limits<float>::max();
2050  auto it_prev = path_interpolated.cbegin();
2051  for (auto it = path_interpolated.cbegin(); it != path_interpolated.cend(); it++)
2052  {
2053  if (it != it_prev)
2054  {
2055  int yaw = std::lround((*it)[2]) % map_info_.angle;
2056  int yaw_prev = std::lround((*it_prev)[2]) % map_info_.angle;
2057  if (yaw < 0)
2058  yaw += map_info_.angle;
2059  if (yaw_prev < 0)
2060  yaw_prev += map_info_.angle;
2061  if (yaw == p[2] || yaw_prev == p[2])
2062  {
2063  const float d =
2064  CyclicVecFloat<3, 2>(p).distLinestrip2d(*it_prev, *it);
2065  if (d < d_min)
2066  d_min = d;
2067  }
2068  }
2069  it_prev = it;
2070  }
2071  d_min = std::max(expand_dist, std::min(expand_dist + max_dist, d_min));
2072  cm_hyst_[p] = std::lround((d_min - expand_dist) * 100.0 / max_dist);
2073  hyst_updated_cells_.push_back(p);
2074  }
2075  has_hysteresis_map_ = true;
2076  const auto tnow = boost::chrono::high_resolution_clock::now();
2077  const float dur = boost::chrono::duration<float>(tnow - ts).count();
2078  ROS_DEBUG("Hysteresis map generated (%0.4f sec.)", dur);
2079  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
2080  "hyst_map_dur",
2081  dur,
2082  "second"));
2083  publishDebug();
2084  }
2085 
2086  return true;
2087  }
2088 
2089  void updateTemporaryEscapeGoal(const Astar::Vec& start_grid, const bool log_on_unready = true)
2090  {
2091  if (!has_map_ || !has_goal_ || !has_start_)
2092  {
2093  if (log_on_unready)
2094  {
2095  ROS_WARN("Not ready to update temporary escape goal");
2096  }
2097  return;
2098  }
2099  if (is_path_switchback_)
2100  {
2101  ROS_INFO("Skipping temporary goal update during switch back");
2102  return;
2103  }
2104 
2105  if (!enable_crowd_mode_)
2106  {
2107  // Just find available (not occupied) pose
2108  Astar::Vec te;
2109  if (!searchAvailablePos(cm_, te, esc_range_, esc_angle_, relocation_acceptable_cost_, esc_range_min_))
2110  {
2111  ROS_WARN("No valid temporary escape goal");
2112  return;
2113  }
2114  escaping_ = true;
2115  ROS_INFO("Temporary goal (%d, %d, %d)", te[0], te[1], te[2]);
2116  goal_raw_.pose = grid2MetricPose(te);
2117  goal_ = goal_raw_;
2118 
2119  createCostEstimCache();
2120  return;
2121  }
2122 
2123  // Find available pos with minumum cost on static distance map
2124  {
2125  const Astar::Vec s(start_grid[0], start_grid[1], 0);
2126  if (!cm_.validate(s, esc_range_))
2127  {
2128  ROS_ERROR("Crowd escape is disabled on the edge of the world.");
2129  return;
2130  }
2131  const Astar::Vec g_orig = metric2Grid(goal_original_.pose);
2132 
2133  {
2134  const auto ts = boost::chrono::high_resolution_clock::now();
2135  // Update without region.
2136  // Distance map will expand distance map using edges_buf if needed.
2137  cost_estim_cache_static_.update(
2138  s, g_orig,
2139  DistanceMap::Rect(Astar::Vec(1, 1, 0), Astar::Vec(0, 0, 0)));
2140  const auto tnow = boost::chrono::high_resolution_clock::now();
2141  const float dur = boost::chrono::duration<float>(tnow - ts).count();
2142  ROS_DEBUG("Cost estimation cache for static map updated (%0.4f sec.)", dur);
2143  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
2144  "distance_map_static_update_dur", dur, "second"));
2145  metrics_.data.push_back(neonavigation_metrics_msgs::metric(
2146  "distance_map_static_init_dur", 0.0, "second"));
2147  }
2148 
2149  // Construct arraivability map
2150  const int local_width = esc_range_ * 2 + 1;
2151  const Astar::Vec local_origin = s - Astar::Vec(esc_range_, esc_range_, 0);
2152  const Astar::Vec local_range(local_width, local_width, 0);
2153  const Astar::Vec local_size(local_width + 1, local_width + 1, 1);
2154  const Astar::Vec local_center(esc_range_, esc_range_, 0);
2155  const DistanceMap::Params dmp =
2156  {
2157  .euclid_cost = ec_,
2158  .range = 0,
2159  .local_range = 0,
2160  .longcut_range = esc_range_,
2161  .size = local_size,
2162  .resolution = map_info_.linear_resolution,
2163  };
2164 
2165  cm_local_esc_.reset(local_size);
2166  arrivable_map_.setParams(cc_, num_cost_estim_task_);
2167  arrivable_map_.init(model_, dmp);
2168  cm_local_esc_.copy_partially(
2169  Astar::Vec(0, 0, 0), cm_rough_, local_origin, local_origin + local_range);
2170  arrivable_map_.create(local_center, local_center);
2171 
2172  // Find temporary goal
2173  float cost_min = std::numeric_limits<float>::max();
2174  Astar::Vec te_out;
2175  const int esc_range_sq = esc_range_ * esc_range_;
2176  const int esc_range_min_sq = esc_range_min_ * esc_range_min_;
2177  for (Astar::Vec d(0, -esc_range_, 0); d[1] <= esc_range_; d[1]++)
2178  {
2179  for (d[0] = -esc_range_; d[0] <= esc_range_; d[0]++)
2180  {
2181  const int sqlen = d.sqlen();
2182  if ((d[0] == 0 && d[1] == 0) || sqlen > esc_range_sq || sqlen < esc_range_min_sq)
2183  {
2184  continue;
2185  }
2186 
2187  Astar::Vec te(start_grid[0] + d[0], start_grid[1] + d[1], 0);
2188  if ((unsigned int)te[0] >= (unsigned int)map_info_.width ||
2189  (unsigned int)te[1] >= (unsigned int)map_info_.height)
2190  {
2191  continue;
2192  }
2193  if (!cm_rough_.validate(te, range_) || cm_rough_[te] >= relocation_acceptable_cost_)
2194  {
2195  continue;
2196  }
2197  const auto cost = cost_estim_cache_static_[te];
2198  if (cost >= cost_min)
2199  {
2200  continue;
2201  }
2202 
2203  // Check arrivability
2204  if (arrivable_map_[te - local_origin] == std::numeric_limits<float>::max())
2205  {
2206  continue;
2207  }
2208 
2209  if (te[0] == g_orig[0] && te[1] == g_orig[1] && cm_[g_orig] < 100)
2210  {
2211  // Original goal is in the temporary escape range and reachable
2212  ROS_INFO("Original goal is reachable. Back to the original goal.");
2213  goal_ = goal_raw_ = goal_original_;
2214  escaping_ = false;
2215  createCostEstimCache();
2216  return;
2217  }
2218 
2219  // Calculate distance map gradient
2220  float grad[2] = {0, 0};
2221  for (Astar::Vec d(0, -1, 0); d[1] <= 1; d[1]++)
2222  {
2223  for (d[0] = -1; d[0] <= 1; d[0]++)
2224  {
2225  if (d[0] == 0 && d[1] == 0)
2226  {
2227  continue;
2228  }
2229 
2230  const auto p = te + d;
2231  const auto cost2 = cost_estim_cache_static_[p];
2232  if (cost2 == std::numeric_limits<float>::max())
2233  {
2234  continue;
2235  }
2236  const float cost_diff = cost2 - cost;
2237  grad[0] += -cost_diff * d[0];
2238  grad[1] += -cost_diff * d[1];
2239  }
2240  }
2241  if (grad[0] == 0 && grad[1] == 0)
2242  {
2243  continue;
2244  }
2245  const float yaw = std::atan2(grad[1], grad[0]);
2246  te[2] = static_cast<int>(yaw / map_info_.angular_resolution);
2247 
2248  te.cycleUnsigned(map_info_.angle);
2249  if (cm_[te] >= relocation_acceptable_cost_)
2250  {
2251  continue;
2252  }
2253 
2254  if (cost < cost_min)
2255  {
2256  cost_min = cost;
2257  te_out = te;
2258  }
2259  }
2260  }
2261  if (cost_min == std::numeric_limits<float>::max())
2262  {
2263  ROS_WARN("No valid temporary escape goal");
2264  return;
2265  }
2266 
2267  escaping_ = true;
2268  ROS_INFO("Temporary goal (%d, %d, %d)",
2269  te_out[0], te_out[1], te_out[2]);
2270  goal_raw_.pose = grid2MetricPose(te_out);
2271  goal_ = goal_raw_;
2272 
2273  createCostEstimCache();
2274  }
2275  }
2276 
2277  int getSwitchIndex(const nav_msgs::Path& path) const
2278  {
2279  geometry_msgs::Pose p_prev;
2280  bool first(true);
2281  bool dir_set(false);
2282  bool dir_prev(false);
2283  for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
2284  {
2285  const auto& p = *it;
2286  if (!first)
2287  {
2288  const float x_diff = p.pose.position.x - p_prev.position.x;
2289  const float y_diff = p.pose.position.y - p_prev.position.y;
2290  const float len_sq = std::pow(y_diff, 2) + std::pow(x_diff, 2);
2291  if (len_sq > std::pow(0.001f, 2))
2292  {
2293  const float yaw = tf2::getYaw(p.pose.orientation);
2294  const bool dir = (std::cos(yaw) * x_diff + std::sin(yaw) * y_diff < 0);
2295 
2296  if (dir_set && (dir_prev ^ dir))
2297  {
2298  return std::distance(path.poses.begin(), it);
2299  }
2300  dir_prev = dir;
2301  dir_set = true;
2302  }
2303  }
2304  first = false;
2305  p_prev = p.pose;
2306  }
2307  // -1 means no switchback in the path
2308  return -1;
2309  }
2310  void diagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper& stat)
2311  {
2312  switch (status_.error)
2313  {
2314  case planner_cspace_msgs::PlannerStatus::GOING_WELL:
2315  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Going well.");
2316  break;
2317  case planner_cspace_msgs::PlannerStatus::IN_ROCK:
2318  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The robot is in rock.");
2319  break;
2320  case planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND:
2321  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Path not found.");
2322  break;
2323  case planner_cspace_msgs::PlannerStatus::DATA_MISSING:
2324  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Required data is missing.");
2325  break;
2326  case planner_cspace_msgs::PlannerStatus::INTERNAL_ERROR:
2327  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Planner internal error.");
2328  break;
2329  default:
2330  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Unknown error.");
2331  break;
2332  }
2333  stat.addf("status", "%u", status_.status);
2334  stat.addf("error", "%u", status_.error);
2335  }
2336 };
2337 } // namespace planner_3d
2338 } // namespace planner_cspace
2339 
2340 int main(int argc, char* argv[])
2341 {
2342  ros::init(argc, argv, "planner_3d");
2343 
2344  planner_cspace::planner_3d::Planner3dNode node;
2345  node.spin();
2346 
2347  return 0;
2348 }
planner_cspace::planner_3d::Planner3dNode::esc_range_min_ratio_
double esc_range_min_ratio_
Definition: planner_3d.cpp:162
planner_cspace::planner_3d::Planner3dNode::pub_distance_map_
ros::Publisher pub_distance_map_
Definition: planner_3d.cpp:114
planner_cspace::planner_3d::CostCoeff
Definition: planner_3d/grid_astar_model.h:51
planner_cspace::planner_3d::Planner3dNode::publishRememberedMap
void publishRememberedMap()
Definition: planner_3d.cpp:723
planner_cspace::planner_3d::Planner3dNode::isPathFinishing
bool isPathFinishing(const Astar::Vec &start_grid, const Astar::Vec &end_grid) const
Definition: planner_3d.cpp:1772
planner_cspace::planner_3d::Planner3dNode::DiscretePoseStatus
DiscretePoseStatus
Definition: planner_3d.cpp:268
planner_cspace::planner_3d::CostCoeff::max_ang_vel_
float max_ang_vel_
Definition: planner_3d/grid_astar_model.h:67
planner_cspace::planner_3d::JumpDetector::setMapFrame
void setMapFrame(const std::string &frame_id)
Definition: jump_detector.h:69
planner_cspace::planner_3d::grid_metric_converter::grid2Metric
void grid2Metric(const costmap_cspace_msgs::MapMetaData3D &map_info, const T x, const T y, const T yaw, float &gx, float &gy, float &gyaw)
Definition: grid_metric_converter.h:50
planner_cspace::planner_3d::Planner3dNode::sw_pos_
geometry_msgs::PoseStamped sw_pos_
Definition: planner_3d.cpp:226
planner_cspace::planner_3d::DistanceMap::DebugData::search_queue_cap
size_t search_queue_cap
Definition: distance_map.h:66
planner_cspace::planner_3d::DistanceMap::DebugData::has_negative_cost
bool has_negative_cost
Definition: distance_map.h:67
planner_cspace::planner_3d::StartPosePredictor::clear
void clear()
Definition: start_pose_predictor.cpp:76
planner_cspace::planner_3d::Planner3dNode::has_goal_
bool has_goal_
Definition: planner_3d.cpp:173
planner_cspace::planner_3d::Planner3dNode::status_
planner_cspace_msgs::PlannerStatus status_
Definition: planner_3d.cpp:221
planner_cspace::planner_3d::Planner3dNode::spin
void spin()
Definition: planner_3d.cpp:1719
planner_cspace::planner_3d::Planner3dNode::temporary_escape_tolerance_lin_f_
double temporary_escape_tolerance_lin_f_
Definition: planner_3d.cpp:216
planner_cspace::planner_3d::Planner3dNode::freq_
float freq_
Definition: planner_3d.cpp:149
planner_cspace::planner_3d::Planner3dNode::hist_ignore_range_max_
int hist_ignore_range_max_
Definition: planner_3d.cpp:185
planner_cspace::planner_3d::Planner3dNode::cbMap
void cbMap(const costmap_cspace_msgs::CSpace3D::ConstPtr &msg)
Definition: planner_3d.cpp:986
planner_cspace
Definition: bbf.h:33
msg
msg
ros::Publisher
planner_cspace::planner_3d::Planner3dNode::bbf_costmap_
CostmapBBF::Ptr bbf_costmap_
Definition: planner_3d.cpp:139
planner_cspace::planner_3d::Planner3dNode::esc_range_
int esc_range_
Definition: planner_3d.cpp:158
planner_cspace::planner_3d::Planner3dNode::temporary_escape_
bool temporary_escape_
Definition: planner_3d.cpp:186
planner_cspace::planner_3d::Planner3dNode::jump_
JumpDetector jump_
Definition: planner_3d.cpp:196
planner_cspace::planner_3d::Planner3dNode::retain_last_error_status_
bool retain_last_error_status_
Definition: planner_3d.cpp:190
planner_cspace::planner_3d::Planner3dNode::Planner3DActionServer
actionlib::SimpleActionServer< move_base_msgs::MoveBaseAction > Planner3DActionServer
Definition: planner_3d.cpp:100
planner_cspace::planner_3d::StartPosePredictor
Definition: start_pose_predictor.h:47
planner_cspace::planner_3d::CostCoeff::weight_decel_
float weight_decel_
Definition: planner_3d/grid_astar_model.h:54
planner_cspace::bbf::MIN_PROBABILITY
const float MIN_PROBABILITY
Definition: bbf.h:47
planner_cspace::planner_3d::Planner3dNode::cm_local_esc_
Astar::Gridmap< char, 0x80 > cm_local_esc_
Definition: planner_3d.cpp:138
planner_cspace::planner_3d::Planner3dNode::cbAction
void cbAction()
Definition: planner_3d.cpp:1100
motion_cache.h
planner_cspace::planner_3d::Planner3dNode::overwrite_cost_
bool overwrite_cost_
Definition: planner_3d.cpp:170
planner_cspace::planner_3d::Planner3dNode::act_
std::shared_ptr< Planner3DActionServer > act_
Definition: planner_3d.cpp:124
planner_cspace::planner_3d::Planner3dNode::pub_goal_
ros::Publisher pub_goal_
Definition: planner_3d.cpp:118
planner_cspace::planner_3d::Planner3dNode::model_
GridAstarModel3D::Ptr model_
Definition: planner_3d.cpp:144
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
helper.h
diagnostic_updater::Updater::force_update
void force_update()
planner_cspace::planner_3d::Planner3dNode::pub_hysteresis_map_
ros::Publisher pub_hysteresis_map_
Definition: planner_3d.cpp:113
planner_cspace::planner_3d::Planner3dNode::pub_remembered_map_
ros::Publisher pub_remembered_map_
Definition: planner_3d.cpp:115
planner_cspace::planner_3d::Planner3dNode::DiscretePoseStatus::OK
@ OK
planner_cspace::planner_3d::Planner3dNode::cc_
CostCoeff cc_
Definition: planner_3d.cpp:204
tf2::getYaw
double getYaw(const A &a)
planner_cspace::planner_3d::Planner3dNode::prev_map_update_y_min_
int prev_map_update_y_min_
Definition: planner_3d.cpp:244
rotation_cache.h
utils.h
planner_cspace::planner_3d::Planner3dNode::updateStart
void updateStart()
Definition: planner_3d.cpp:1126
s
XmlRpcServer s
planner_cspace::planner_3d::CostCoeff::weight_costmap_turn_
float weight_costmap_turn_
Definition: planner_3d/grid_astar_model.h:58
planner_cspace::planner_3d::Planner3dNode::has_map_
bool has_map_
Definition: planner_3d.cpp:172
planner_cspace::planner_3d::Planner3dNode::start_
geometry_msgs::PoseStamped start_
Definition: planner_3d.cpp:206
planner_cspace::planner_3d::DistanceMap::update
void update(const Astar::Vec &s, const Astar::Vec &e, const Rect &rect)
Definition: distance_map.cpp:250
planner_cspace::planner_3d::Planner3dNode::sub_map_
ros::Subscriber sub_map_
Definition: planner_3d.cpp:105
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(period,...)
planner_cspace::planner_3d::GridAstarModel3D
Definition: planner_3d/grid_astar_model.h:72
planner_cspace::planner_3d::Planner3dNode::goal_
geometry_msgs::PoseStamped goal_
Definition: planner_3d.cpp:207
ros.h
planner_cspace::planner_3d::CostmapBBFImpl
Definition: costmap_bbf.h:62
planner_cspace::planner_3d::Planner3dNode::esc_range_min_
int esc_range_min_
Definition: planner_3d.cpp:159
bbf.h
planner_cspace::planner_3d::Planner3dNode::search_list_
std::vector< Astar::Vec > search_list_
Definition: planner_3d.cpp:180
ros::Timer::stop
void stop()
planner_cspace::planner_3d::Planner3dNode::publishPath
void publishPath(const nav_msgs::Path &path)
Definition: planner_3d.cpp:765
planner_cspace::planner_3d::Planner3dNode::pub_path_
ros::Publisher pub_path_
Definition: planner_3d.cpp:109
planner_cspace::planner_3d::JumpDetector
Definition: jump_detector.h:46
start_pose_predictor.h
planner_cspace::planner_3d::CostCoeff::weight_ang_vel_
float weight_ang_vel_
Definition: planner_3d/grid_astar_model.h:56
planner_cspace::planner_3d::Planner3dNode::DiscretePoseStatus::OUT_OF_MAP
@ OUT_OF_MAP
ros::spinOnce
ROSCPP_DECL void spinOnce()
diagnostic_updater::Updater
planner_cspace::GridAstar< 3, 2 >::VecWithCost
typename GridAstarModelBase< DIM, NONCYCLIC >::VecWithCost VecWithCost
Definition: grid_astar.h:69
TimeBase< Time, Duration >::toSec
double toSec() const
planner_cspace::planner_3d::JumpDetector::setThresholds
void setThresholds(const double pos_jump, const double yaw_jump)
Definition: jump_detector.h:77
planner_cspace::planner_3d::Planner3dNode::map_info_
costmap_cspace_msgs::MapMetaData3D map_info_
Definition: planner_3d.cpp:146
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
planner_cspace::planner_3d::Planner3dNode::createCostEstimCache
bool createCostEstimCache(const bool goal_changed=true)
Definition: planner_3d.cpp:560
planner_cspace::planner_3d::Planner3dNode::grid2MetricPoint
geometry_msgs::Point32 grid2MetricPoint(const Astar::Vec &grid) const
Definition: planner_3d.cpp:330
planner_cspace::planner_3d::Planner3dNode::cbTemporaryEscape
void cbTemporaryEscape(const std_msgs::Empty::ConstPtr &)
Definition: planner_3d.cpp:259
planner_cspace::planner_3d::Planner3dNode::pnh_
ros::NodeHandle pnh_
Definition: planner_3d.cpp:104
simple_action_server.h
planner_cspace::planner_3d::Planner3dNode::cbMakePlan
bool cbMakePlan(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res)
Definition: planner_3d.cpp:340
planner_cspace::planner_3d::Planner3dNode::temporary_escape_tolerance_lin_
int temporary_escape_tolerance_lin_
Definition: planner_3d.cpp:218
compatibility.h
planner_cspace::planner_3d::Planner3dNode::nh_
ros::NodeHandle nh_
Definition: planner_3d.cpp:103
planner_cspace::planner_3d::Planner3dNode::pub_end_
ros::Publisher pub_end_
Definition: planner_3d.cpp:117
planner_cspace::planner_3d::Planner3dNode::tolerance_angle_
int tolerance_angle_
Definition: planner_3d.cpp:164
ros::console::set_logger_level
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
planner_cspace::planner_3d::Planner3dNode::has_hysteresis_map_
bool has_hysteresis_map_
Definition: planner_3d.cpp:175
grid_metric_converter.h
planner_cspace::planner_3d::Planner3dNode::setGoal
bool setGoal(const geometry_msgs::PoseStamped &msg)
Definition: planner_3d.cpp:458
converter.h
planner_cspace::planner_3d::Planner3dNode::prev_map_update_y_max_
int prev_map_update_y_max_
Definition: planner_3d.cpp:245
planner_cspace::planner_3d::Planner3dNode::cm_updates_
Astar::Gridmap< char, 0x80 > cm_updates_
Definition: planner_3d.cpp:137
planner_cspace::planner_3d::Planner3dNode::goal_tolerance_lin_
int goal_tolerance_lin_
Definition: planner_3d.cpp:214
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
grid_astar_model.h
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
planner_cspace::planner_3d::Planner3dNode::is_start_occupied_
bool is_start_occupied_
Definition: planner_3d.cpp:236
ros::ok
ROSCPP_DECL bool ok()
planner_cspace::planner_3d::Planner3dNode::cbNoMapUpdateTimer
void cbNoMapUpdateTimer(const ros::TimerEvent &e)
Definition: planner_3d.cpp:958
planner_cspace::planner_3d::CostmapBBF
Definition: costmap_bbf.h:43
planner_cspace::bbf::probabilityToOdds
constexpr float probabilityToOdds(const float &p)
Definition: bbf.h:42
planner_cspace::planner_3d::Planner3dNode::cbMapUpdate
void cbMapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr &msg)
Definition: planner_3d.cpp:964
planner_cspace::planner_3d::Planner3dNode::range_
int range_
Definition: planner_3d.cpp:154
diagnostic_updater.h
f
f
planner_cspace::planner_3d::Planner3dNode::max_retry_num_
int max_retry_num_
Definition: planner_3d.cpp:199
ros::ServiceServer
planner_cspace::planner_3d::Planner3dNode::trigger_plan_by_costmap_update_
bool trigger_plan_by_costmap_update_
Definition: planner_3d.cpp:193
costmap_bbf.h
planner_cspace::planner_3d::Planner3dNode::relocation_acceptable_cost_
int relocation_acceptable_cost_
Definition: planner_3d.cpp:171
tf2_ros::TransformListener
neonavigation_common::compat::advertiseService
ros::ServiceServer advertiseService(ros::NodeHandle &nh_new, const std::string &service_new, ros::NodeHandle &nh_old, const std::string &service_old, bool(T::*srv_func)(MReq &, MRes &), T *obj)
planner_cspace::planner_3d::Planner3dNode::goal_tolerance_ang_finish_
double goal_tolerance_ang_finish_
Definition: planner_3d.cpp:213
planner_cspace::planner_3d::DistanceMap::size
const Astar::Vec & size() const
Definition: distance_map.h:102
console.h
planner_cspace::planner_3d::Planner3dNode::diag_updater_
diagnostic_updater::Updater diag_updater_
Definition: planner_3d.cpp:238
planner_cspace::planner_3d::Planner3dNode::cbPreempt
void cbPreempt()
Definition: planner_3d.cpp:445
planner_cspace::planner_3d::Planner3dNode::esc_range_f_
double esc_range_f_
Definition: planner_3d.cpp:161
planner_cspace::planner_3d::grid_metric_converter::metric2Grid
void metric2Grid(const costmap_cspace_msgs::MapMetaData3D &map_info, int &x, int &y, int &yaw, const float gx, const float gy, const float gyaw)
Definition: grid_metric_converter.h:62
planner_cspace::planner_3d::Planner3dNode::DiscretePoseStatus::RELOCATED
@ RELOCATED
neonavigation_common::compat::subscribe
ros::Subscriber subscribe(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, const boost::function< void(const boost::shared_ptr< M const > &)> &callback, const ros::VoidConstPtr &tracked_object=ros::VoidConstPtr(), const ros::TransportHints &transport_hints=ros::TransportHints())
ros::console::levels::Debug
Debug
main
int main(int argc, char *argv[])
Definition: planner_3d.cpp:2340
planner_cspace::planner_3d::CostCoeff::weight_costmap_turn_heuristics_
float weight_costmap_turn_heuristics_
Definition: planner_3d/grid_astar_model.h:59
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(const diagnostic_msgs::DiagnosticStatus &src)
planner_cspace::planner_3d::Planner3dNode
Definition: planner_3d.cpp:94
planner_cspace::planner_3d::Planner3dNode::find_best_
bool find_best_
Definition: planner_3d.cpp:224
planner_cspace::planner_3d::CostCoeff::turn_penalty_cost_threshold_
int turn_penalty_cost_threshold_
Definition: planner_3d/grid_astar_model.h:69
planner_cspace::planner_3d::Planner3dNode::start_pose_predictor_
StartPosePredictor start_pose_predictor_
Definition: planner_3d.cpp:247
planner_cspace::planner_3d::Planner3dNode::cm_rough_base_
Astar::Gridmap< char, 0x80 > cm_rough_base_
Definition: planner_3d.cpp:135
planner_cspace::planner_3d::Planner3dNode::StartPoseStatus::FINISHING
@ FINISHING
planner_cspace::planner_3d::Planner3dNode::use_path_with_velocity_
bool use_path_with_velocity_
Definition: planner_3d.cpp:189
planner_cspace::planner_3d::Planner3dNode::remember_updates_
bool remember_updates_
Definition: planner_3d.cpp:178
planner_cspace::planner_3d::Planner3dNode::goal_tolerance_lin_f_
double goal_tolerance_lin_f_
Definition: planner_3d.cpp:211
planner_cspace::planner_3d::Planner3dNode::tolerance_range_f_
double tolerance_range_f_
Definition: planner_3d.cpp:165
planner_cspace::planner_3d::Planner3dNode::no_map_update_timer_
ros::Timer no_map_update_timer_
Definition: planner_3d.cpp:248
planner_cspace::planner_3d::Planner3dNode::publishStartAndGoalMarkers
void publishStartAndGoalMarkers(const Astar::Vec &start_grid, const Astar::Vec &end_grid)
Definition: planner_3d.cpp:1755
planner_cspace::planner_3d::Planner3dNode::prev_map_update_x_min_
int prev_map_update_x_min_
Definition: planner_3d.cpp:242
planner_cspace::planner_3d::CostCoeff::angle_resolution_aspect_
float angle_resolution_aspect_
Definition: planner_3d/grid_astar_model.h:68
planner_cspace::planner_3d::Planner3dNode::DiscretePoseStatus::IN_ROCK
@ IN_ROCK
planner_cspace::planner_3d::Planner3dNode::planPath
void planPath(const ros::Time &now)
Definition: planner_3d.cpp:1545
ROS_DEBUG
#define ROS_DEBUG(...)
planner_cspace::planner_3d::Planner3dNode::longcut_range_f_
double longcut_range_f_
Definition: planner_3d.cpp:157
planner_cspace::planner_3d::Planner3dNode::searchAvailablePos
bool searchAvailablePos(const T &cm, Astar::Vec &s, const int xy_range, const int angle_range, int cost_acceptable=-1, const int min_xy_range=0) const
Definition: planner_3d.cpp:501
planner_cspace::planner_3d::Planner3dNode::pub_path_poses_
ros::Publisher pub_path_poses_
Definition: planner_3d.cpp:111
planner_cspace::planner_3d::DistanceMap::getDebugData
const DebugData & getDebugData() const
Definition: distance_map.h:118
planner_cspace::planner_3d::Planner3dNode::tolerance_range_
int tolerance_range_
Definition: planner_3d.cpp:163
planner_cspace::planner_3d::Planner3dNode::map_header_
std_msgs::Header map_header_
Definition: planner_3d.cpp:148
planner_cspace::GridAstar< 3, 2 >
planner_cspace::GridAstar::setSearchTaskNum
void setSearchTaskNum(const size_t &search_task_num)
Definition: grid_astar.h:142
planner_cspace::planner_3d::CostCoeff::max_vel_
float max_vel_
Definition: planner_3d/grid_astar_model.h:66
planner_cspace::GridAstar::search
bool search(const std::vector< VecWithCost > &ss, const Vec &e, std::list< Vec > &path, const typename GridAstarModelBase< DIM, NONCYCLIC >::Ptr &model, ProgressCallback cb_progress, const float cost_leave, const float progress_interval, const bool return_best=false)
Definition: grid_astar.h:169
planner_cspace::planner_3d::CostCoeff::min_curve_radius_
float min_curve_radius_
Definition: planner_3d/grid_astar_model.h:65
planner_cspace::planner_3d::Planner3dNode::clearHysteresis
void clearHysteresis()
Definition: planner_3d.cpp:657
neonavigation_metrics_msgs::metric
Metric metric(const std::string &name, const double value, const std::string &unit, Strings &&... tags)
planner_cspace::planner_3d::DistanceMap::Params
Definition: distance_map.h:70
planner_cspace::planner_3d::Planner3dNode::hyst_updated_cells_
std::vector< Astar::Vec > hyst_updated_cells_
Definition: planner_3d.cpp:176
planner_cspace::planner_3d::StartPosePredictor::Config::switch_back_prediction_sec_
double switch_back_prediction_sec_
Definition: start_pose_predictor.h:55
planner_cspace::planner_3d::Planner3dNode::temporary_escape_tolerance_ang_
int temporary_escape_tolerance_ang_
Definition: planner_3d.cpp:219
planner_cspace::planner_3d::StartPosePredictor::Config::prediction_sec_
double prediction_sec_
Definition: start_pose_predictor.h:54
planner_cspace::planner_3d::Planner3dNode::cbGoal
void cbGoal(const geometry_msgs::PoseStamped::ConstPtr &msg)
Definition: planner_3d.cpp:436
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
d
d
planner_cspace::planner_3d::Planner3dNode::search_range_
float search_range_
Definition: planner_3d.cpp:152
planner_cspace::planner_3d::Planner3dNode::metric2Grid
Astar::Vec metric2Grid(const geometry_msgs::Pose &pose) const
Definition: planner_3d.cpp:310
ROS_WARN
#define ROS_WARN(...)
planner_cspace::planner_3d::StartPosePredictor::process
bool process(const geometry_msgs::Pose &robot_pose, const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &cm, const costmap_cspace_msgs::MapMetaData3D &map_info, const nav_msgs::Path &previous_path_msg, Astar::Vec &result_start_grid)
Definition: start_pose_predictor.cpp:42
planner_cspace::planner_3d::Planner3dNode::tfbuf_
tf2_ros::Buffer tfbuf_
Definition: planner_3d.cpp:127
planner_cspace::planner_3d::Planner3dNode::freq_min_
float freq_min_
Definition: planner_3d.cpp:150
tf2_ros::Buffer
planner_cspace::planner_3d::Planner3dNode::escaping_
bool escaping_
Definition: planner_3d.cpp:233
planner_cspace::planner_3d::CostCoeff::weight_remembered_
float weight_remembered_
Definition: planner_3d/grid_astar_model.h:60
planner_cspace::bbf::MAX_PROBABILITY
const float MAX_PROBABILITY
Definition: bbf.h:48
distance_map.h
planner_cspace::planner_3d::Planner3dNode::as_
Astar as_
Definition: planner_3d.cpp:131
planner_cspace::bbf::BinaryBayesFilter
Definition: bbf.h:52
planner_cspace::planner_3d::Planner3dNode::unknown_cost_
int unknown_cost_
Definition: planner_3d.cpp:169
planner_cspace::planner_3d::Planner3dNode::hist_ignore_range_f_
double hist_ignore_range_f_
Definition: planner_3d.cpp:182
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
planner_cspace::planner_3d::Planner3dNode::applyCostmapUpdate
void applyCostmapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr &msg)
Definition: planner_3d.cpp:780
planner_cspace::planner_3d::Planner3dNode::search_list_rough_
std::vector< Astar::Vec > search_list_rough_
Definition: planner_3d.cpp:181
ros::TimerEvent
planner_cspace::planner_3d::Planner3dNode::hist_ignore_range_max_f_
double hist_ignore_range_max_f_
Definition: planner_3d.cpp:184
planner_cspace::planner_3d::Planner3dNode::robot_frame_
std::string robot_frame_
Definition: planner_3d.cpp:197
planner_cspace::planner_3d::Planner3dNode::search_timeout_abort_
float search_timeout_abort_
Definition: planner_3d.cpp:151
planner_cspace::GridAstar< 3, 2 >::Vecf
CyclicVecFloat< DIM, NONCYCLIC > Vecf
Definition: grid_astar.h:68
planner_cspace::planner_3d::GridAstarModel2D
Definition: planner_3d/grid_astar_model.h:137
planner_cspace::planner_3d::Planner3dNode::StartPoseStatus::NORMAL
@ NORMAL
ros::TimerEvent::current_real
Time current_real
planner_cspace::planner_3d::StartPosePredictor::Config::lin_vel_
double lin_vel_
Definition: start_pose_predictor.h:57
jump_detector.h
diagnostic_updater::Updater::setHardwareID
void setHardwareID(const std::string &hwid)
ros::Rate::sleep
bool sleep()
planner_cspace::planner_3d::DistanceMap::create
void create(const Astar::Vec &s, const Astar::Vec &e)
Definition: distance_map.cpp:356
planner_cspace::planner_3d::StartPosePredictor::setConfig
void setConfig(const Config &config)
Definition: start_pose_predictor.h:60
planner_cspace::planner_3d::Planner3dNode::remember_miss_odds_
float remember_miss_odds_
Definition: planner_3d.cpp:188
neonavigation_common::compat::checkCompatMode
void checkCompatMode()
grid_astar.h
planner_cspace::planner_3d::Planner3dNode::publishCurrentGoal
void publishCurrentGoal()
Definition: planner_3d.cpp:1765
planner_cspace::planner_3d::Planner3dNode::cbTolerantAction
void cbTolerantAction()
Definition: planner_3d.cpp:1113
planner_cspace::planner_3d::Planner3dNode::diagnoseStatus
void diagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: planner_3d.cpp:2310
planner_cspace::planner_3d::Planner3dNode::rough_
bool rough_
Definition: planner_3d.cpp:229
planner_cspace::planner_3d::Planner3dNode::grid_enumeration_resolution_
double grid_enumeration_resolution_
Definition: planner_3d.cpp:168
planner_cspace::planner_3d::Planner3dNode::tolerance_angle_f_
double tolerance_angle_f_
Definition: planner_3d.cpp:166
transform_listener.h
planner_cspace::planner_3d::Planner3dNode::last_costmap_
ros::Time last_costmap_
Definition: planner_3d.cpp:240
planner_cspace::planner_3d::Planner3dNode::cost_estim_cache_
DistanceMap cost_estim_cache_
Definition: planner_3d.cpp:140
start
ROSCPP_DECL void start()
planner_cspace::CyclicVecBase::cycleUnsigned
void cycleUnsigned(const int res, const ArgList &... rest)
Definition: cyclic_vec.h:289
planner_cspace::planner_3d::Planner3dNode::waitUntil
void waitUntil(const ros::Time &next_replan_time)
Definition: planner_3d.cpp:1489
planner_cspace::planner_3d::Planner3dNode::hist_ignore_range_
int hist_ignore_range_
Definition: planner_3d.cpp:183
planner_cspace::planner_3d::Planner3dNode::publishFinishPath
void publishFinishPath()
Definition: planner_3d.cpp:751
planner_cspace::planner_3d::Planner3dNode::path_interpolation_resolution_
double path_interpolation_resolution_
Definition: planner_3d.cpp:167
diagnostic_updater::Updater::update
void update()
planner_cspace::planner_3d::Planner3dNode::cbForget
bool cbForget(std_srvs::EmptyRequest &req, std_srvs::EmptyResponse &res)
Definition: planner_3d.cpp:250
planner_cspace::planner_3d::Planner3dNode::pub_preserved_path_poses_
ros::Publisher pub_preserved_path_poses_
Definition: planner_3d.cpp:112
planner_cspace::CyclicVecBase::distLinestrip2d
float distLinestrip2d(const CyclicVecBase< DIM, NONCYCLIC, T > &a, const CyclicVecBase< DIM, NONCYCLIC, T > &b) const
Definition: cyclic_vec.h:220
planner_cspace::planner_3d::Planner3dNode::num_cost_estim_task_
int num_cost_estim_task_
Definition: planner_3d.cpp:191
planner_cspace::planner_3d::Planner3dNode::map_update_retained_
costmap_cspace_msgs::CSpace3DUpdate::ConstPtr map_update_retained_
Definition: planner_3d.cpp:147
planner_cspace::planner_3d::Planner3dNode::costmap_watchdog_
ros::Duration costmap_watchdog_
Definition: planner_3d.cpp:239
planner_cspace::planner_3d::Planner3dNode::pub_path_velocity_
ros::Publisher pub_path_velocity_
Definition: planner_3d.cpp:110
planner_cspace::planner_3d::Planner3dNode::esc_angle_
int esc_angle_
Definition: planner_3d.cpp:160
planner_cspace::planner_3d::Planner3dNode::resetGridAstarModel
void resetGridAstarModel(const bool force_reset)
Definition: planner_3d.cpp:1353
planner_cspace::planner_3d::Planner3dNode::goal_original_
geometry_msgs::PoseStamped goal_original_
Definition: planner_3d.cpp:209
planner_cspace::planner_3d::Planner3dNode::sub_temporary_escape_trigger_
ros::Subscriber sub_temporary_escape_trigger_
Definition: planner_3d.cpp:108
planner_cspace::planner_3d::Planner3dNode::has_start_
bool has_start_
Definition: planner_3d.cpp:174
planner_cspace::planner_3d::Planner3dNode::cm_hyst_
Astar::Gridmap< char, 0x80 > cm_hyst_
Definition: planner_3d.cpp:136
ros::console::notifyLoggerLevelsChanged
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
planner_cspace::planner_3d::Planner3dNode::local_range_
int local_range_
Definition: planner_3d.cpp:155
ros::Time
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
planner_cspace::planner_3d::Planner3dNode::temporary_escape_tolerance_ang_f_
double temporary_escape_tolerance_ang_f_
Definition: planner_3d.cpp:217
planner_cspace::planner_3d::Planner3dNode::StartPoseStatus
StartPoseStatus
Definition: planner_3d.cpp:1795
planner_cspace::planner_3d::DistanceMap
Definition: distance_map.h:47
planner_cspace::planner_3d::CostCoeff::hysteresis_max_dist_
float hysteresis_max_dist_
Definition: planner_3d/grid_astar_model.h:63
planner_cspace::planner_3d::JumpDetector::detectJump
bool detectJump()
Definition: jump_detector.h:82
planner_cspace::planner_3d::StartPosePredictor::Config::ang_vel_
double ang_vel_
Definition: start_pose_predictor.h:58
planner_cspace::planner_3d::Planner3dNode::force_goal_orientation_
bool force_goal_orientation_
Definition: planner_3d.cpp:231
planner_cspace::planner_3d::Planner3dNode::keep_a_part_of_previous_path_
bool keep_a_part_of_previous_path_
Definition: planner_3d.cpp:192
planner_cspace::planner_3d::Planner3dNode::makePlan
bool makePlan(const geometry_msgs::Pose &start_metric, const geometry_msgs::Pose &end_metric, nav_msgs::Path &path, bool hyst)
Definition: planner_3d.cpp:1891
planner_cspace::planner_3d::CostCoeff::hysteresis_expand_
float hysteresis_expand_
Definition: planner_3d/grid_astar_model.h:64
planner_cspace::planner_3d::GridAstarModel3D::Ptr
std::shared_ptr< GridAstarModel3D > Ptr
Definition: planner_3d/grid_astar_model.h:76
planner_cspace::planner_3d::Planner3dNode::cm_
Astar::Gridmap< char, 0x40 > cm_
Definition: planner_3d.cpp:132
planner_cspace::planner_3d::DistanceMap::DebugData
Definition: distance_map.h:63
planner_cspace::planner_3d::Planner3dNode::antialias_start_
bool antialias_start_
Definition: planner_3d.cpp:153
actionlib::SimpleActionServer
planner_cspace::planner_3d::Planner3dNode::cost_estim_cache_static_
DistanceMap cost_estim_cache_static_
Definition: planner_3d.cpp:141
planner_cspace::planner_3d::CostCoeff::weight_costmap_
float weight_costmap_
Definition: planner_3d/grid_astar_model.h:57
ROS_ERROR
#define ROS_ERROR(...)
planner_cspace::CyclicVecBase::cycle
void cycle(const int res, const ArgList &... rest)
Definition: cyclic_vec.h:282
planner_cspace::planner_3d::Planner3dNode::prev_map_update_x_max_
int prev_map_update_x_max_
Definition: planner_3d.cpp:243
planner_cspace::planner_3d::Planner3dNode::goal_raw_
geometry_msgs::PoseStamped goal_raw_
Definition: planner_3d.cpp:208
planner_cspace::planner_3d::DistanceMap::gridmap
const Astar::Gridmap< float > & gridmap() const
Definition: distance_map.h:114
planner_cspace::planner_3d::Planner3dNode::goal_tolerance_ang_f_
double goal_tolerance_ang_f_
Definition: planner_3d.cpp:212
planner_cspace::GridAstar< 3, 2 >::Vec
CyclicVecInt< DIM, NONCYCLIC > Vec
Definition: grid_astar.h:67
diagnostic_updater::DiagnosticStatusWrapper::addf
void addf(const std::string &key, const char *format,...)
planner_cspace::planner_3d::DistanceMap::DebugData::search_queue_size
size_t search_queue_size
Definition: distance_map.h:65
planner_cspace::planner_3d::DistanceMap::init
void init(const GridAstarModel3D::Ptr model, const Params &p)
Definition: distance_map.cpp:198
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
tf2::Quaternion
planner_cspace::planner_3d::CostCoeff::in_place_turn_
float in_place_turn_
Definition: planner_3d/grid_astar_model.h:62
diagnostic_updater::DiagnosticStatusWrapper
planner_cspace::planner_3d::Planner3dNode::cnt_stuck_
int cnt_stuck_
Definition: planner_3d.cpp:235
tf2_geometry_msgs.h
tf2::toMsg
B toMsg(const A &a)
planner_cspace::planner_3d::Planner3dNode::getSwitchIndex
int getSwitchIndex(const nav_msgs::Path &path) const
Definition: planner_3d.cpp:2277
planner_cspace::planner_3d::Planner3dNode::sub_goal_
ros::Subscriber sub_goal_
Definition: planner_3d.cpp:107
planner_cspace::planner_3d::Planner3dNode::ec_
Astar::Vecf ec_
Definition: planner_3d.cpp:210
planner_cspace::planner_3d::Planner3dNode::srs_make_plan_
ros::ServiceServer srs_make_plan_
Definition: planner_3d.cpp:122
trajectory_tracker_msgs::toPathWithVelocity
PathWithVelocity toPathWithVelocity(const nav_msgs::Path &src, const double vel)
planner_cspace::planner_3d::Planner3dNode::sw_wait_
float sw_wait_
Definition: planner_3d.cpp:225
ros::Rate
planner_cspace::planner_3d::DistanceMap::setParams
void setParams(const CostCoeff &cc, const int num_cost_estim_task)
Definition: distance_map.cpp:192
planner_cspace::planner_3d::Planner3dNode::parameter_server_
dynamic_reconfigure::Server< Planner3DConfig > parameter_server_
Definition: planner_3d.cpp:129
planner_cspace::SearchStats
Definition: grid_astar.h:55
planner_cspace::planner_3d::Planner3dNode::metrics_
neonavigation_metrics_msgs::Metrics metrics_
Definition: planner_3d.cpp:222
planner_cspace::planner_3d::Planner3dNode::cm_base_
Astar::Gridmap< char, 0x40 > cm_base_
Definition: planner_3d.cpp:134
planner_cspace::planner_3d::Planner3dNode::arrivable_map_
DistanceMap arrivable_map_
Definition: planner_3d.cpp:142
planner_cspace::planner_3d::DistanceMap::Rect
Definition: distance_map.h:52
planner_cspace::planner_3d::Planner3dNode::pub_metrics_
ros::Publisher pub_metrics_
Definition: planner_3d.cpp:120
ros::Duration::sleep
bool sleep() const
planner_cspace::planner_3d::Planner3dNode::goal_tolerant_
planner_cspace_msgs::MoveWithToleranceGoalConstPtr goal_tolerant_
Definition: planner_3d.cpp:126
planner_cspace::planner_3d::Planner3dNode::grid2MetricPose
geometry_msgs::Pose grid2MetricPose(const Astar::Vec &grid) const
Definition: planner_3d.cpp:320
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
planner_cspace::planner_3d::Planner3dNode::updateTemporaryEscapeGoal
void updateTemporaryEscapeGoal(const Astar::Vec &start_grid, const bool log_on_unready=true)
Definition: planner_3d.cpp:2089
DurationBase< Duration >::toSec
double toSec() const
planner_cspace::planner_3d::Planner3dNode::sub_map_update_
ros::Subscriber sub_map_update_
Definition: planner_3d.cpp:106
planner_cspace::CyclicVecBase::sqlen
T sqlen() const
Definition: cyclic_vec.h:240
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
planner_cspace::planner_3d::StartPosePredictor::Config
Definition: start_pose_predictor.h:52
planner_cspace::planner_3d::CostCoeff::weight_hysteresis_
float weight_hysteresis_
Definition: planner_3d/grid_astar_model.h:61
planner_cspace::planner_3d::Planner3dNode::fast_map_update_
bool fast_map_update_
Definition: planner_3d.cpp:179
planner_cspace::planner_3d::Planner3dNode::is_path_switchback_
bool is_path_switchback_
Definition: planner_3d.cpp:227
planner_cspace::planner_3d::Planner3dNode::cost_estim_cache_created_
bool cost_estim_cache_created_
Definition: planner_3d.cpp:177
planner_cspace::planner_3d::StartPosePredictor::getPreservedPath
const nav_msgs::Path & getPreservedPath() const
Definition: start_pose_predictor.h:71
planner_cspace::planner_3d::Planner3dNode::relocateDiscretePoseIfNeeded
DiscretePoseStatus relocateDiscretePoseIfNeeded(Astar::Vec &pose_discrete, const int tolerance_range, const int tolerance_angle, bool use_cm_rough=false) const
Definition: planner_3d.cpp:294
tf2::TransformException
ROS_INFO
#define ROS_INFO(...)
planner_cspace::GridAstar::reset
void reset(const Vec size)
Definition: grid_astar.h:147
planner_cspace::planner_3d::CostmapBBF::Ptr
std::shared_ptr< CostmapBBF > Ptr
Definition: costmap_bbf.h:47
planner_cspace::planner_3d::Planner3dNode::relocateDiscretePoseIfNeededImpl
DiscretePoseStatus relocateDiscretePoseIfNeededImpl(const T &cm, const int tolerance_range, const int tolerance_angle, Astar::Vec &pose_discrete) const
Definition: planner_3d.cpp:276
diagnostic_updater::DiagnosticTaskVector::add
void add(const std::string &name, TaskFunction f)
planner_cspace::GridAstar::setQueueSizeLimit
void setQueueSizeLimit(const size_t size)
Definition: grid_astar.h:164
planner_cspace::planner_3d::grid_metric_converter::appendGridPath2MetricPath
void appendGridPath2MetricPath(const costmap_cspace_msgs::MapMetaData3D &map_info, const STL_CONTAINER< CyclicVecFloat< 3, 2 >, std::allocator< CyclicVecFloat< 3, 2 >>> &path_grid, nav_msgs::Path &path)
Definition: grid_metric_converter.h:82
planner_cspace::planner_3d::Planner3dNode::enable_crowd_mode_
bool enable_crowd_mode_
Definition: planner_3d.cpp:194
planner_cspace::planner_3d::Planner3dNode::num_task_
int num_task_
Definition: planner_3d.cpp:201
planner_cspace::planner_3d::Planner3dNode::buildStartPoses
StartPoseStatus buildStartPoses(const geometry_msgs::Pose &start_metric, const geometry_msgs::Pose &end_metric, std::vector< Astar::VecWithCost > &result_start_poses)
Definition: planner_3d.cpp:1801
planner_cspace::planner_3d::Planner3dNode::publishDebug
void publishDebug()
Definition: planner_3d.cpp:665
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
planner_cspace::planner_3d::Planner3dNode::publishEmptyPath
void publishEmptyPath()
Definition: planner_3d.cpp:744
ros::Duration
planner_cspace::planner_3d::Planner3dNode::goal_tolerance_ang_
int goal_tolerance_ang_
Definition: planner_3d.cpp:215
planner_cspace::planner_3d::Planner3dNode::cm_rough_
Astar::Gridmap< char, 0x80 > cm_rough_
Definition: planner_3d.cpp:133
planner_cspace::planner_3d::Planner3dNode::act_tolerant_
std::shared_ptr< Planner3DTolerantActionServer > act_tolerant_
Definition: planner_3d.cpp:125
planner_cspace::planner_3d::JumpDetector::setBaseFrame
void setBaseFrame(const std::string &frame_id)
Definition: jump_detector.h:73
ros::Timer
planner_cspace::planner_3d::Planner3dNode::Planner3DTolerantActionServer
actionlib::SimpleActionServer< planner_cspace_msgs::MoveWithToleranceAction > Planner3DTolerantActionServer
Definition: planner_3d.cpp:101
planner_cspace::planner_3d::Planner3dNode::pub_status_
ros::Publisher pub_status_
Definition: planner_3d.cpp:119
planner_cspace::planner_3d::Planner3dNode::cbParameter
void cbParameter(const Planner3DConfig &config, const uint32_t)
Definition: planner_3d.cpp:1388
planner_cspace::planner_3d::Planner3dNode::tfl_
tf2_ros::TransformListener tfl_
Definition: planner_3d.cpp:128
planner_cspace::planner_3d::DistanceMap::Params::euclid_cost
Astar::Vecf euclid_cost
Definition: distance_map.h:72
tf2_ros::Buffer::lookupTransform
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout) const
planner_cspace::planner_3d::Planner3dNode::srs_forget_
ros::ServiceServer srs_forget_
Definition: planner_3d.cpp:121
planner_cspace::planner_3d::Planner3dNode::Planner3dNode
Planner3dNode()
Definition: planner_3d.cpp:1154
planner_cspace::planner_3d::Planner3dNode::previous_path_
nav_msgs::Path previous_path_
Definition: planner_3d.cpp:246
ros::NodeHandle
ros::Subscriber
planner_cspace::planner_3d::StartPosePredictor::Config::dist_stop_
double dist_stop_
Definition: start_pose_predictor.h:56
planner_cspace::planner_3d::GridAstarModel2D::Ptr
std::shared_ptr< GridAstarModel2D > Ptr
Definition: planner_3d/grid_astar_model.h:140
planner_cspace::planner_3d::CostmapBBFNoOp
Definition: costmap_bbf.h:105
planner_cspace::planner_3d::CostCoeff::weight_backward_
float weight_backward_
Definition: planner_3d/grid_astar_model.h:55
planner_cspace::planner_3d::Planner3dNode::remember_hit_odds_
float remember_hit_odds_
Definition: planner_3d.cpp:187
planner_cspace::planner_3d::Planner3dNode::StartPoseStatus::START_OCCUPIED
@ START_OCCUPIED
planner_cspace::planner_3d::Planner3dNode::pub_start_
ros::Publisher pub_start_
Definition: planner_3d.cpp:116
planner_cspace::CyclicVecBase
Definition: cyclic_vec.h:78
ros::Time::now
static Time now()
planner_cspace::planner_3d::Planner3dNode::local_range_f_
double local_range_f_
Definition: planner_3d.cpp:156

planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Fri May 16 2025 02:15:23