PALP — Perception-Aware Local Planning

Perception-aware full body trajectory planning for ground based robots that can orient their camera independently from their direction of travel, e.g. planetary rovers with an actuated pan/tilt camera unit.

This repository contains the code for the IROS 2024 paper Perception-aware Full Body Trajectory Planning for Autonomous Systems using Motion Primitives:

Many robotic systems rely on visual sensing to accomplish simultaneously the tasks of state estimation, mapping, and path planning. One one hand, the usage of camera sensors represents a power-efficient and lightweight option for solving this problem. On the other hand, these tasks pose requirements on the quality of the visual input (e.g. number of tracked features for Visual Odometry) that are often in contrast to the optimal viewpoint planning for local mapping and obstacle avoidance. Dealing with this constraint is actively researched in the field of perception-aware planning. The approaches delivered by this field mostly concern Micro air vehicles (MAVs), but could be applied to a larger group of robotic systems. We propose a perception-aware trajectory planner for a class of robotic systems that can orient their cameras independently from their direction of travel. By using motion primitives, our planner does not require differentiable models for motion and perception objectives. We evaluate our method in simulation, showing increased capabilities in localization-aware motions around obstacles, and demonstrate its run-time capability on a real planetary rover. The code is released publicly under github.com/DLR-RM/palp.

What the algorithm does

The planner runs in a receding horizon fashion, like the classic dynamic window approach (DWA) it is built on. In every planning cycle it:

1. Samples body motion primitives — candidate trajectories of the robot body over a short horizon, generated from the currently admissible velocities.
2. Discards colliding candidates — each body trajectory is checked against the local obstacle cost map with the robot's full footprint; trajectories in collision are rejected.
3. Augments the survivors with camera motion primitives — a set of pan/tilt motions is added to every collision-free body trajectory, yielding full body trajectories: combined states of robot body and camera orientation.
4. Scores every full body trajectory with a sum of barrier-shaped cost functions: progress towards the goal, distance to obstacles, alignment of camera and direction of travel, and — the perception objective — co-visibility of the visual features that the on-board visual odometry tracks for state estimation. The features are aggregated in a robot-centric 3D feature map built from the camera's corner detections.
5. Executes the best candidate — the body velocity and pan/tilt command of the cheapest trajectory are sent to the robot, and the cycle repeats.

Because both motion and perception objectives are evaluated on sampled primitives, none of the models need to be differentiable — cost functions and trajectory generators can be swapped to match other robots and objectives.

Repository layout

Package	Content
palp_trajectory_planning	The planner itself: C++ libraries (pure algorithm + ROS runtime layer), the planner nodes and their regression tests.
palp_visualization	Python plotting utilities that visualize the bagfiles written by the palp_trajectory_planning tests (plot-driven development). Deliberately quick-and-dirty 💩 research tooling that was never meant to outlive the research project — see its README.
rmc_control_msgs	Vendored message/action definitions (PantiltCmd, JointTrajectories, follow_path action).
rmc_local_mapping	Vendored header-only cost value definitions shared with the local mapping side.

How the code is modularized

The planner started from the ROS 1 navigation stack: the relevant parts of costmap_2d, base_local_planner and dwa_local_planner were copied into palp_trajectory_planning and refactored so that the ROS runtime (node handles, publishers, subscribers, parameter handling) is separated from the planning implementation. The perception-aware full body planning layer was then added on top of the refactored DWA planner. This is reflected in the two libraries the package builds:

• palp_trajectory_planning_core — the algorithm: trajectory generators and search, cost functions, cost map and feature map models. Uses ROS message types and logging, but has no ROS runtime dependency.
• palp_trajectory_planning_ros — the runtime layer that feeds the core: ROS wrappers (in the ros/ include subdirectories) that subscribe to inputs, read parameters and publish the planner's outputs.

Files that originate in the navigation stack keep their BSD license headers (Willow Garage, Inc.); see LICENSE.

Interacting with the planner

The perception-aware planner node pa_trajectory_planner offers a follow_path action server (type rmc_control_msgs/follow_pathAction, action name pa_trajectory_planner): the goal is a geometry_msgs/PoseStamped, and the planner drives the robot along the previously received path towards it. The baseline node trajectory_planner (body-only DWA, no perception objectives) offers the same interface under the action name trajectory_planner.

Subscribed topics:

Topic	Type	Purpose
plan	nav_msgs/Path	Global path to follow.
local_costmap	grid_map_msgs/GridMap	Local obstacle cost map (float cost values as defined in rmc_local_mapping).
pan_tilt_camera/left/corner_positions	sensor_msgs/PointCloud2	Feature (corner) detections of the pan/tilt camera's left stereo image, used to build the 3D feature map.
odom	nav_msgs/Odometry	Robot odometry.

Published topics (main ones):

Topic	Type	Purpose
cmd_vel	geometry_msgs/Twist	Body velocity command.
pan_tilt_cmd	rmc_control_msgs/PantiltCmd	Camera pan/tilt command.
global_plan, local_plan	nav_msgs/Path	Visualization of the followed plan.
full_body_trajectory_marker, all_explored_full_body_trajectory_marker	visualization_msgs/MarkerArray	Visualization of the chosen / all explored full body trajectories.
all_explored_full_body_trajectory	rmc_control_msgs/JointTrajectories	All explored full body trajectories.
feature_map, prepared_features	sensor_msgs/PointCloud2	The 3D feature map (also published by the standalone occupancy_feature_map_3D node).
cost_cloud, trajectory_cloud	sensor_msgs/PointCloud2	Cost function debugging clouds.

tf frames (names are parameters, defaults shown): the planner looks up the robot pose as global_frame → robot_frame (odom → base_link) and the pan/tilt camera mounting as tcp_base_frame/tcp_frame (tcp_base/tcp, where TCP is the tool center point of the pan/tilt unit, i.e. the camera frame; tcp_base is its static mount on the robot body). All node parameters are declared in palp_trajectory_planning/cfg/*.params (rosparam_handler) and can be inspected/tuned via dynamic_reconfigure.

Building and testing

The repository is a set of plain catkin packages; clone it into a catkin workspace and resolve the dependencies declared in the package.xml files (ROS 1, grid_map, octomap, PCL, rosparam_handler, ...):

cd ~/catkin_ws/src
git clone https://github.com/DLR-RM/palp.git
rosdep install --from-paths palp --ignore-src
catkin build

Run the tests in two steps — the palp_trajectory_planning tests write the bagfiles that the palp_visualization tests visualize, so they must finish first (running both in one invocation executes them in parallel):

catkin test palp_trajectory_planning
catkin test --no-deps palp_visualization

or with catkin_make:

catkin_make run_tests_palp_trajectory_planning
catkin_make run_tests_palp_visualization

Tests and plot-driven development

The C++ test suite covers the planner with regression tests on recorded and synthetic scenarios (e.g. a cost map recorded on Mt. Etna and an L-shaped feature wall). While running, the tests write their results — planned trajectories, explored candidates, feature maps, cost maps — as bagfiles to palp_trajectory_planning/test/data/output/.

The downstream palp_visualization package turns those bagfiles into plots (saved to test/data/output/visualization/), so a researcher can visually verify the planner's behavior after every change. This workflow is described in the accompanying paper Addressing the Entry Barrier for Experimentation in Perception-aware Trajectory Planning for Planetary Rovers:

To fully evaluate perception-aware planning methods for planetary rover, we need a platform that takes action commands and captures perception data. Even with suitable hardware and simulation available to us, there exists an "entry barrier" for performing research in active vision, as developing methods with a system-in-the-loop is time intensive. We present our approach for tackling this entry barrier by incrementally moving from toy examples to integration and deployment on real robots. Our approach aims at reducing the overall development complexity by producing intermediate results that are used to validate and evaluate the active algorithm.

Citation

If you use this code in your research, please cite:

@inproceedings{kuhne2024perception,
  author    = {Kuhne, Moritz and Giubilato, Riccardo and Schuster, Martin J. and Roa, Maximo A.},
  title     = {Perception-aware Full Body Trajectory Planning for Autonomous Systems using Motion Primitives},
  booktitle = {2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  year      = {2024},
  pages     = {7765--7772},
  doi       = {10.1109/IROS58592.2024.10801381}
}

For the plot-driven development workflow:

@inproceedings{kuhne2023addressing,
  author    = {Kuhne, Moritz and Giubilato, Riccardo and Leidner, Daniel and Roa, Maximo A.},
  title     = {Addressing the Entry Barrier for Experimentation in Perception-aware Trajectory Planning for Planetary Rovers},
  booktitle = {ICRA 2023 Workshop on Active Methods in Autonomous Navigation},
  address   = {London, UK},
  year      = {2023},
  url       = {https://elib.dlr.de/195631/}
}

License

BSD 3-Clause, see LICENSE. Files derived from the ROS navigation stack retain their original Willow Garage BSD license headers.