Skip to main content

Nav2: Path Planning for Bipedal Humanoid Movement

Introduction to Navigation 2 (Nav2)​

Navigation 2 (Nav2) is the next-generation navigation stack for ROS 2, designed to provide advanced path planning, obstacle avoidance, and navigation capabilities for various robot types. While traditionally used for wheeled robots, Nav2 can be adapted for more complex robot types including bipedal humanoids. This section focuses on leveraging Nav2's capabilities for humanoid navigation with specialized considerations for bipedal locomotion.

Key Features of Nav2​

Behavior Trees​

  • Declarative approach to navigation behaviors
  • Modular and composable navigation tasks
  • Runtime reconfiguration of navigation flows
  • Integration with AI planning systems

Costmap 2D​

  • 2D costmap representation for obstacle avoidance
  • Layered approach for different sensor inputs
  • Dynamic obstacle inflation and clearance
  • 3D extension capabilities

Path Planners​

  • Global planners (A*, Dijkstra, etc.)
  • Local planners (Teb, DWA, etc.)
  • Custom planner integration capabilities
  • Multi-modal path planning

Controllers​

  • Trajectory controllers for smooth navigation
  • Footstep planning for legged robots
  • Balance and stability considerations
  • Humanoid-specific locomotion patterns

Traditional vs Humanoid Navigation​

Traditional navigation assumes wheeled platforms with:

  • Continuous ground contact
  • Simple kinematic constraints
  • Predictable motion patterns

Humanoid navigation requires:

  • Discrete foot contact points
  • Balance and stability considerations
  • Complex kinematic chains
  • Multi-step planning for walking

Humanoid-Specific Challenges​

  1. Balance and Stability: Maintaining center of mass during navigation
  2. Footstep Planning: Planning safe and stable foot placements
  3. Dynamic Obstacles: Handling moving obstacles in human environments
  4. Terrain Adaptation: Navigating uneven surfaces and obstacles
  5. Social Navigation: Following human social norms and behaviors

System Requirements​

# Install Nav2 packages
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
sudo apt install ros-humble-nav2-rviz-plugins

# Install humanoid-specific packages
sudo apt install ros-humble-legged-controllers
sudo apt install ros-humble-footstep-planner

Basic Nav2 Launch File for Humanoid​

<!-- humanoid_nav2_config/launch/navigation_launch.py -->
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml


def generate_launch_description():
    # Launch arguments
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('params_file')
    bt_xml_file = LaunchConfiguration('bt_xml_file')

    # Create our own temporary YAML files that include substitutions
    param_substitutions = {
        'use_sim_time': use_sim_time,
        'autostart': autostart
    }

    configured_params = RewrittenYaml(
        source_file=params_file,
        root_key='humanoid',
        param_rewrites=param_substitutions,
        convert_types=True)

    return LaunchDescription([
        # Nodes
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[configured_params],
            remappings=[('cmd_vel', 'cmd_vel_out')]
        ),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[configured_params]
        ),

        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            output='screen',
            parameters=[configured_params]
        ),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[configured_params, {'bt_xml_filename': bt_xml_file}]
        ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_navigation',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': autostart},
                        {'node_names': ['controller_server',
                                        'planner_server',
                                        'recoveries_server',
                                        'bt_navigator']}]
        )
    ])

Humanoid-Specific Configuration​

