search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.56k stars 1.29k forks source link

No particlecloud displayed in RViz #2238

Closed EricSun787 closed 3 years ago

EricSun787 commented 3 years ago

Bug report

Required Info:

Steps to reproduce issue

I started the map_server node and amcl node （with lifecycle) , everything looks fine, but in Rviz, amcl particlecloud is not displayed ( Display plugin has been added ）

launch file: 

import os

from ament_index_python.packages import get_package_share_directory

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():

    bringup_dir = get_package_share_directory('aibot_nav2')

    namespace     = LaunchConfiguration('namespace')
    map_yaml_file = LaunchConfiguration('map')
    use_sim_time  = LaunchConfiguration('use_sim_time')
    autostart     = LaunchConfiguration('autostart')
    params_file   = LaunchConfiguration('params_file')

    lifecycle_nodes = ['map_server', 'amcl']
    #lifecycle_nodes = ['map_server']

    remappings = [('/tf','tf'),('/tf_static', 'tf_static')]

    param_substitutions = {
            'use_sim_time': use_sim_time,
            'yaml_filename': map_yaml_file}

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

    set_env_var = SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM','1')

    declare_namespace_argument = DeclareLaunchArgument(
            'namespace', default_value='',
            description='Top-level namespace')

    declare_map_argument = DeclareLaunchArgument(
            'map',
            default_value=os.path.join(bringup_dir,'maps','map.yaml'),
            description='Full path to map yaml file to load')

    declare_use_sim_time_argument = DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation clock if true')

    declare_autostart_argument = DeclareLaunchArgument(
            'autostart', default_value='true',
            description='Automatically startup the nav2 stack')

    declare_paramsfile_argument = DeclareLaunchArgument(
            'params_file',
            default_value=os.path.join(bringup_dir,'config','nav2_params.yaml'),
            description='Full path to the ROS2 parameters file to use')

    map_server_node = Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[configured_params],
            remappings=remappings)

    amcl_node = Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[configured_params],
            remappings=remappings)

    lifecycle_node = Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_localization',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': autostart},
                        {'node_names': lifecycle_nodes}])

    ld = LaunchDescription()

    ld.add_action(declare_namespace_argument)
    ld.add_action(declare_map_argument)
    ld.add_action(declare_use_sim_time_argument)
    ld.add_action(declare_autostart_argument)
    ld.add_action(declare_paramsfile_argument)

    ld.add_action(map_server_node)
    ld.add_action(amcl_node)
    ld.add_action(lifecycle_node)

    return ld

Terminal Output:

