planner_cspace package

The topic names will be migrated to ROS recommended namespace model. Set /neonavigation_compatible parameter to 1 to use new topic names.

planner_3d

planner_3d node provides 2-D/3-DOF seamless global-local path and motion planner.

Subscribed topics

• ~/costmap (new: costmap) [costmap_cspace_msgs::CSpace3D]
• ~/costmap_update (new: costmap_update) [costmap_cspace_msgs::CSpace3DUpdate]
• ~/goal (new: move_base_simple/goal) [geometry_msgs::PoseStamped]
• /tf

Published topics

• ~/path (new: path) [nav_msgs::Path]
• ~/debug [sensor_msgs::PointCloud]

  debug output of planner internal costmap

• ~/remembered [sensor_msgs::PointCloud]

  debug output of obstacles probability estimated by BBF

• ~/path_start [geometry_msgs::PoseStamped]
• ~/path_end [geometry_msgs::PoseStamped]
• ~/status [planner_cspace_msgs::PlannerStatus]

Services

• ~/forget (new: forget_planning_cost) [std_srvs::Empty]

Called services

Parameters

• “goal_tolerance_lin” (double, default: 0.05)
• “goal_tolerance_ang” (double, default: 0.1)
• “goal_tolerance_ang_finish” (double, default: 0.05)
• “unknown_cost” (int, default: 100)
• “hist_cnt_max” (int, default: 20)
• “hist_cnt_thres” (int, default: 19)
• “hist_cost” (int, default: 90)
• “hist_ignore_range” (double, default: 1.0)
• “remember_updates” (bool, default: false)
• “local_range” (double, default: 2.5)
• “longcut_range” (double, default: 0.0)
• “esc_range” (double, default: 0.25)
• “find_best” (bool, default: true)
• “pos_jump” (double, default: 1.0)
• “yaw_jump” (double, default: 1.5)
• “jump_detect_frame” (string, default: base_link)
• “force_goal_orientation” (bool, default: true)
• “temporary_escape” (bool, default: true)
• “fast_map_update” (bool, default: false)
• “debug_mode” (string, default: std::string(“cost_estim”))

  debug output data type

  • “hyst”: path hysteresis cost
  • “cost_estim”: estimated cost to the goal used as A* heuristic function

• “queue_size_limit” (int, default: 0)
• “antialias_start” (bool, default: false)

  If enabled, the planner searches path from multiple surrounding grids within the grid size to reduce path chattering.

---

planner_2dof_serial_joints

planner_2dof_serial_joints provides collision avoidance for 2-DOF serial joint (e.g. controlling interfering pairs of sub-tracks for tracked vehicle.)

Subscribed topics

• ~/trajectory_in (new: trajectory_in) [trajectory_msgs::JointTrajectory]
• ~/joint (new: joint_states) [sensor_msgs::JointState]
• /tf

Published topics

• ~/trajectory_out (new: joint_trajectory) [trajectory_msgs::JointTrajectory]
• ~/status [planner_cspace_msgs::PlannerStatus]

Services

Called services

Parameters

• “resolution” (int, default: 128)
• “debug_aa” (bool, default: false)
• “replan_interval” (double, default: 0.2)
• “queue_size_limit” (int, default: 0)
• “link0_name” (string, default: std::string(“link0”))
• “link1_name” (string, default: std::string(“link1”))
• “point_vel_mode” (string, default: std::string(“prev”))
• “range” (int, default: 8)
• “num_groups” (int, default: 1)

---

dummy_robot

stub

---

patrol

stub

Changelog for package planner_cspace

0.18.0 (2025-05-13)