# humanoid_nav2_config/config/nav2_params.yaml
humanoid:
  ros__parameters:
    use_sim_time: False
    autostart: True
    transform_tolerance: 0.1
    global_frame: map
    robot_base_frame: base_link
    update_frequency: 5.0
    publish_frequency: 5.0
    always_send_velocity: False
    restore_defaults: False

    controller_server:
      ros__parameters:
        use_sim_time: False
        controller_frequency: 20.0
        min_x_velocity_threshold: 0.001
        min_y_velocity_threshold: 0.5
        min_theta_velocity_threshold: 0.001
        failure_tolerance: 0.3
        progress_checker_plugin: "progress_checker"
        goal_checker_plugin: "goal_checker"
        controller_plugins: ["FollowPath"]

        # Humanoid-specific controller
        FollowPath:
          plugin: "nav2_mppi_controller::MppiController"
          time_steps: 20
          control_frequency: 10.0
          model_dt: 0.1
          xy_goal_tolerance: 0.25
          yaw_goal_tolerance: 0.25
          state_bounds_x: [-2.0, 2.0]
          state_bounds_y: [-2.0, 2.0]
          state_bounds_theta: [-1.57, 1.57]
          control_bounds_x: [-0.8, 0.8]
          control_bounds_y: [-0.8, 0.8]
          control_bounds_theta: [-0.6, 0.6]
          weights: [1.0, 1.0, 0.0, 0.0, 0.0, 0.3, 0.2]
          horizon: 2.0
          reference_speed: 0.3
          rate_limit_scaling: 1.5
          motion_model: "DiffDrive"
          # Humanoid-specific parameters
          balance_margin: 0.1  # Safety margin for balance
          step_duration: 0.8   # Time per step
          max_step_length: 0.4 # Maximum step length

    planner_server:
      ros__parameters:
        use_sim_time: False
        planner_plugins: ["GridBased"]
        GridBased:
          plugin: "nav2_navfn_planner/NavfnPlanner"
          tolerance: 0.5
          use_astar: false
          allow_unknown: true
          # Humanoid-specific parameters
          step_size: 0.1       # Grid resolution for footstep planning
          min_step_size: 0.2   # Minimum viable step size
          max_step_size: 0.4   # Maximum safe step size
          balance_penalty: 10.0 # Penalty for unstable positions

    recoveries_server:
      ros__parameters:
        use_sim_time: False
        recovery_plugins: ["spin", "backup", "wait"]
        spin:
          plugin: "nav2_recoveries/Spin"
          tolerance: 1.57
          max_rotational_vel: 0.4
          min_rotational_vel: 0.1
          rotational_acc_lim: 3.2
        backup:
          plugin: "nav2_recoveries/BackUp"
          sim_time: 2.0
          translation_weight: 1.0
          rotational_weight: 1.0
          velocity_scaling_factor: 0.5
        wait:
          plugin: "nav2_recoveries/Wait"
          sim_time: 5.0

    bt_navigator:
      ros__parameters:
        use_sim_time: False
        global_frame: map
        robot_base_frame: base_link
        odom_topic: /odom
        bt_loop_duration: 10
        default_server_timeout: 20
        enable_groot_monitoring: True
        groot_zmq_publisher_port: 1666
        groot_zmq_server_port: 1667
        default_nav_through_poses_bt_xml: "humanoid_navigate_through_poses_w_replanning_and_recovery.xml"
        default_nav_to_pose_bt_xml: "humanoid_navigate_to_pose_w_replanning_and_recovery.xml"
        # Humanoid-specific parameters
        max_linear_speed: 0.4
        max_angular_speed: 0.5
        step_planning_enabled: true
        balance_check_frequency: 10.0

Behavior Trees for Humanoid Navigation​

Custom Behavior Tree for Humanoid Navigation​

<!-- humanoid_nav2_config/behavior_trees/humanoid_navigate_to_pose_w_replanning_and_recovery.xml -->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <RecoveryNode number_of_retries="6" name="NavigateRecovery">
          <PipelineSequence name="Navigate">
            <RecoveryNode number_of_retries="2" name="ComputePathToPose">
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
              <ClearEntirely name="ClearPath" error_handler="backup"/>
            </RecoveryNode>
            <RecoveryNode number_of_retries="2" name="FollowPath">
              <FollowPath path="{path}" controller_id="FollowPath"/>
              <ClearEntirely name="ClearFollowPath" error_handler="backup"/>
            </RecoveryNode>
          </PipelineSequence>
          <ReactiveFallback name="RecoveryFallback">
            <GoalUpdated/>
            <RoundRobin name="RecoveryActions">
              <Spin spin_dist="1.57"/>
              <Wait wait_duration="5"/>
              <Backup backup_dist="0.15" backup_speed="0.05"/>
            </RoundRobin>
          </ReactiveFallback>
        </RecoveryNode>
      </RateController>
      <ReactiveSequence name="CheckBalance">
        <IsHumanoidBalanced/>
        <UpdatePathWithStepPlanning path="{path}" step_planner="FootstepPlanner"/>
      </ReactiveSequence>
    </PipelineSequence>
  </BehaviorTree>
