search

NVIDIA-ISAAC-ROS / isaac_ros_nvblox

NVIDIA-accelerated 3D scene reconstruction and Nav2 local costmap provider using nvblox
https://developer.nvidia.com/isaac-ros-gems
Apache License 2.0
433 stars 80 forks source link

Waiting for service map_server/get_state on nav2 application #37

Closed chivas1000 closed 1 year ago

chivas1000 commented 1 year ago

Hi, I’m modifying the carter_sim_elbrus.launch.py example in nvblox_nav2 to do navigation in my robot, The application is ok at DP1.1 release before but for DP2.0, after the configurinig is done, the terminal keep popping “waiting for server state”

Error Output: [lifecycle_manager-10] [INFO] [1666756876.922715146] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state... [lifecycle_manager-1] [INFO] [1666756876.922716862] [lifecycle_manager_map]: Waiting for service map_server/get_state... [lifecycle_manager-1] [INFO] [1666756878.923016306] [lifecycle_manager_map]: Waiting for service map_server/get_state... [lifecycle_manager-10] [INFO] [1666756878.923042882] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state... [lifecycle_manager-1] [INFO] [1666756880.923268323] [lifecycle_manager_map]: Waiting for service map_server/get_state... [lifecycle_manager-10] [INFO] [1666756880.923292501] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state... [lifecycle_manager-10] [INFO] [1666756882.923515151] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state... [lifecycle_manager-1] [INFO] [1666756882.923517381] [lifecycle_manager_map]: Waiting for service map_server/get_state... [lifecycle_manager-1] [INFO] [1666756884.923782959] [lifecycle_manager_map]: Waiting for service map_server/get_state... [lifecycle_manager-10] [INFO] [1666756884.923803742] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state... [nvblox_node-11] [INFO] [1666756885.119232012] [nvblox_node]: RGB frame statistics: [nvblox_node-11] avg=66.713883, min=55.955423, max=84.688353, std_dev=5.970719, count=13211 [nvblox_node-11] [INFO] [1666756885.463369156] [nvblox_node]: Depth frame statistics: [nvblox_node-11] avg=36.677327, min=13.215166, max=126.695587, std_dev=10.915946, count=24039 [nvblox_node-11] [INFO] [1666756885.463661005] [nvblox_node]: Timing statistics: [nvblox_node-11] NVBlox Timing [nvblox_node-11] ----------- [nvblox_node-11] color/integrate

But the nvblox and vslam seem working as expect Screenshot from 2022-10-26 10-58-51

so maybe is that the navigation stack doesn’t working since there are no availiable map input even though the nvblox does produced the map, so I assume that the nvblox has output esdf map but map server didn’t receive it

My launch files:

