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. .. code-block:: text ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │ Depth Camera │────►│ Grid Mapping │────►│ Autonomy │ │ Plugin │ │ Plugin │ │ Plugin │ └──────────────┘ └──────────────┘ └──────────────┘ │ │ │ ▼ ▼ ▼ Depth Images Terrain Heightmap Path Planning (local_grid_map) + Avoidance **Data Flow:** 1. **Depth Camera Plugin** captures depth images from an Intel RealSense camera 2. **Grid Mapping Plugin** processes depth data to generate a terrain heightmap (``local_grid_map``) 3. **Autonomy Plugin** subscribes to this heightmap via the ``heightMapName`` parameter 4. 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 .. code-block:: yaml # 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 ^^^^^^^^^^^^^^ .. code-block:: yaml rate: 100 maxSpeed: 4.0 autonomyMode: true autonomySpeed: 1.0 waypointXYRadius: 1.0 twoWayDrive: false joyToSpeedDelay: 2.0 Parameter Reference ^^^^^^^^^^^^^^^^^^^ Core Settings: .. list-table:: :header-rows: 1 :widths: 25 15 15 45 * - 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: yaml localPlanner: vehicleLength: 1.2 vehicleWidth: 0.5 useTerrainAnalysis: true checkObstacle: true adjacentRange: 3.5 Local Planner Parameters: .. list-table:: :header-rows: 1 :widths: 25 15 15 45 * - 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: yaml pathFollower: lookAheadDis: 0.5 yawRateGain: 2.5 maxYawRate: 90.0 maxAccel: 3.0 Path Follower Parameters: .. list-table:: :header-rows: 1 :widths: 25 15 15 45 * - 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: yaml terrainAnalysis: scanVoxelSize: 0.05 decayTime: 0.5 clearingDis: 8.0 vehicleHeight: 1.5 Terrain Analysis Parameters: .. list-table:: :header-rows: 1 :widths: 25 15 15 45 * - 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 ^^^^^^^^^^ .. list-table:: :header-rows: 1 :widths: 30 30 40 * - 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 ^^^^^^^^^^^ .. list-table:: :header-rows: 1 :widths: 30 30 40 * - Topic - Type - Description * - /Odometry/base - nav_msgs/Odometry - Robot odometry (from FAST-LIO) * - - sensor_msgs/PointCloud2 - Local height map * - /fix - sensor_msgs/NavSatFix - GPS coordinates (optional) * - yaw_offset - std_msgs/Float64 - GPS heading calibration (optional) Services ^^^^^^^^ .. list-table:: :header-rows: 1 :widths: 40 60 * - Service - Description * - planning/set_waypoints - Set mission waypoint list * - planning/get_waypoints - Get current waypoint list Usage ----- Setting Waypoints ^^^^^^^^^^^^^^^^^ Waypoints can be set via: 1. **GUI Map Window**: Click to add waypoints on the map 2. **Service Call**: Programmatically set waypoint list Robot Dimensions ^^^^^^^^^^^^^^^^ Set ``vehicleLength`` and ``vehicleWidth`` to match your robot: .. code-block:: yaml localPlanner: vehicleLength: 1.2 # Your robot length vehicleWidth: 0.5 # Your robot width GPS Navigation ^^^^^^^^^^^^^^ For GPS-based waypoints: 1. Ensure GPS provides ``/fix`` topic 2. Calibrate heading with ``yaw_offset`` 3. Set waypoints using latitude/longitude .. warning:: GPS accuracy affects navigation precision. Use RTK-GPS for best results.