</root>

Creating Custom Behavior Tree Nodes​

// include/humanoid_bt_nodes/is_humanoid_balanced_condition.hpp
#ifndef HUMANOID_BT_NODES__IS_HUMANOID_BALANCED_CONDITION_HPP_
#define HUMANOID_BT_NODES__IS_HUMANOID_BALANCED_CONDITION_HPP_

#include <string>
#include "behaviortree_cpp_v3/condition_node.h"
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/transform_listener.h"

namespace humanoid_bt_nodes
{

class IsHumanoidBalancedCondition : public BT::ConditionNode
{
public:
  IsHumanoidBalancedCondition(
    const std::string & condition_name,
    const BT::NodeConfiguration & config);

  BT::NodeStatus tick() override;

  static BT::PortsList providedPorts()
  {
    return BT::PortsList({});
  }

private:
  rclcpp::Node::SharedPtr node_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  std::string robot_base_frame_;
  std::string odom_frame_;
  double balance_threshold_;
};

}  // namespace humanoid_bt_nodes

#endif  // HUMANOID_BT_NODES__IS_HUMANOID_BALANCED_CONDITION_HPP_
// src/is_humanoid_balanced_condition.cpp
#include "humanoid_bt_nodes/is_humanoid_balanced_condition.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

namespace humanoid_bt_nodes
{

IsHumanoidBalancedCondition::IsHumanoidBalancedCondition(
  const std::string & condition_name,
  const BT::NodeConfiguration & config)
: BT::ConditionNode(condition_name, config)
{
  node_ = std::make_shared<rclcpp::Node>("is_humanoid_balanced_node");
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  // Get parameters
  node_->declare_parameter("robot_base_frame", "base_link");
  node_->declare_parameter("odom_frame", "odom");
  node_->declare_parameter("balance_threshold", 0.1);

  node_->get_parameter("robot_base_frame", robot_base_frame_);
  node_->get_parameter("odom_frame", odom_frame_);
  node_->get_parameter("balance_threshold", balance_threshold_);
}

BT::NodeStatus IsHumanoidBalancedCondition::tick()
{
  try {
    // Get transform to check balance
    geometry_msgs::msg::TransformStamped transform =
      tf_buffer_->lookupTransform(odom_frame_, robot_base_frame_, tf2::TimePointZero);

    // Check balance based on center of mass position
    double com_x = transform.transform.translation.x;
    double com_y = transform.transform.translation.y;

    // Calculate distance from center (simplified balance check)
    double balance_distance = sqrt(com_x * com_x + com_y * com_y);

    if (balance_distance < balance_threshold_) {
      RCLCPP_DEBUG(node_->get_logger(), "Humanoid is balanced (distance: %f)", balance_distance);
      return BT::NodeStatus::SUCCESS;
    } else {
      RCLCPP_WARN(node_->get_logger(), "Humanoid is unbalanced (distance: %f)", balance_distance);
      return BT::NodeStatus::FAILURE;
    }
  } catch (const tf2::TransformException & ex) {
    RCLCPP_ERROR(node_->get_logger(), "Could not get transform: %s", ex.what());
    return BT::NodeStatus::FAILURE;
  }
}

}  // namespace humanoid_bt_nodes

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<humanoid_bt_nodes::IsHumanoidBalancedCondition>("IsHumanoidBalanced");
}

Footstep Planning for Bipedal Navigation​

Footstep Planning Node​

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Point
from nav_msgs.msg import Path
from visualization_msgs.msg import Marker, MarkerArray
from std_msgs.msg import ColorRGBA
from builtin_interfaces.msg import Duration
import numpy as np
from scipy.spatial.transform import Rotation as R

