search

at-wat / mcl_3dl

A ROS node to perform a probabilistic 3-D/6-DOF localization system for mobile robots with 3-D LIDAR(s). It implements pointcloud based Monte Carlo localization that uses a reference pointcloud as a map.
BSD 3-Clause "New" or "Revised" License
502 stars 118 forks source link

I am having trouble running it on my bag file. #393

Open isaogotu opened 2 years ago

isaogotu commented 2 years ago

Nice to meet you! I am a researcher of mobile robots.

I am very much helped by at-wat's package. Thank you very much.

I would like to run mcl_3dl with a bag file observed with Velodyne 32 channel and a point cloud map constructed from the bag file. However, when I run it, the self-position estimation does not start. Any advice would be appreciated.

This is my first time asking this question, so if I am being rude, please let me know.

Here are the results of the run.

isao@isaoPC:~/catkin_at-wat$ roslaunch mcl_3dl test.launch use_neonavigation:=true without_odom:=true use_pointcloud_map:=true map_pcd:=${HOME}/Downloads/廊下2.pcd use_cad_map:=false use_bag_file:=true bag_file:=${HOME}/Downloads/廊下2.bag 
... logging to /home/isao/.ros/log/9a238e5e-141b-11ed-aa02-1b5bf0a38f45/roslaunch-isaoPC-99551.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/isao/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://isaoPC:46531/

SUMMARY
========

PARAMETERS
 * /mcl_3dl/accum_cloud: 1
 * /mcl_3dl/bias_var_dist: 100.0
 * /mcl_3dl/compatible: 1
 * /mcl_3dl/fake_odom: True
 * /mcl_3dl/likelihood/match_dist_min: 0.3
 * /mcl_3dl/likelihood/match_output_dist: 0.3
 * /mcl_3dl/lpf_step: 0.0
 * /mcl_3dl/map_frame: map
 * /mcl_3dl/num_particles: 256
 * /mcl_3dl/odom_frame: base_link
 * /mcl_3dl/resample_var_x: 0.2
 * /mcl_3dl/resample_var_y: 0.2
 * /mcl_3dl/resample_var_z: 0.05
 * /obj_to_pointcloud/downsample_grid: 0.1
 * /obj_to_pointcloud/frame_id: map
 * /obj_to_pointcloud/points_per_meter_sq: 1200.0
 * /path_recorder/ang_interval: 0.2
 * /path_recorder/dist_interval: 0.1
 * /pcd_to_pointcloud/frame_id: map
 * /pcd_to_pointcloud/latch: True
 * /poses_ref/ang_interval: 100.0
 * /poses_ref/dist_interval: 0.5
 * /robot_description: <...>
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /use_sim_time: True

NODES
  /
    mcl_3dl (mcl_3dl/mcl_3dl)
    path_recorder (trajectory_tracker/trajectory_recorder)
    pcd_to_pointcloud (pcl_ros/pcd_to_pointcloud)
    playback (rosbag/play)
    poses_ref (trajectory_tracker/trajectory_recorder)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [99559]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 9a238e5e-141b-11ed-aa02-1b5bf0a38f45
process[rosout-1]: started with pid [99569]
started core service [/rosout]
process[playback-2]: started with pid [99576]
process[rviz-3]: started with pid [99577]
process[mcl_3dl-4]: started with pid [99578]
process[pcd_to_pointcloud-5]: started with pid [99584]
process[path_recorder-6]: started with pid [99585]
process[poses_ref-7]: started with pid [99586]
process[robot_state_publisher-8]: started with pid [99587]
[ERROR] [1659634430.900386172]: ======= [Deprecated] /path_recorder is run in compatible mode =======
=========================================================
Set _compatible:=1 to switch to new topic namespaces.
Compatible mode will be obsolated in the future update.
=========================================================
[ERROR] [1659634430.919919965]: Use /path (path) topic instead of /path_recorder/recpath (~/recpath)
[ERROR] [1659634430.924809409]: ======= [Deprecated] /poses_ref is run in compatible mode =======
=========================================================
Set _compatible:=1 to switch to new topic namespaces.
Compatible mode will be obsolated in the future update.
=========================================================
[ERROR] [1659634430.935958330]: Use /path (path) topic instead of /poses_ref/recpath (~/recpath)
[ INFO] [1659634431.158663482]: map received
[ WARN] [1659634431.198846319, 1658905467.678612563]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1659634431.198843907, 1658905467.678612563]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1659634431.340250124, 1658905467.819420855]: map original: 1242179 points
[ INFO] [1659634431.340283338, 1658905467.819420855]: map reduced: 279970 points
[ WARN] [1659634431.399476705, 1658905467.879847424]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1659634431.400336448, 1658905467.879847424]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1659634431.601886843, 1658905468.081586032]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1659634431.601934151, 1658905468.081586032]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1659634431.803372075, 1658905468.283739904]: TF exception: "map" passed to lookupTransform argument target_frame does not exist.