carter_sim_elbrus_new.launch.py (for this the I didn't modified the map server related lines)

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode

def generate_launch_description():

#simulation time, this would not be use in the our application
use_sim_time = LaunchConfiguration('use_sim_time', default='False')

#costmap param, need to be modify
param_dir = LaunchConfiguration(
    'params_file',
    default=os.path.join(
        get_package_share_directory(
            'nvblox_nav2'), 'params', 'carter_nav2_new.yaml'
    ),
)

#nvblox params, if the output is not good, tune later
nvblox_param_dir = LaunchConfiguration(
    'nvblox_params_file', default=os.path.join(
        get_package_share_directory('nvblox_nav2'),
        'params', 'nvblox.yaml'),)

nav2_bringup_launch_dir = os.path.join(
    get_package_share_directory('nav2_bringup'), 'launch')

rviz_config_dir = os.path.join(get_package_share_directory(
    'nvblox_nav2'), 'config', 'carter_nvblox_nav2.rviz')

lifecycle_nodes = ['map_server']

#setting up vslam node, to be modify, we use infrared image here
visual_slam_node = ComposableNode(
    name='visual_slam_node',
    package='isaac_ros_visual_slam',
    plugin='isaac_ros::visual_slam::VisualSlamNode',
    remappings=[('/stereo_camera/left/camera_info', '/camera/infra1/camera_info'),
                ('/stereo_camera/right/camera_info', '/camera/infra2/camera_info'),
                ('/stereo_camera/left/image', '/camera/realsense_splitter_node/output/infra_1'),
                ('/stereo_camera/right/image', '/camera/realsense_splitter_node/output/infra_2')],
    parameters=[{
        'image_qos': 'SENSOR_DATA',
        'enable_rectified_pose': True,
        'denoise_input_images': True,
        'rectified_images': True,
        'enable_debug_mode': False,
        'debug_dump_path': '/tmp/elbrus',
        'left_camera_frame': 'camera_infra1_frame',
        'right_camera_frame': 'camera_infra2_frame',
        'map_frame': 'map',
        'fixed_frame': 'odom',
        'odom_frame': 'odom',
        'base_frame': 'base_link',
        #these two does not have reference
        'current_smooth_frame': 'base_link_smooth',
        'current_rectified_frame': 'base_link_rectified',
        'enable_observations_view': True,
        'enable_landmarks_view': True,
        'enable_reading_slam_internals': True,
        'enable_slam_visualization': True,
        'enable_localization_n_mapping': True,
        'use_sim_time': use_sim_time
    }]
)
visual_slam_launch_container = ComposableNodeContainer(
    name='visual_slam_launch_container',
    namespace='',
    package='rclcpp_components',
    executable='component_container',
    composable_node_descriptions=[
        visual_slam_node
    ],
    output='screen'
)
#now vslam should be set

return LaunchDescription(
    [
        #this lacks the realsense node, would add it in later(on the above)
        #here we don't run the realsense at edge
        #realsense_container,
        #adding TF joints to the camera and baselink
        #base_link_tf_node,
        #loading args of params file
        DeclareLaunchArgument(
            'params_file', default_value=param_dir,
            description='Full path to param file to load'),

        #is it simulation, in our application it is not, it is false
        DeclareLaunchArgument(
            'use_sim_time', default_value='False',
            description='Use simulation (Omniverse Isaac Sim) clock if true'),
        #rviz args, leave it
        DeclareLaunchArgument(
            'run_rviz', default_value='true',
            description='Whether to start RVIZ'),

        #running map_server
        DeclareLaunchArgument(
            'run_nav2', default_value='true',
            description='Whether to run nav2'),
        Node(
            package='nav2_lifecycle_manager', executable='lifecycle_manager',
            name='lifecycle_manager_map', output='screen',
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}],
            condition=IfCondition(
                LaunchConfiguration('run_nav2'))
        ),

            #bringup rviz
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav2_bringup_launch_dir, 'rviz_launch.py')),
            launch_arguments={'namespace': '', 'use_namespace': 'False',
                              'autostart': 'True',
                              'rviz_config': rviz_config_dir}.items(),
            condition=IfCondition(LaunchConfiguration('run_rviz')),),

            #setting up nav2 stack and bring up, points to carter_nav2_new.yaml
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(
                    nav2_bringup_launch_dir, 'navigation_launch.py')),
            launch_arguments={'params_file': param_dir,
                       'use_sim_time': use_sim_time,
                              'autostart': 'True'}.items(),
            condition=IfCondition(
                LaunchConfiguration('run_nav2'))
        ),

        #bringing up nvblox, modify the camera input
        Node(
            package='nvblox_ros', executable='nvblox_node',
            parameters=[nvblox_param_dir, {'use_sim_time': use_sim_time,
                                           'global_frame': 'odom'}],
            output='screen',
            remappings=[('depth/image', '/camera/realsense_splitter_node/output/depth'),
                        ('depth/camera_info', '/camera/depth/camera_info'),
                        ('color/image', '/camera/color/image_raw'),
                        ('color/camera_info', '/camera/color/camera_info'), ]),
        #bringing up vslam
        visual_slam_launch_container,

    ])