class FootstepPlannerNode(Node):
    def __init__(self):
        super().__init__('footstep_planner_node')

        # Publishers
        self.footstep_path_pub = self.create_publisher(
            Path,
            '/footstep_path',
            10
        )

        self.visualization_pub = self.create_publisher(
            MarkerArray,
            '/footstep_visualization',
            10
        )

        # Parameters
        self.declare_parameter('step_width', 0.2)  # Distance between feet
        self.declare_parameter('step_length', 0.4)  # Forward step distance
        self.declare_parameter('max_step_rotation', 0.3)  # Max rotation per step

        # Timer for continuous planning
        self.timer = self.create_timer(0.1, self.plan_footsteps_callback)

    def plan_footsteps_callback(self):
        """Plan footsteps based on navigation path"""
        # This would receive the global path and convert it to footsteps
        pass

    def plan_footsteps(self, global_path):
        """Convert global path to footstep sequence for bipedal robot"""
        footsteps = []

        # Start with current robot pose
        current_left_foot = self.get_current_left_foot_pose()
        current_right_foot = self.get_current_right_foot_pose()

        # Determine which foot to step with first
        use_left_first = True  # Could be based on current stance

        for i in range(len(global_path.poses) - 1):
            target_pose = global_path.poses[i + 1]

            # Calculate required step
            step_vector = np.array([
                target_pose.pose.position.x - current_left_foot.position.x if use_left_first else target_pose.pose.position.x - current_right_foot.position.x,
                target_pose.pose.position.y - current_left_foot.position.y if use_left_first else target_pose.pose.position.y - current_right_foot.position.y
            ])

            # Check if step is within limits
            step_distance = np.linalg.norm(step_vector)

            if step_distance > self.get_parameter('step_length').value:
                # Need to break into multiple steps
                num_steps = int(np.ceil(step_distance / self.get_parameter('step_length').value))
                step_increment = step_vector / num_steps

                for j in range(num_steps):
                    if use_left_first:
                        new_left_foot = self.calculate_foot_pose(current_left_foot, step_increment)
                        footsteps.append(('left', new_left_foot))
                        current_left_foot = new_left_foot
                    else:
                        new_right_foot = self.calculate_foot_pose(current_right_foot, step_increment)
                        footsteps.append(('right', new_right_foot))
                        current_right_foot = new_right_foot

                    use_left_first = not use_left_first  # Alternate feet
            else:
                # Single step
                if use_left_first:
                    new_left_foot = self.calculate_foot_pose(current_left_foot, step_vector)
                    footsteps.append(('left', new_left_foot))
                    current_left_foot = new_left_foot
                else:
                    new_right_foot = self.calculate_foot_pose(current_right_foot, step_vector)
                    footsteps.append(('right', new_right_foot))
                    current_right_foot = new_right_foot

                use_left_first = not use_left_first  # Alternate feet

        return footsteps

    def calculate_foot_pose(self, current_pose, step_vector):
        """Calculate next foot pose based on step vector"""
        new_pose = PoseStamped()
        new_pose.header = current_pose.header
        new_pose.pose.position.x = current_pose.pose.position.x + step_vector[0]
        new_pose.pose.position.y = current_pose.pose.position.y + step_vector[1]
        new_pose.pose.position.z = current_pose.pose.position.z  # Maintain ground contact

        # Maintain orientation or adjust for step direction
        step_angle = np.arctan2(step_vector[1], step_vector[0])

        # Create quaternion for rotation
        rot = R.from_euler('z', step_angle)
        quat = rot.as_quat()
        new_pose.pose.orientation.x = quat[0]
        new_pose.pose.orientation.y = quat[1]
        new_pose.pose.orientation.z = quat[2]
        new_pose.pose.orientation.w = quat[3]

        return new_pose

    def get_current_left_foot_pose(self):
        """Get current left foot pose from robot state"""
        # This would typically come from robot state publisher
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = self.get_clock().now().to_msg()
        pose.pose.position.x = 0.0
        pose.pose.position.y = 0.1  # Offset for left foot
        pose.pose.position.z = 0.0
        pose.pose.orientation.w = 1.0
        return pose

    def get_current_right_foot_pose(self):
        """Get current right foot pose from robot state"""
        # This would typically come from robot state publisher
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = self.get_clock().now().to_msg()
        pose.pose.position.x = 0.0
        pose.pose.position.y = -0.1  # Offset for right foot
        pose.pose.position.z = 0.0
        pose.pose.orientation.w = 1.0
        return pose

    def visualize_footsteps(self, footsteps):
        """Visualize footsteps in RViz"""
        marker_array = MarkerArray()

        for i, (foot, pose) in enumerate(footsteps):
            marker = Marker()
            marker.header = pose.header
            marker.ns = "footsteps"
            marker.id = i
            marker.type = Marker.CUBE
            marker.action = Marker.ADD

            # Position
            marker.pose = pose.pose
            marker.pose.position.z += 0.05  # Lift slightly above ground

            # Scale
            marker.scale.x = 0.15  # Foot length
            marker.scale.y = 0.08  # Foot width
            marker.scale.z = 0.02  # Foot height

            # Color
            if foot == 'left':
                marker.color.r = 1.0
                marker.color.g = 0.0
                marker.color.b = 0.0
            else:
                marker.color.r = 0.0
                marker.color.g = 0.0
                marker.color.b = 1.0
            marker.color.a = 0.7

            # Lifetime
            marker.lifetime = Duration(sec=10)

            marker_array.markers.append(marker)

        self.visualization_pub.publish(marker_array)