Below is the information from the bag file.

isao@isaoPC:~/Downloads$ rosbag info 廊下2.bag  

path:        廊下2.bag 

version:     2.0 

duration:    8:14s (494s) 

start:       Jul 27 2022 16:04:27.67 (1658905467.67) 

end:         Jul 27 2022 16:12:42.05 (1658905962.05) 

size:        6.3 GB 

messages:    79593 

compression: none [4933/4933 chunks] 

types:       bond/Status                           [eacc84bf5d65b6777d4c50f463dfb9c8] 

             diagnostic_msgs/DiagnosticArray       [60810da900de1dd6ddd437c3503511da] 

             dynamic_reconfigure/Config            [958f16a05573709014982821e6822580] 

             dynamic_reconfigure/ConfigDescription [757ce9d44ba8ddd801bb30bc456f946f] 

             geometry_msgs/Twist                   [9f195f881246fdfa2798d1d3eebca84a] 

             nav_msgs/Odometry                     [cd5e73d190d741a2f92e81eda573aca7] 

             rosaria/BumperState                   [f81947761ff7e166a3bbaf937b9869b5] 

             rosgraph_msgs/Log                     [acffd30cd6b6de30f120938c17c593fb] 

             sensor_msgs/LaserScan                 [90c7ef2dc6895d81024acba2ac42f369] 

             sensor_msgs/PointCloud                [d8e9c3f5afbdd8a130fd1d2763945fca] 

             sensor_msgs/PointCloud2               [1158d486dd51d683ce2f1be655c3c181] 

             std_msgs/Bool                         [8b94c1b53db61fb6aed406028ad6332a] 

             std_msgs/Float64                      [fdb28210bfa9d7c91146260178d9a584] 

             std_msgs/Int8                         [27ffa0c9c4b8fb8492252bcad9e5c57b] 

             tf2_msgs/TFMessage                    [94810edda583a504dfda3829e70d7eec] 

             velodyne_msgs/VelodyneScan            [50804fc9533a0e579e6322c04ae70566] 