[INFO] [launch]: All log files can be found below /home/ssj/.ros/log/2021-03-11-18-05-01-885410-ubuntu-51469
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_server-1]: process started with pid [51471]
[INFO] [amcl-2]: process started with pid [51473]
[INFO] [lifecycle_manager-3]: process started with pid [51475]
[map_server-1] [INFO] [1615514702.680622217] [map_server]: 
[map_server-1]  map_server lifecycle node launched. 
[map_server-1]  Waiting on external lifecycle transitions to activate
[map_server-1]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-1] [INFO] [1615514702.680775019] [map_server]: Creating
[amcl-2] [INFO] [1615514702.683460985] [amcl]: 
[amcl-2]    amcl lifecycle node launched. 
[amcl-2]    Waiting on external lifecycle transitions to activate
[amcl-2]    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[amcl-2] [INFO] [1615514702.683645153] [amcl]: Creating
[lifecycle_manager-3] [INFO] [1615514702.688426012] [lifecycle_manager_localization]: Creating
[lifecycle_manager-3] [INFO] [1615514702.726990281] [lifecycle_manager_localization]: Creating and initializing lifecycle service clients
[lifecycle_manager-3] [INFO] [1615514702.730044638] [lifecycle_manager_localization]: Starting managed nodes bringup...
[lifecycle_manager-3] [INFO] [1615514702.730080327] [lifecycle_manager_localization]: Configuring map_server
[map_server-1] [INFO] [1615514702.730497532] [map_server]: Configuring
[map_server-1] [INFO] [map_io]: Loading yaml file: /home/ssj/dev_ws/install/aibot_nav2/share/aibot_nav2/maps/map.yaml
[map_server-1] [DEBUG] [map_io]: resolution: 0.05
[map_server-1] [DEBUG] [map_io]: origin[0]: -2.46
[map_server-1] [DEBUG] [map_io]: origin[1]: -9.63
[map_server-1] [DEBUG] [map_io]: origin[2]: 0
[map_server-1] [DEBUG] [map_io]: free_thresh: 0.196
[map_server-1] [DEBUG] [map_io]: occupied_thresh: 0.65
[map_server-1] [DEBUG] [map_io]: mode: trinary
[map_server-1] [DEBUG] [map_io]: negate: 0
[map_server-1] [INFO] [map_io]: Loading image_file: /home/ssj/dev_ws/install/aibot_nav2/share/aibot_nav2/maps/map.pgm
[map_server-1] [DEBUG] [map_io]: Read map /home/ssj/dev_ws/install/aibot_nav2/share/aibot_nav2/maps/map.pgm: 245 X 260 map @ 0.05 m/cell
[lifecycle_manager-3] [INFO] [1615514702.838635689] [lifecycle_manager_localization]: Configuring amcl
[amcl-2] [INFO] [1615514702.839112344] [amcl]: Configuring
[amcl-2] [INFO] [1615514702.843307280] [amcl]: initTransforms
[amcl-2] [INFO] [1615514702.876009777] [amcl]: initPubSub
[amcl-2] [INFO] [1615514702.894006766] [amcl]: Subscribed to map topic.
[lifecycle_manager-3] [INFO] [1615514702.899760652] [lifecycle_manager_localization]: Activating map_server
[map_server-1] [INFO] [1615514702.900844720] [map_server]: Activating
[lifecycle_manager-3] [INFO] [1615514702.903174335] [lifecycle_manager_localization]: Activating amcl
[amcl-2] [INFO] [1615514702.903274369] [amcl]: Received a 245 X 260 map @ 0.050 m/pix
[amcl-2] [INFO] [1615514702.905339838] [amcl]: Activating
[amcl-2] [WARN] [1615514702.905376053] [amcl]: Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, will be published as nav2_msgs/ParticleCloud in the future
[lifecycle_manager-3] [INFO] [1615514702.907075779] [lifecycle_manager_localization]: Managed nodes are active
[amcl-2] [INFO] [1615514703.023120521] [amcl]: createLaserObject
[amcl-2] [WARN] [1615514704.845068045] [amcl]: ACML cannot publish a pose or update the transform. Please set the initial pose...
[amcl-2] [INFO] [1615514727.314293828] [amcl]: initialPoseReceived
[amcl-2] [WARN] [1615514727.314419606] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future.  Requested time 1615514727.314331 but the latest data is at time 1615514727.301206, when looking up transform from frame [base_footprint] to frame [odom])
[amcl-2] [INFO] [1615514727.340698429] [amcl]: Setting pose (1615514727.340698): -0.474 0.177 0.049
[amcl-2] [INFO] [1615514742.797817304] [amcl]: initialPoseReceived
[amcl-2] [WARN] [1615514742.797901463] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future.  Requested time 1615514742.797867 but the latest data is at time 1615514742.772900, when looking up transform from frame [base_footprint] to frame [odom])
[amcl-2] [INFO] [1615514742.797917056] [amcl]: Setting pose (1615514742.797917): -0.366 0.281 0.527
[amcl-2] [INFO] [1615514749.502619348] [amcl]: initialPoseReceived
[amcl-2] [WARN] [1615514749.502811076] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future.  Requested time 1615514749.502677 but the latest data is at time 1615514749.478153, when looking up transform from frame [base_footprint] to frame [odom])
[amcl-2] [INFO] [1615514749.502876892] [amcl]: Setting pose (1615514749.502877): -0.070 0.206 0.412
EricSun787 commented 3 years ago

issue RViz Display

SteveMacenski commented 3 years ago

Check QoS settings and make sure its being properly published. I don't think there's anything broken about this in Nav2, no one's reported it and so far things are working fine on my end. Did you try with the Nav2 provided rviz configuration file?

SteveMacenski commented 3 years ago

ros2 launch nav2_bringup tb3_simulation_launch.py on foxy binaries,

image

looks OK to me...

EricSun787 commented 3 years ago

That's weird , I tried with the Nav2 default rviz configuration file，It also doesn't show particlecloud. issue2 You mentioned the QoS setting. Where is this parameter , it is in the launch file or in the parameter configuration file (I used the default nav2_bringup parameter configuration file). Besides, I cannot run tb3_simulation_launch.py in the virtual machine to test if this is normal on my VM , because Gazebo cannot be started. It may be because of the poor performance of my VM. I use a real robot to provide Odom data and Scan data, and TF is also correct. Thank you very much for your reply ！

EricSun787 commented 3 years ago

I found there is no messages published on /particlecloud topic. issue3

SteveMacenski commented 3 years ago

Please try our demo, I see another map so I don't know what else has been changed. Use the local demo to debug any such issues. If it works there, try to work back the issue yourself. On QoS please refer to ROS2 docs, I can't explain ROS2 basics in this ticket.

SteveMacenski commented 3 years ago

any update?

EricSun787 commented 3 years ago

Yes， I changed the location where the navigation runs. I launch the navigation on the VM before. If it launch on the robot side, particlecloud can be displayed normally. This may be related to the quality of my network environment. I did not find a detailed reason.
Actually this issue can be closed now，but I still hope to find the exact reason. Thanks