• Ignore temporary escape trigger if map is not received (#770)
• planner_cspace: crowd escape mode (#763)
• planner_cspace: fix flaky navigation tests (#768)
• Contributors: Atsushi Watanabe

0.17.7 (2025-04-16)

0.17.6 (2025-03-18)

• planner_cspace: fix flaky dynamic reconfigure test (#756)
• Contributors: Atsushi Watanabe

0.17.5 (2025-02-21)

• planner_cspace: reconstuct GridAstarModel3D only when necessary (#751)
• Contributors: Naotaka Hatao

0.17.4 (2025-01-20)

0.17.3 (2024-12-25)

• planner_cspace: add costmap_cspace integration test for out-of-bound position (#746)
• costmap_cspace: publish empty costmap_update on out-of-bound (#743)
• planner_cspace: reduce remember_updates calculation cost (#742)
• Contributors: Atsushi Watanabe

0.17.2 (2024-11-07)

• planner_3d: apply turn_penalty_cost_threshold in curve path (#739)
• Contributors: Naotaka Hatao

0.17.1 (2024-03-22)

• planner_3d: remove duplicated interpolation method (#733)
• Contributors: Naotaka Hatao

0.17.0 (2023-11-02)

• planner_3d: add an option to trigger planning by costmap updates (#727)
• planner_3d: fix cost calculation bug on in-place turning (#725)
• Contributors: Naotaka Hatao

0.16.0 (2023-09-14)

• planner_cspace: start planning from expected robot pose (#717)
• planner_cspace: add new parameters for cost function (#720)
• planner_cspace: refactor start pose building algorithm and fix finishing condition when start is relocated (#718)
• Contributors: Naotaka Hatao

0.15.0 (2023-08-30)

• planner_cspace: enable dynamic reconfigure (#714)
• planner_space: fix segmentation fault on out-of-map (#712)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.14.2 (2023-07-31)

• planner_cspace: improve peformance of DistanceMap (#709)
• planner_cspace: avoid multiple goal pose relocation (#708)
• Contributors: Naotaka Hatao

0.14.1 (2023-07-07)

• planner_cspace: fix error status handling after cost_estim_cache_ was not created (#705)
• Contributors: Naotaka Hatao

0.14.0 (2023-06-06)

• planner_cspace: publish planner_3d metrics (#701)
• Contributors: Atsushi Watanabe

0.13.0 (2023-05-31)

• planner_cspace: fix fast map update (#692)
• planner_cspace: fix status when goal or robot is in rock on updating goal (#694)
• planner_cspace: fix string template constexpr in test (#685)
• Contributors: Atsushi Watanabe

0.12.2 (2023-02-28)

• planner_cspace: fix planner_2dof_serial_joints status (#682)
• planner_cspace: set timestamp to planner status message (#681)
• planner_cspace: fix uninitialized variables in DistanceMap (#679)
• planner_cspace: add scoped trace to some test cases (#680)
• Contributors: Atsushi Watanabe

0.12.1 (2023-02-25)

• Improve test logs on timeout (#673)
• planner_cspace: fix map update range (#672)
• planner_cspace: clear velocity of dummy_robot on position set (#669)
• planner_cspace: fix flaky action tests (#665)
• planner_cspace: fix planner_cspace_msgs version constraint (#663)
• Contributors: Atsushi Watanabe

0.12.0 (2023-01-30)

• planner_cspace: add continuous movement mode to MoveWithTolerance (#652)
• Contributors: Naotaka Hatao

0.11.8 (2022-12-28)

• planner_cspace: add missing array include (#658)
• Contributors: Atsushi Watanabe

0.11.7 (2022-08-05)

• planner_cspace: fix pose duplication on path interpolation (#648)
• Contributors: Atsushi Watanabe

0.11.6 (2022-07-20)

• planner_cspace: add more test case to Planner3DMapSize (#640)
• Contributors: Atsushi Watanabe

0.11.5 (2022-07-06)

• planner_cspace: fix access violation on wrong size costmap_update message (#637)
• Contributors: Atsushi Watanabe

0.11.4 (2022-04-13)

0.11.3 (2021-12-02)

• planner_cspace: fix DistanceMap::init() (#625)
• Fix flaky tests and add debug outputs (#628)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.11.2 (2021-11-08)

• planner_cspace: Fix header of fihish path (#622)
• Contributors: Kazuki Takahashi

0.11.1 (2021-10-29)

• planner_cspace: refactor distance map generator (#617)
• planner_cspace: recover error on FINISHING state (#615)
• planner_cspace: fix flaky actionlib tests (#616)
• planner_cspace: remove unused variable (#614)
• Contributors: Atsushi Watanabe

0.11.0 (2021-08-30)

• Apply clang-format-11 with new setting (#605)
• Contributors: Naotaka Hatao

0.10.11 (2021-06-21)

• planner_cspace: reset next_replan_time after waitUntil() (#602)
• Contributors: Kazuki Takahashi

0.10.10 (2021-03-18)

• planner_cspace: fix goal unreachable condition (#595)
• Contributors: Atsushi Watanabe

0.10.9 (2021-03-16)

• planner_cspace: abort A* search on continuous timeout (#592)
• Contributors: Atsushi Watanabe

0.10.8 (2021-03-10)

• planner_cspace: improve performance of costmap reset (#587)
• Contributors: Naotaka Hatao

0.10.7 (2021-03-07)

• planner_cspace: improve performance of hysteresis clearing (#586)
• Contributors: Naotaka Hatao

0.10.6 (2021-01-28)

• planner_cspace: fix condition of open queue (#576)
• planner_cspace: add debug output about cost_estim_cache update (#577)
• planner_cspace: reuse open/erase queue (#575)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.10.5 (2020-12-24)

• planner_cspace: enable tolerance in make_plan (#570)
• Contributors: Naotaka Hatao

0.10.4 (2020-11-12)

0.10.3 (2020-10-22)

0.10.2 (2020-10-07)

0.10.1 (2020-08-26)

• planner_cspace: avoid publishing invalid path when escaping (#546)
• Contributors: Naotaka Hatao

0.10.0 (2020-08-06)

• planner_cspace: add test to MoveWithToleranceAction (#528)
• planner_cspace: add test to distance map debug output (#526)
• planner_cspace: periodically update local map in the test (#522)
• Merge rostest coverage profiles (#520)
• planner_cspace: fix search range of minimum cost in fast_map_update mode (#518)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.9.1 (2020-07-16)

0.9.0 (2020-07-02)

0.8.8 (2020-06-15)

• planner_cspace: avoid showing too many warning messages (#501)
• Contributors: Naotaka Hatao

0.8.7 (2020-05-22)

0.8.6 (2020-05-15)

• Fix duplicated tf timestamp (#494)
• planner_cspace: add wait to navigation tests (#492)
• planner_cspace: simplify path switch detection condition (#488)
• planner_cspace: fix uninitialized variable (#486)
• planner_cspace: enable replan when robot reaches the switchback point (#449)
• planner_cspace: fix test_debug_outputs initial wait (#485)
• Contributors: Atsushi Watanabe, Kazuki Takahashi

0.8.5 (2020-05-04)

0.8.4 (2020-04-30)

• Clean unused dependencies (#472)
• Contributors: Atsushi Watanabe

0.8.3 (2020-04-26)

0.8.2 (2020-04-07)

• Support Noetic (#461)
• Contributors: Atsushi Watanabe

0.8.1 (2020-03-12)

• planner_cspace: fix flaky debug_output test (#452)
• planner_cspace: fix condition of planning finish (#451)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.8.0 (2020-03-04)

• planner_cspace: replan immediately when path is blocked by new obstacles (#446)
• Add message package version constraints (#443)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.7.0 (2020-02-04)

• planner_cspace: add MoveWithToleranceAction server (#433)
• planner_cspace: fix typo (#436)
• planner_cspace: implement motion primitive algorithm for speed-up (#431)
• Contributors: Daiki Maekawa, Naotaka Hatao

0.6.0 (2020-01-18)

0.5.1 (2020-01-06)

• planner_cspace: disable blockmem_gridmap_performance test (#413)
• Fix header namespaces (#408)
• planner_cspace: fix installing planner_2dof_serial_joints node (#409)
• Migrate from C math functions to C++ (#407)
• planner_cspace: split search model definition (#323)
• planner_cspace: fix debug output test (#404)
• planner_cspace: fix navigation test stability (#403)
• planner_cspace: add planner_2dof_serial_joints node test (#402)
• Contributors: Atsushi Watanabe

0.5.0 (2019-10-21)

• planner_cspace: fix debug output test stability (#399)
• planner_cspace: publish internally used maps as OccupancyGrid (#396)
• planner_cspace: clear hysteresis if new obstacle is on the previous path (#393)
• planner_cspace: fix remember_updates feature (#391)
• Contributors: Atsushi Watanabe

0.4.3 (2019-09-10)

• planner_cspace: make sure that planner error will be cleared if the goal is aborted (#372)
• Contributors: Daiki Maekawa

0.4.2 (2019-08-19)

• planner_cspace: fix planner performance (#369)
• Contributors: Atsushi Watanabe

0.4.1 (2019-08-15)

• planner_cspace: fix debug build compatibility (#368)
• planner_cspace: fix out-of-boundary validation (#362)
• planner_cspace: fix incomplete output path after search timeout (#357)
• planner_cspace: reduce position quantization error on planning (#351)
• planner_cspace: latch publish data in navigation test (#353)
• planner_cspace: improve grid search performance (#342)
• planner_cspace: optimize BlockmemGridmap (#315)
• planner_cspace: add a launch for planner performance evaluation (#343)
• planner_cspace: fix parallel memory access (#306)
• planner_cspace: remove hist mode of debug output (#336)
• planner_cspace: fix navigation test setup (#335)
• planner_cspace: add a navigation test case with map update (#334)
• planner_cspace: add const to the end pos (#332)
• planner_cspace: reject request if input frame are located at diffrent frame to the map (#327)
• planner_cspace: publish empty path immediately after planning aborted (#326)
• planner_cspace: revert default sw_wait parameter (#313)
• Drop ROS Indigo and Ubuntu Trusty support (#310)
• planner_cspace: calculate path hysteresis in 3-DOF space (#304)
• Fix include directory priority (#308)
• planner_cspace: fix CyclicVector dimension of planner_2dof_serial_joints (#307)
• planner_cspace, costmap_cspace: minor refactoring (#305)
• Fix empty path publish (#301)
• planner_cspace: refactor CyclicVec (#300)
• planner_cspace: refactor rotation cache (#299)
• planner_cspace: fix path cost calculation and interpolation (#298)
• Contributors: Atsushi Watanabe, Daiki Maekawa, Yuta Koga

0.4.0 (2019-05-09)

• planner_cspace: limit negative cost to avoid infinite search loop (#288)
• trajectory_tracker: remove unused parameters (#274)
• Support melodic (#266)
• Contributors: Atsushi Watanabe, Yuta Koga

0.3.1 (2019-01-10)

• trajectory_tracker: support PathWithVelocity (#244)
• planner_cspace: fix stability of test_costmap_watchdog (#242)
• planner_cspace: add watchdog to costmap update (#235)
• planner_cspace: add missing test dependencies (#234)
• Fix pointer alignment style (#233)
• Migrate tf to tf2 (#230)
• planner_cspace: add diagnostics to planner node (#226)
• planner_cspace: stop robot motion if new map received (#218)
• planner_cspace: split grid-metric converter functions (#213)
• planner_cspace: split motion cache class (#212)
• planner_cspace: fix goal and start tolerance parameter (#211)
• planner_cspace: add cost for turning near obstacles (#210)
• Fix catkin package definitions (#206)
• planner_cspace: use odometry position difference in jump detection (#205)
• planner_cspace: refactoring (#204)
• Contributors: Atsushi Watanabe, So Jomura, Yuta Koga

0.2.3 (2018-07-19)

• Fix test names (#202)
• Contributors: Atsushi Watanabe

0.2.2 (2018-07-17)

0.2.1 (2018-07-14)

• Fix missing package dependencies (#194)
• Contributors: Atsushi Watanabe

0.2.0 (2018-07-12)

• planner_cspace: fix restriction of path segment connection (#191)
• planner_cspace: fix boundary check (#190)
• planner_cspace: fix unconverged switching back vibration (#183)
• Reduce random test failure (#181)
• Update CI (#179)
• Fix cost in heuristic function for make_plan service (#178)
• Fix namespace migration messages (#174)
• planner_cspace: add make plan service (#169)
• Fix topic/service namespace model (#168)
• Fix package dependencies (#167)
• Fix naming styles (#166)
• Update package descriptions and unify license and version (#165)
• Use neonavigation_msgs package (#164)
• planner_cspace: fix clearing remembered costmap (#158)
• planner_cspace: fix partial costmap update with unknown cells (#156)
• planner_cspace: remember costmap using binary bayes filter (#149)
• planner_cspace: fix position jump detection (#150)
• planner_cspace: fix remembering costmap (#147)
• planner_cspace: use frame_id of incoming message to set dummy robot pose (#145)
• planner_cspace: add odom publisher to dummy robot (#143)
• planner_cspace: add preempt (#137)
• planner_cspace: minor optimizations (#129)
• planner_cspace: disable performance test by default (#127)
• planner_cspace: support parallel distance map search (#125)
• planner_cspace: support parallel aster search (#118)
• Add abort (#116)
• planner_cspace: increase navigation test time limit (#98)
• planner_cspace: validate goal position. (#90)
• Suppress compile warnings and test with -Werror. (#82)
• Fix header of empty path. (#79)
• planner_cspace: cache motion interpolation. (#75)
• planner_cspace: add planning performance test. (#74)
• planner_cspace: add navigation integration test. (#73)
• planner_cspace: add test for cyclic_vec. (#72)
• planner_cspace: fix naming styles in blockmem_gridmap. (#69)
• planner_cspace: add test for blockmem_gridmap. (#70)
• planner_cspace: install patrol actionlib client. (#64)
• planner_cspace: initialize dummy robot status. (#62)
• planner_cspace: add simple action client for robot patrol. (#61)
• planner_cspace: add missing dependency to boost::chrono. (#60)
• planner_cspace: add actionlib support. (#58)
• neonavigation_launch, planner_cspace: add simple simulator. (#59)
• planner_space: fix naming styles. (#57)
• planner_cspace: refactor separating classes. (#55)
• planner_cspace: fix distance map init timing. (#53)
• Remove dummy dep to system_lib. (#51)
• Support package install. (#45)
• Fix coding styles. (#39)
• planner_cspace: fixes ignore range handling (#28)
• planner_cspace: fixes memory leak on remembered costmap (#27)
• planner_cspace: adds service to forget remembered costmap (#26)
• planner_cspace: fixes logic of remember_update parameter (#25)
• planner_cspace: fixes wrong direction of path end (#24)
• planner_cspace: fixes straight motion discriminant (#23)
• adds READMEs (#11)
• costmap_cspace, planner_cspace: fixes pkg dependencies
• planner_cspace: adds planner for 2dof serial joints (#6)
• planner_cspace: uses template to specify dimension
• changes planner and costmap package names with a postfix _cspace
• Contributors: Atsushi Watanabe, Yuta Koga, Yutaka Takaoka