topics:      /RosAria/battery_recharge_state   1 msg     : std_msgs/Int8                         

             /RosAria/battery_voltage    5094 msgs    : std_msgs/Float64                      

             /RosAria/bumper_state     5094 msgs    : rosaria/BumperState                   

             /RosAria/cmd_vel       10084 msgs    : geometry_msgs/Twist                   

             /RosAria/motors_state      1 msg     : std_msgs/Bool                         

             /RosAria/parameter_descriptions     1 msg     : dynamic_reconfigure/ConfigDescription 

             /RosAria/parameter_updates       1 msg     : dynamic_reconfigure/Config            

             /RosAria/pose       5094 msgs    : nav_msgs/Odometry                     

             /RosAria/sonar     5094 msgs    : sensor_msgs/PointCloud                

             /RosAria/sonar_pointcloud2    5094 msgs    : sensor_msgs/PointCloud2               

             /diagnostics   981 msgs    : diagnostic_msgs/DiagnosticArray       

             /rosout    10112 msgs    : rosgraph_msgs/Log                     (3 connections) 

             /rosout_agg     10086 msgs    : rosgraph_msgs/Log                     

             /scan    4931 msgs    : sensor_msgs/LaserScan                 

             /tf  5094 msgs    : tf2_msgs/TFMessage                    

             /velodyne_nodelet_manager/bond   2970 msgs    : bond/Status      (4 connections) 

             /velodyne_nodelet_manager_driver/parameter_descriptions  1 msg     : dynamic_reconfigure/ConfigDescription 

             /velodyne_nodelet_manager_driver/parameter_updates  1 msg     : dynamic_reconfigure/Config            

             /velodyne_nodelet_manager_laserscan/parameter_descriptions  1 msg     : dynamic_reconfigure/ConfigDescription 

             /velodyne_nodelet_manager_laserscan/parameter_updates 1 msg     : dynamic_reconfigure/Config            

             /velodyne_nodelet_manager_transform/parameter_descriptions　1 msg     : dynamic_reconfigure/ConfigDescription 

             /velodyne_nodelet_manager_transform/parameter_updates    1 msg     : dynamic_reconfigure/Config            

             /velodyne_packets     4923 msgs    : velodyne_msgs/VelodyneScan            

             /velodyne_points    4932 msgs    : sensor_msgs/PointCloud2
at-wat commented 2 years ago

  1. Looks like required topics aren't connected. I guess following remapping is required:

    • /RosAria/pose -> /odom
    • /velodyne_points -> /cloud

  2. without_odom:=true is specified but IMU data is not provided This node requires at least one of odometry or IMU. (basically recommended to have both) See https://github.com/at-wat/mcl_3dl/blob/master/doc/ExperimentalDemos.md

  3. Default parameters aren't good for velodyne Horizontal scan of Velodyne is split into several pointcloud messages. In this case, the ~/accum_cloud parameter should be increased to the number of the pointcloud messages to reconstruct a full horizontal scan.

isaogotu commented 2 years ago

Thank you for your kind reply.

  1. I have changed the test.launchu file as advised in 1. as follows. Before： <remap from="horizontal_laser_3d" to="cloud"/> After： <remap from="/velodyne_points" to="cloud"/> However, I don't know which part of /odom I should change. I am sorry to trouble you, but could you please let me know?

  2. I changed the execution command as advised in 2. as follows. Before： roslaunch mcl_3dl test.launch use_neonavigation:=true without_odom=true use_pointcloud_map:=true map_pcd:=${HOME}/Downloads/廊下2.pcd use_cad_map:=false use_bag_file:=true bag_file:=${HOME}/Downloads/廊下2.bag After： roslaunch mcl_3dl test.launch use_neonavigation:=true without_imu:=true use_pointcloud_map:=true map_pcd:=${HOME}/Downloads/廊下2.pcd use_cad_map:=false use_bag_file:=true bag_file:=${HOME}/Downloads/廊下2.bag

  3. I am not sure where to adjust the parameters, although I am not competent enough regarding the part of 3. to change the parameters. I am sorry, but could you please tell me where and how to change the parameters? Thank you in advance.

isaogotu commented 2 years ago

It has been a while. Thank you for your help.

Based on your advice in 1., I rewrote the "test.launch" file as follows

<remap from="/velodyne_points" to="cloud"/>
<remap from="/RosAria/pose" to="/odom"/>

After the change, I tried to run the following command, but the error occurred again. roslaunch mcl_3dl test.launch use_neonavigation:=true without_imu:=true use_pointcloud_map:=true map_pcd:=${HOME}/Downloads/廊下2.pcd use_cad_map:=false use_bag_file:=true bag_file:=${HOME}/Downloads/廊下2.bag

The result is as follows.

... logging to /home/isao/.ros/log/58c007aa-2602-11ed-9689-c9edbca94522/roslaunch-isaoPC-262795.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/isao/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://isaoPC:38521/

SUMMARY
========