def main(args=None):
    rclpy.init(args=args)
    node = FootstepPlannerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Humanoid-Specific Controllers​

Balance-Aware Controller​

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from std_msgs.msg import Float64MultiArray
from builtin_interfaces.msg import Duration
import numpy as np
from scipy import signal

class HumanoidBalanceController(Node):
    def __init__(self):
        super().__init__('humanoid_balance_controller')

        # Subscriptions
        self.odom_sub = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10
        )

        self.imu_sub = self.create_subscription(
            Imu,
            '/imu/data',
            self.imu_callback,
            10
        )

        self.cmd_vel_sub = self.create_subscription(
            Twist,
            '/cmd_vel_in',
            self.cmd_vel_callback,
            10
        )

        # Publishers
        self.cmd_vel_pub = self.create_publisher(
            Twist,
            '/cmd_vel_out',
            10
        )

        self.balance_pub = self.create_publisher(
            Float64MultiArray,
            '/balance_status',
            10
        )

        # Parameters
        self.declare_parameter('balance_kp', 2.0)
        self.declare_parameter('balance_kd', 1.0)
        self.declare_parameter('max_angular_correction', 0.1)
        self.declare_parameter('balance_threshold', 0.05)

        # State variables
        self.current_pose = None
        self.current_twist = None
        self.current_imu = None
        self.desired_twist = Twist()
        self.balance_error = 0.0
        self.balance_derivative = 0.0
        self.previous_balance_error = 0.0
        self.last_time = self.get_clock().now()

    def odom_callback(self, msg):
        """Update robot pose and twist"""
        self.current_pose = msg.pose.pose
        self.current_twist = msg.twist.twist

    def imu_callback(self, msg):
        """Update IMU data for balance calculation"""
        self.current_imu = msg
        self.update_balance_state()

    def cmd_vel_callback(self, msg):
        """Receive desired velocity command"""
        self.desired_twist = msg

    def update_balance_state(self):
        """Calculate balance state from IMU data"""
        if self.current_imu is not None:
            # Calculate roll and pitch from IMU quaternion
            quat = self.current_imu.orientation
            # Convert quaternion to roll/pitch
            sinr_cosp = 2 * (quat.w * quat.x + quat.y * quat.z)
            cosr_cosp = 1 - 2 * (quat.x * quat.x + quat.y * quat.y)
            roll = np.arctan2(sinr_cosp, cosr_cosp)

            sinp = 2 * (quat.w * quat.y - quat.z * quat.x)
            pitch = np.arcsin(sinp)

            # Use pitch as primary balance indicator for bipedal
            self.balance_error = pitch

            # Calculate derivative for PD controller
            current_time = self.get_clock().now()
            dt = (current_time - self.last_time).nanoseconds / 1e9
            if dt > 0:
                self.balance_derivative = (self.balance_error - self.previous_balance_error) / dt
            else:
                self.balance_derivative = 0.0

            self.previous_balance_error = self.balance_error
            self.last_time = current_time

    def compute_balance_correction(self):
        """Compute balance correction using PD controller"""
        kp = self.get_parameter('balance_kp').value
        kd = self.get_parameter('balance_kd').value
        max_correction = self.get_parameter('max_angular_correction').value

        correction = - (kp * self.balance_error + kd * self.balance_derivative)
        correction = np.clip(correction, -max_correction, max_correction)

        return correction

    def control_loop(self):
        """Main control loop"""
        if self.current_imu is not None and self.current_pose is not None:
            # Compute balance correction
            balance_correction = self.compute_balance_correction()

            # Create output command combining desired motion and balance correction
            output_cmd = Twist()

            # Copy linear velocities (balance correction doesn't affect linear motion)
            output_cmd.linear = self.desired_twist.linear

            # Add balance correction to angular velocity
            output_cmd.angular.z = self.desired_twist.angular.z + balance_correction
            # Limit total angular velocity
            max_angular = 0.5  # rad/s
            output_cmd.angular.z = np.clip(output_cmd.angular.z, -max_angular, max_angular)

            # Check if balance is critical
            balance_threshold = self.get_parameter('balance_threshold').value
            if abs(self.balance_error) > balance_threshold:
                # Reduce linear velocity when out of balance
                output_cmd.linear.x *= 0.5
                output_cmd.linear.y *= 0.5

            # Publish command
            self.cmd_vel_pub.publish(output_cmd)

            # Publish balance status
            balance_status = Float64MultiArray()
            balance_status.data = [self.balance_error, balance_correction, abs(self.balance_error) > balance_threshold]
            self.balance_pub.publish(balance_status)

    def timer_callback(self):
        """Timer callback for control loop"""
        self.control_loop()

