Autonomy Plugin

The Autonomy plugin enables autonomous waypoint navigation with terrain analysis, obstacle avoidance, and intelligent path planning.

Overview

The Autonomy plugin provides:

Waypoint-based navigation
Local path planning with obstacle avoidance
Terrain analysis and traversability assessment
Speed adaptation based on terrain and obstacles
GPS waypoint support

Heightmap-Based Obstacle Avoidance

The Autonomy plugin uses terrain heightmap data for intelligent obstacle avoidance. This heightmap is generated by the Grid Mapping plugin which processes depth camera data.

┌──────────────┐     ┌──────────────┐     ┌──────────────┐
│ Depth Camera │────►│ Grid Mapping │────►│   Autonomy   │
│    Plugin    │     │    Plugin    │     │    Plugin    │
└──────────────┘     └──────────────┘     └──────────────┘
       │                    │                    │
       ▼                    ▼                    ▼
  Depth Images       Terrain Heightmap     Path Planning
                     (local_grid_map)      + Avoidance

Data Flow:

Depth Camera Plugin captures depth images from an Intel RealSense camera
Grid Mapping Plugin processes depth data to generate a terrain heightmap (local_grid_map)
Autonomy Plugin subscribes to this heightmap via the heightMapName parameter
The local planner uses heightmap data to detect obstacles and plan collision-free paths

Configuration:

To enable heightmap-based obstacle avoidance, ensure:

Depth camera plugin is loaded and configured
Grid Mapping plugin is running with proper parameters
Autonomy plugin’s heightMapName matches Grid Mapping’s output topic

# In Autonomy plugin config
heightMapName: local_grid_map  # Must match Grid Mapping output topic

Note

For optimal obstacle avoidance, the Grid Mapping plugin should be configured with appropriate map_resolution and local_map_size to cover the robot’s navigation area.

Configuration

Configuration File

Location: raisin_autonomy_plugin/config/params.yaml

Basic Settings

rate: 100
maxSpeed: 4.0
autonomyMode: true
autonomySpeed: 1.0
waypointXYRadius: 1.0
twoWayDrive: false
joyToSpeedDelay: 2.0

Parameter Reference

Core Settings:

Parameter	Type	Default	Description
rate	double	100	Update frequency (Hz)
maxSpeed	double	4.0	Maximum speed limit (m/s)
autonomyMode	bool	true	Enable autonomous navigation
autonomySpeed	double	1.0	Default cruising speed (m/s)
waypointXYRadius	double	1.0	Distance to consider waypoint reached (m)
twoWayDrive	bool	false	Enable reverse driving
joyToSpeedDelay	double	2.0	Delay before resuming after joystick (s)
heightMapName	string	local_grid_map	Height map topic to use

Local Planner Configuration

localPlanner:
  vehicleLength: 1.2
  vehicleWidth: 0.5
  useTerrainAnalysis: true
  checkObstacle: true
  adjacentRange: 3.5

Local Planner Parameters:

Parameter	Type	Default	Description
vehicleLength	double	1.2	Robot length for collision checking (m)
vehicleWidth	double	0.5	Robot width for collision checking (m)
laserVoxelSize	double	0.05	Obstacle detection resolution (m)
terrainVoxelSize	double	0.2	Terrain mapping resolution (m)
useTerrainAnalysis	bool	true	Enable terrain cost analysis
checkObstacle	bool	true	Enable obstacle collision checking
adjacentRange	double	3.5	Planning horizon distance (m)
heightGapThre	double	0.25	Max allowed ground discontinuity (m)

Path Following Configuration

pathFollower:
  lookAheadDis: 0.5
  yawRateGain: 2.5
  maxYawRate: 90.0
  maxAccel: 3.0

Path Follower Parameters:

Parameter	Type	Default	Description
lookAheadDis	double	0.5	Pure pursuit lookahead distance (m)
yawRateGain	double	2.5	Steering responsiveness gain
maxYawRate	double	90.0	Maximum turning rate (deg/s)
maxAccel	double	3.0	Maximum acceleration (m/s²)
slowDwnDisThre	double	1.8	Distance to start slowing (m)
stopDisThre	double	0.4	Distance to stop at waypoint (m)

Terrain Analysis Configuration

terrainAnalysis:
  scanVoxelSize: 0.05
  decayTime: 0.5
  clearingDis: 8.0
  vehicleHeight: 1.5

Terrain Analysis Parameters:

Parameter	Type	Default	Description
scanVoxelSize	double	0.05	Point cloud voxel size (m)
decayTime	double	0.5	Obstacle memory decay time (s)
clearingDis	double	8.0	Maximum sensor range (m)
vehicleHeight	double	1.5	Robot height for clearance check (m)
minRelZ / maxRelZ	double	-1.5 / 0.5	Valid height range relative to robot (m)

Message Topics

Publishers

Topic	Type	Description
/way_point	geometry_msgs/PointStamped	Current target waypoint
/speed	std_msgs/Float32	Commanded speed
/path	nav_msgs/Path	Planned path visualization
/free_paths	sensor_msgs/PointCloud2	Available path options (debug)
/terrain_map	sensor_msgs/PointCloud2	Terrain elevation map

Subscribers

Topic	Type	Description
/Odometry/base	nav_msgs/Odometry	Robot odometry (from FAST-LIO)
<heightMapName>	sensor_msgs/PointCloud2	Local height map
/fix	sensor_msgs/NavSatFix	GPS coordinates (optional)
yaw_offset	std_msgs/Float64	GPS heading calibration (optional)

Services

Service	Description
planning/set_waypoints	Set mission waypoint list
planning/get_waypoints	Get current waypoint list

Usage

Setting Waypoints

Waypoints can be set via:

GUI Map Window: Click to add waypoints on the map
Service Call: Programmatically set waypoint list

Robot Dimensions

Set vehicleLength and vehicleWidth to match your robot:

localPlanner:
  vehicleLength: 1.2  # Your robot length
  vehicleWidth: 0.5   # Your robot width