PARAMETERS
 * /mcl_3dl/acc_var: 1.2
 * /mcl_3dl/accum_cloud: 2
 * /mcl_3dl/compatible: 1
 * /mcl_3dl/fake_imu: True
 * /mcl_3dl/likelihood/match_dist_min: 0.3
 * /mcl_3dl/likelihood/match_output_dist: 0.3
 * /mcl_3dl/lpf_step: 1.0
 * /mcl_3dl/map_frame: map
 * /mcl_3dl/num_particles: 128
 * /mcl_3dl/resample_var_pitch: 0.1
 * /mcl_3dl/resample_var_roll: 0.1
 * /mcl_3dl/resample_var_yaw: 0.05
 * /obj_to_pointcloud/downsample_grid: 0.1
 * /obj_to_pointcloud/frame_id: map
 * /obj_to_pointcloud/points_per_meter_sq: 1200.0
 * /path_recorder/ang_interval: 0.2
 * /path_recorder/dist_interval: 0.1
 * /pcd_to_pointcloud/frame_id: map
 * /pcd_to_pointcloud/latch: True
 * /poses_ref/ang_interval: 100.0
 * /poses_ref/dist_interval: 0.5
 * /robot_description: <...>
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /use_sim_time: True

NODES
  /
    mcl_3dl (mcl_3dl/mcl_3dl)
    path_recorder (trajectory_tracker/trajectory_recorder)
    pcd_to_pointcloud (pcl_ros/pcd_to_pointcloud)
    playback (rosbag/play)
    poses_ref (trajectory_tracker/trajectory_recorder)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [262803]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 58c007aa-2602-11ed-9689-c9edbca94522
process[rosout-1]: started with pid [262813]
started core service [/rosout]
process[playback-2]: started with pid [262820]
process[rviz-3]: started with pid [262821]
process[mcl_3dl-4]: started with pid [262822]
process[pcd_to_pointcloud-5]: started with pid [262827]
process[path_recorder-6]: started with pid [262829]
process[poses_ref-7]: started with pid [262830]
process[robot_state_publisher-8]: started with pid [262832]
[ERROR] [1661602704.603946358]: ======= [Deprecated] /path_recorder is run in compatible mode =======
=========================================================
Set _compatible:=1 to switch to new topic namespaces.
Compatible mode will be obsolated in the future update.
=========================================================
[ERROR] [1661602704.628417213]: Use /path (path) topic instead of /path_recorder/recpath (~/recpath)
[ERROR] [1661602704.631881642]: ======= [Deprecated] /poses_ref is run in compatible mode =======
=========================================================
Set _compatible:=1 to switch to new topic namespaces.
Compatible mode will be obsolated in the future update.
=========================================================
[ERROR] [1661602704.652803786]: Use /path (path) topic instead of /poses_ref/recpath (~/recpath)
[ WARN] [1661602704.953176774, 1658905467.678391750]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1661602704.953811226, 1658905467.678391750]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1661602705.020083669, 1658905467.738745699]: map received
[ WARN] [1661602705.154825874, 1658905467.879607474]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1661602705.155166976, 1658905467.879607474]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1661602705.219543678, 1658905467.939985959]: map original: 1242179 points
[ INFO] [1661602705.219594638, 1658905467.939985959]: map reduced: 279970 points
[ WARN] [1661602705.357155983, 1658905468.081543373]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1661602705.357328758, 1658905468.081543373]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1661602705.418477274, 1658905468.142027868]: Failed to transform pointcloud: "velodyne" passed to lookupTransform argument source_frame does not exist. 
[ INFO] [1661602705.518544660, 1658905468.242761338]: Failed to transform pointcloud: "velodyne" passed to lookupTransform argument source_frame does not exist. 
[ WARN] [1661602705.558511420, 1658905468.283019513]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ WARN] [1661602705.558782444, 1658905468.283019513]: TF exception: "map" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1661602705.617990599, 1658905468.343383709]: Failed to transform pointcloud: "velodyne" passed to lookupTransform argument source_frame does not exist. 
[ INFO] [1661602705.719062220, 1658905468.444160571]: Failed to transform pointcloud: "velodyne" passed to lookupTransform argument source_frame does not exist.

Can you please give me some advice on how to solve these problems? Thank you in advance for your time.