def main(args=None):
    rclpy.init(args=args)

    controller = HumanoidBalanceController()

    # Create timer for control loop
    timer = controller.create_timer(0.05, controller.timer_callback)  # 20 Hz

    rclpy.spin(controller)

    controller.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Advanced Path Planning for Humanoids​

3D Costmap for Humanoid Navigation​

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, LaserScan
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point
from std_msgs.msg import Header
import numpy as np
from scipy.spatial import cKDTree
import sensor_msgs_py.point_cloud2 as pc2

class Humanoid3DCostmapNode(Node):
    def __init__(self):
        super().__init__('humanoid_3d_costmap_node')

        # Subscriptions
        self.pointcloud_sub = self.create_subscription(
            PointCloud2,
            '/points',
            self.pointcloud_callback,
            10
        )

        self.scan_sub = self.create_subscription(
            LaserScan,
            '/scan',
            self.scan_callback,
            10
        )

        # Publishers
        self.costmap_pub = self.create_publisher(
            OccupancyGrid,
            '/humanoid_costmap',
            10
        )

        # Parameters
        self.declare_parameter('resolution', 0.1)  # meters per cell
        self.declare_parameter('robot_height', 1.5)  # meters
        self.declare_parameter('max_obstacle_height', 0.5)  # meters (for step-over)
        self.declare_parameter('min_obstacle_height', 0.1)  # meters (for step-over)

        # Costmap grid
        self.grid_size = 20  # meters
        self.grid_resolution = self.get_parameter('resolution').value
        self.grid_width = int(self.grid_size / self.grid_resolution)
        self.grid = np.zeros((self.grid_width, self.grid_width), dtype=np.int8)

        # Point cloud storage for 3D analysis
        self.point_cloud_data = []

    def pointcloud_callback(self, msg):
        """Process 3D point cloud data"""
        # Convert PointCloud2 to list of points
        points = list(pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True))

        # Store for 3D costmap calculation
        self.point_cloud_data = points

        # Update 3D costmap
        self.update_3d_costmap()

    def scan_callback(self, msg):
        """Process 2D laser scan data"""
        # Convert scan to 2D points at robot height level
        angles = np.array([msg.angle_min + i * msg.angle_increment for i in range(len(msg.ranges))])
        ranges = np.array(msg.ranges)

        # Filter valid ranges
        valid_indices = (ranges >= msg.range_min) & (ranges <= msg.range_max)
        valid_angles = angles[valid_indices]
        valid_ranges = ranges[valid_indices]

        # Convert to Cartesian coordinates
        x_points = valid_ranges * np.cos(valid_angles)
        y_points = valid_ranges * np.sin(valid_angles)

        # Add to point cloud data
        for x, y in zip(x_points, y_points):
            # Assume points are at ground level
            self.point_cloud_data.append((x, y, 0.0))

    def update_3d_costmap(self):
        """Update costmap considering 3D obstacles"""
        # Clear costmap
        self.grid.fill(0)

        # Convert point cloud to numpy array
        if len(self.point_cloud_data) == 0:
            return

        points_3d = np.array(self.point_cloud_data)

        # Create 2D projection of obstacles
        for point in points_3d:
            x, y, z = point

            # Convert world coordinates to grid coordinates
            grid_x = int((x + self.grid_size/2) / self.grid_resolution)
            grid_y = int((y + self.grid_size/2) / self.grid_resolution)

            # Check bounds
            if 0 <= grid_x < self.grid_width and 0 <= grid_y < self.grid_width:
                # Determine cost based on obstacle height
                if z > self.get_parameter('max_obstacle_height').value:
                    # Too high to step over - mark as lethal obstacle
                    self.grid[grid_y, grid_x] = 100
                elif z > self.get_parameter('min_obstacle_height').value:
                    # Can step over but requires careful planning
                    self.grid[grid_y, grid_x] = 75
                elif z < 0:  # Below ground level
                    # Potential hole or step down - mark as risky
                    self.grid[grid_y, grid_x] = 50
                else:
                    # Ground-level obstacle
                    self.grid[grid_y, grid_x] = 90

        # Publish updated costmap
        self.publish_costmap()

    def publish_costmap(self):
        """Publish the 2D costmap"""
        costmap_msg = OccupancyGrid()
        costmap_msg.header = Header()
        costmap_msg.header.stamp = self.get_clock().now().to_msg()
        costmap_msg.header.frame_id = 'map'

        # Set metadata
        costmap_msg.info.resolution = self.grid_resolution
        costmap_msg.info.width = self.grid_width
        costmap_msg.info.height = self.grid_width
        costmap_msg.info.origin.position.x = -self.grid_size/2
        costmap_msg.info.origin.position.y = -self.grid_size/2
        costmap_msg.info.origin.orientation.w = 1.0

        # Flatten grid for message
        costmap_msg.data = self.grid.flatten().tolist()

        self.costmap_pub.publish(costmap_msg)