carter_nav2_new.yaml (for this I only modified the tolerance and threshold to fit my own robot, the map form or name didn't changed)

bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 20 default_server_timeout: 40

'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:

# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node: ros__parameters: use_sim_time: True

controller_server: ros__parameters: use_sim_time: True controller_frequency: 10.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.05 min_theta_velocity_threshold: 0.001

change the failure_tolerance for tunning

failure_tolerance: 2
progress_checker_plugin: "progress_checker"
goal_checker_plugin: ["general_goal_checker"]
controller_plugins: ["FollowPath"]

# Progress checker parameters
progress_checker:
  plugin: "nav2_controller::SimpleProgressChecker"
  required_movement_radius: 0.5
  movement_time_allowance: 10.0
# Goal checker parameters
general_goal_checker:
  plugin: "nav2_controller::SimpleGoalChecker"
  #changed goal tolerance for tunning
  xy_goal_tolerance: 0.4
  yaw_goal_tolerance: 0.4
  stateful: True
# DWB parameters
FollowPath:
  plugin: "dwb_core::DWBLocalPlanner"
  debug_trajectory_details: True
  min_vel_x: 0.0
  min_vel_y: 0.0
  max_vel_x: 0.2
  max_vel_y: 0.0
  max_vel_theta: 0.2
  min_speed_xy: 0.0
  max_speed_xy: 0.2
  min_speed_theta: 0.0
  # Add high threshold velocity for turtlebot 3 issue.
  # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
  acc_lim_x: 2.5
  acc_lim_y: 0.0
  acc_lim_theta: 1.2
  decel_lim_x: -2.5
  decel_lim_y: 0.0
  decel_lim_theta: -1.2
  vx_samples: 30
  vy_samples: 10
  vtheta_samples: 20
  sim_time: 1.7
  linear_granularity: 0.05
  angular_granularity: 0.025
  transform_tolerance: 0.2
  xy_goal_tolerance: 0.25
  trans_stopped_velocity: 0.25
  short_circuit_trajectory_evaluation: True
  stateful: True
  critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
  BaseObstacle.scale: 0.02
  PathAlign.scale: 32.0
  PathAlign.forward_point_distance: 0.1
  GoalAlign.scale: 24.0
  GoalAlign.forward_point_distance: 0.1
  PathDist.scale: 32.0
  GoalDist.scale: 24.0
  RotateToGoal.scale: 32.0
  RotateToGoal.slowing_factor: 1.0
  RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node: ros__parameters: use_sim_time: True

local_costmap: local_costmap: rosparameters: update_frequency: 10.0 publish_frequency: 10.0 global_frame: map robot_base_frame: base_link use_sim_time: True rolling_window: True width: 10 height: 10 resolution: 0.05 robot_radius: 0.4 plugins: ["nvblox_layer"] nvblox_layer: plugin: "nvblox::nav2::NvbloxCostmapLayer" enabled: True max_obstacle_distance: 1.0 inflation_distance: 0.4 always_send_full_costmap: True local_costmap_client: rosparameters: use_sim_time: True local_costmap_rclcpp_node: ros__parameters: use_sim_time: True

global_costmap: global_costmap: rosparameters: update_frequency: 10.0 publish_frequency: 10.0 global_frame: map robot_base_frame: base_link use_sim_time: True rolling_window: True width: 200 height: 200 robot_radius: 0.4 resolution: 0.1 origin_x: -100.0 origin_y: -100.0 plugins: ["nvblox_layer"] nvblox_layer: plugin: "nvblox::nav2::NvbloxCostmapLayer" enabled: True max_obstacle_distance: 1.0 inflation_distance: 0.4 always_send_full_costmap: True global_costmap_client: rosparameters: use_sim_time: True global_costmap_rclcpp_node: ros__parameters: use_sim_time: True

planner_server: ros__parameters: expected_planner_frequency: 10.0 use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true

planner_server_rclcpp_node: ros__parameters: use_sim_time: True

smoother_server: ros__parameters: use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" tolerance: 1.0e-10 max_its: 1000 do_refinement: True

behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 5.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] spin: plugin: "nav2_behaviors/Spin" backup: plugin: "nav2_behaviors/BackUp" drive_on_heading: plugin: "nav2_behaviors/DriveOnHeading" wait: plugin: "nav2_behaviors/Wait" global_frame: map robot_base_frame: base_link transform_tolerance: 0.2 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 0.2 min_rotational_vel: 0.05 rotational_acc_lim: 3.2

robot_state_publisher: ros__parameters: use_sim_time: True

waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200

has anyone ran into this problem? and any advises are appreciated. Thanks

chivas1000 commented 1 year ago

Sorry, the problem should be in my modifided carter_nav2.yaml that I tweaked with tolerance time and radius and acc_lim, or maybe some syntax problem, Since I use default yaml the map successfully received.

helenol commented 1 year ago

Since the default yaml is working, I'm not sure how we can help here. Nothing you've changed stands out at something that should cause this issue, but this seems like a nav2 issue rather than an nvblox one. Sorry we couldn't help more!