Drone: Planner Node - rollthecloudinc/hedge GitHub Wiki

PlannerNode Documentation

Overview

PlannerNode is a ROS 2 node designed for path planning in robotic systems. It generates a path from the robot's current position to a target position while optionally incorporating basic obstacle avoidance. The node supports straight-line path generation or obstacle-aware path generation using a simple greedy algorithm. It relies on input from position, target, and obstacle data sources and publishes the computed path as a nav_msgs/Path.

Features

  • Straight-line or obstacle-avoiding path generation
  • Supports configurable waypoint resolution
  • Uses transformation buffers for tracking poses in different coordinate frames
  • Publishes planned paths for downstream consumption
  • Dynamically adjustable waypoint resolution through ROS 2 services
  • Integration with sensor data (e.g., LiDAR via LaserScan) for obstacle detection

Node Name

planner_node

Parameters

  1. waypoint_resolution (default: 0.5)
    The resolution (in meters) between consecutive waypoints in the generated path.

    • Type: float
    • Purpose: Determines granularity of the path.

  2. frame_id (default: 'map')
    The coordinate frame in which the path is generated and processed.

    • Type: string

  3. obstacle_avoidance_enabled (default: True)
    Enables or disables obstacle avoidance during path generation.

    • Type: bool

Subscribed Topics

  1. /mavros/local_position/pose (geometry_msgs/PoseStamped)
    Provides the current position (start pose) of the robot.

  2. /target_pose (geometry_msgs/PoseStamped)
    Provides the desired target position for the robot to navigate to. Triggers path generation whenever a new target is received.

  3. /scan (sensor_msgs/LaserScan, optional)
    Provides obstacle distance data from a LiDAR sensor. This topic is subscribed only if obstacle_avoidance_enabled is set to True.

Published Topics

  1. /planned_path (nav_msgs/Path)
    Publishes the generated path as a collection of waypoints from the current position to the target position.

Services

  1. /update_resolution (std_srvs/SetBool)
    Allows dynamic adjustment of the waypoint resolution parameter.
    • Input: Boolean (True to apply current waypoint_resolution setting).
    • Output: Success or failure message and status.

Class Structure and Key Methods

Class: PlannerNode

The PlannerNode class inherits from rclpy.node.Node and contains all the logic for path planning, obstacle avoidance, and data handling.

__init__()

Description: Initializes the node, declares configurable parameters, sets up publishers, subscribers, and services, and initializes internal state variables.

Key tasks:

  • Declare and read node parameters.
  • Create necessary publishers and subscribers.
  • Initialize transformation buffer (TF).
  • Set up an obstacle array to cache detected obstacles.

local_position_callback(msg)

Description: Handles updates to the robot's position. Sets the start_pose to the robot's current position.

Input:

  • msg (geometry_msgs/PoseStamped): The robot's position data.

Key tasks:

  • Updates the start_pose variable with the received pose.
  • Logs the updated start position.

target_callback(msg)

Description: Handles the receipt of a new target position and triggers path generation.

Input:

  • msg (geometry_msgs/PoseStamped): The target position.

Key tasks:

  • Updates target_pose with the received data.
  • Generates a path (linear or obstacle-aware) if start_pose is initialized.
  • Publishes the generated path to /planned_path.

obstacle_callback(msg)

Description: Updates the set of detected obstacles based on LaserScan data from a LiDAR.

Input:

  • msg (sensor_msgs/LaserScan): Obstacle range data.

Key tasks:

  • Converts laser data from polar to Cartesian coordinates.
  • Stores and updates detected obstacles in self.obstacles.

update_resolution_callback(request, response)

Description: A service callback to dynamically update the waypoint resolution parameter.

Input:

  • request (std_srvs/SetBool.Request): A boolean flag to trigger the update.
  • response (std_srvs/SetBool.Response): Response indicating success/failure.

Key tasks:

  • Updates waypoint_resolution with the current parameter value.
  • Logs success or failure messages.

generate_path_linear(start_pose, target_pose)

Description: Generates a straight-line path between two poses, sampled at the given resolution.

Input:

  • start_pose (geometry_msgs/Pose): The robot's starting position.
  • target_pose (geometry_msgs/Pose): The robot's target position.

Output:

  • A nav_msgs/Path object representing the planned path.

Key tasks:

  • Computes waypoints by interpolating between start_pose and target_pose.
  • Appends interpolated poses into the path object.

generate_path_with_obstacle_avoidance(start_pose, target_pose)

Description: Generates a path to avoid obstacles using a simple greedy algorithm.

Input:

  • start_pose (geometry_msgs/Pose): The robot's starting position.
  • target_pose (geometry_msgs/Pose): The robot's target position.

Output:

  • A nav_msgs/Path object representing the planned path.

Key tasks:

  • Iteratively computes waypoints while avoiding detected obstacles.
  • Employs collision checking based on detected obstacle positions.
  • Uses a rudimentary re-routing mechanism when an obstacle is encountered.

euclidean_distance(point1, point2)

Description: Calculates the straight-line distance between two 3D points.

Input:

  • point1, point2: (geometry_msgs/Point) Points to compute the distance between.

Output:

  • Distance (float).

Main Entry Point: main()

Description: Initializes the ROS 2 system, instantiates the PlannerNode, and starts the spinning loop. Cleans up resources upon termination.

Behavior Summary

  1. Initialization: The node initializes publishers, subscribers, parameters, and the TF listener.
  2. Runtime:
    • Subscribes to /mavros/local_position/pose to track the robot's current position.
    • Subscribes to /target_pose to receive the target destination and plan the path.
    • Subscribes to /scan (optional) to receive obstacle data and adjust the path accordingly.
    • Publishes planned paths to /planned_path.
    • Dynamically updates waypoint resolution using the /update_resolution service.
  3. Shutdown: Stops spinning and cleans up resources once terminated.

Example Usage

Launch the node:

ros2 run <your_package_name> planner_node

Set parameters via YAML configuration:

planner_node:
  ros__parameters:
    waypoint_resolution: 0.3
    frame_id: "base_link"
    obstacle_avoidance_enabled: True

Interact with the service to update resolution:

ros2 service call /update_resolution std_srvs/srv/SetBool "{data: true}"

This documentation provides a clear and detailed overview of the PlannerNode for developers, making it easier to understand, configure, and interact with the node.