def main(args=None):
    rclpy.init(args=args)
    node = Humanoid3DCostmapNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Integration with Isaac ROS​

Isaac ROS Humanoid Navigation Pipeline​

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, Imu, PointCloud2
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from visualization_msgs.msg import MarkerArray
from std_msgs.msg import String
import numpy as np

class IsaacHumanoidNavigationNode(Node):
    def __init__(self):
        super().__init__('isaac_humanoid_navigation_node')

        # Isaac ROS integration
        self.setup_isaac_ros_interfaces()

        # Humanoid-specific navigation
        self.setup_humanoid_navigation()

        # Publishers and subscribers
        self.setup_communication_interfaces()

    def setup_isaac_ros_interfaces(self):
        """Setup Isaac ROS perception interfaces"""
        # Subscribe to Isaac ROS perception outputs
        self.perception_sub = self.create_subscription(
            MarkerArray,
            '/isaac_ros_perception/markers',
            self.perception_callback,
            10
        )

        # Subscribe to Isaac ROS VSLAM output
        self.vslam_sub = self.create_subscription(
            Odometry,
            '/isaac_ros_vslam/odometry',
            self.vslam_callback,
            10
        )

        # Subscribe to Isaac ROS object detection
        self.detection_sub = self.create_subscription(
            String,  # Placeholder - would be actual detection message
            '/isaac_ros_detection/objects',
            self.detection_callback,
            10
        )

    def setup_humanoid_navigation(self):
        """Setup humanoid-specific navigation components"""
        # Initialize footstep planner
        self.footstep_planner = FootstepPlannerNode()

        # Initialize balance controller
        self.balance_controller = HumanoidBalanceController()

        # Initialize 3D costmap
        self.costmap_3d = Humanoid3DCostmapNode()

    def setup_communication_interfaces(self):
        """Setup communication interfaces with Nav2"""
        # Subscribe to Nav2 planner outputs
        self.nav2_path_sub = self.create_subscription(
            PoseStamped,
            '/nav2/path',
            self.nav2_path_callback,
            10
        )

        # Publish to Nav2 controller
        self.nav2_cmd_pub = self.create_publisher(
            Twist,
            '/nav2/cmd_vel',
            10
        )

    def perception_callback(self, msg):
        """Process Isaac ROS perception data"""
        # Integrate perception data into humanoid navigation
        for marker in msg.markers:
            if marker.type == marker.CUBE:  # Obstacle
                self.update_3d_costmap_with_obstacle(marker)
            elif marker.type == marker.SPHERE:  # Goal
                self.update_navigation_goal(marker)

    def vslam_callback(self, msg):
        """Process Isaac ROS VSLAM data"""
        # Update robot pose for navigation
        self.current_pose = msg.pose.pose
        self.current_twist = msg.twist.twist

        # Update balance controller with new pose
        self.balance_controller.current_pose = msg.pose.pose

    def detection_callback(self, msg):
        """Process Isaac ROS object detection"""
        # Parse detection results
        # Update costmap with detected objects
        # Plan around dynamic obstacles
        pass

    def nav2_path_callback(self, msg):
        """Process Nav2 planned path"""
        # Convert Nav2 path to footstep sequence
        footstep_path = self.footstep_planner.plan_footsteps(msg)

        # Visualize footsteps
        self.footstep_planner.visualize_footsteps(footstep_path)

        # Execute footstep plan
        self.execute_footstep_plan(footstep_path)

    def execute_footstep_plan(self, footstep_path):
        """Execute planned footsteps"""
        for foot, pose in footstep_path:
            # Send step command to robot
            self.send_step_command(foot, pose)

            # Wait for step completion
            self.wait_for_step_completion()

            # Check balance after each step
            if not self.is_balanced():
                self.execute_balance_recovery()
                break

    def is_balanced(self):
        """Check if humanoid is balanced"""
        # This would check actual balance state
        # For now, return True as placeholder
        return True

    def execute_balance_recovery(self):
        """Execute balance recovery procedure"""
        # Stop current motion
        stop_cmd = Twist()
        self.nav2_cmd_pub.publish(stop_cmd)

        # Execute balance recovery routine
        # This would involve adjusting stance, etc.
        pass

    def send_step_command(self, foot, pose):
        """Send step command to robot"""
        # This would send actual command to robot controller
        cmd = Twist()
        cmd.linear.x = 0.1  # Move forward slowly
        cmd.angular.z = 0.05  # Small turn if needed
        self.nav2_cmd_pub.publish(cmd)

    def wait_for_step_completion(self):
        """Wait for step to complete"""
        # This would wait for robot feedback
        # For now, just sleep
        import time
        time.sleep(1.0)

def main(args=None):
    rclpy.init(args=args)
    node = IsaacHumanoidNavigationNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Best Practices for Humanoid Navigation​

Safety Considerations​

  1. Balance Monitoring: Continuously monitor robot's center of mass
  2. Step Validation: Verify each step is safe before execution
  3. Recovery Procedures: Implement balance recovery routines
  4. Emergency Stops: Quick stop mechanisms for dangerous situations
  5. Terrain Assessment: Evaluate terrain before navigation

Performance Optimization​

  1. Multi-threading: Separate perception, planning, and control threads
  2. Predictive Planning: Plan multiple steps ahead
  3. Sensor Fusion: Combine multiple sensor modalities
  4. Model Predictive Control: Use MPC for dynamic balancing
  5. Hierarchical Planning: Coarse-to-fine planning approach

Exercise: Humanoid Navigation Challenge​

Implement a complete humanoid navigation system that:

  1. Integrates Nav2 with Isaac ROS perception
  2. Implements footstep planning for bipedal locomotion
  3. Includes balance-aware control algorithms
  4. Handles 3D obstacles and terrain variations
  5. Demonstrates safe navigation in a complex environment

Summary​

In this section, we've covered:

  • Nav2 architecture adapted for humanoid robots
  • Footstep planning algorithms for bipedal navigation
  • Balance-aware control systems
  • 3D costmap generation for complex environments
  • Integration with Isaac ROS perception systems
  • Safety considerations for humanoid navigation

The next module will explore Vision-Language-Action models that enable robots to understand and execute natural language commands.