dddmr_navigation An error occurred when mapping lego_loam.launch using the RS-Helios-5515 radar

~/dddmr_navigation$ ros2 launch lego_loam_bor lego_loam.launch [INFO] [launch]: All log files can be found below /home/unitree/.ros/log/2025-07-18-14-56-36-244880-unitree-b2-pc4-60256 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [lego_loam-1]: process started with pid [60257] [INFO] [rviz2-2]: process started with pid [60259] [lego_loam-1] [INFO] [1752821796.389351773] [lego_loam_ip]: laser.num_vertical_scans: 32 [lego_loam-1] [INFO] [1752821796.389511461] [lego_loam_ip]: laser.num_horizontal_scans: 1800 [lego_loam-1] [INFO] [1752821796.389589665] [lego_loam_ip]: laser.scan_period: 0.10 [lego_loam-1] [INFO] [1752821796.389654745] [lego_loam_ip]: laser.vertical_angle_bottom: -55.00 [lego_loam-1] [INFO] [1752821796.389709330] [lego_loam_ip]: laser.vertical_angle_top: 15.00 [lego_loam-1] [INFO] [1752821796.389766807] [lego_loam_ip]: imageProjection.segment_theta: 30.00 [lego_loam-1] [INFO] [1752821796.389820494] [lego_loam_ip]: imageProjection.segment_valid_point_num: 5 [lego_loam-1] [INFO] [1752821796.389872918] [lego_loam_ip]: imageProjection.segment_valid_line_num: 3 [lego_loam-1] [INFO] [1752821796.389942906] [lego_loam_ip]: laser.ground_scan_index: 20 [lego_loam-1] [INFO] [1752821796.390009226] [lego_loam_ip]: laser.odom_type: laser_odometry [lego_loam-1] [INFO] [1752821796.390062297] [lego_loam_ip]: laser.baselink_frame: robot_center [lego_loam-1] [INFO] [1752821796.390117214] [lego_loam_ip]: imageProjection.maximum_detection_range: 150.00 [lego_loam-1] [INFO] [1752821796.390170609] [lego_loam_ip]: imageProjection.minimum_detection_range: 0.50 [lego_loam-1] [INFO] [1752821796.390222514] [lego_loam_ip]: imageProjection.distance_for_patch_between_rings: 5.00 [lego_loam-1] [INFO] [1752821796.399376360] [lego_loam_fa]: featureAssociation.edge_threshold: 0.10 [lego_loam-1] [INFO] [1752821796.399434038] [lego_loam_fa]: featureAssociation.surf_threshold: 0.10 [lego_loam-1] [INFO] [1752821796.399463683] [lego_loam_fa]: featureAssociation.nearest_feature_search_distance: 3.00 [lego_loam-1] [INFO] [1752821796.399501930] [lego_loam_fa]: mapping.to_map_optimization: 1 [lego_loam-1] [INFO] [1752821796.412126932] [lego_loam_mo]: mapping.enable_loop_closure: 1 [lego_loam-1] [INFO] [1752821796.412193417] [lego_loam_mo]: mapping.history_keyframe_search_radius: 15.00 [lego_loam-1] [INFO] [1752821796.412232199] [lego_loam_mo]: mapping.history_keyframe_search_num: 5 [lego_loam-1] [INFO] [1752821796.412259577] [lego_loam_mo]: mapping.history_keyframe_fitness_score: 0.50 [lego_loam-1] [INFO] [1752821796.412289374] [lego_loam_mo]: mapping.surrounding_keyframe_search_num: 10 [lego_loam-1] [INFO] [1752821796.412320748] [lego_loam_mo]: mapping.angle_between_key_frame: 1.00 [lego_loam-1] [INFO] [1752821796.412350439] [lego_loam_mo]: mapping.distance_between_key_frame: 1.00 [lego_loam-1] [INFO] [1752821796.412378470] [lego_loam_mo]: mapping.ground_voxel_size: 0.40 [lego_loam-1] [INFO] [1752821796.412409041] [lego_loam_mo]: mapping.ground_edge_threshold_num: 600 [rviz2-2] [INFO] [1752821796.526732975] [rviz2]: Stereo is NOT SUPPORTED [rviz2-2] [INFO] [1752821796.526789588] [rviz2]: OpenGl version: 4.5 (GLSL 4.5) [rviz2-2] [INFO] [1752821796.544710170] [rviz2]: Stereo is NOT SUPPORTED [ERROR] [lego_loam-1]: process has died [pid 60257, exit code -11, cmd '/home/unitree/dddmr_navigation/install/lego_loam_bor/lib/lego_loam_bor/lego_loam --ros-args --params-file /home/unitree/dddmr_navigation/install/lego_loam_bor/share/lego_loam_bor/config/loam_rs32_config.yaml -r /lslidar_point_cloud:=/rslidar_points -r /odom:=/dog_odom']. [rviz2-2] [WARN] [1752821796.634526911] [pluginlib.ClassLoader]: given plugin name 'libmapping_panel' should be 'mapping_panel' for better portability [rviz2-2] [WARN] [1752821796.638732604] [pluginlib.ClassLoader]: given plugin name 'libpose_graph_editor_panel' should be 'pose_graph_editor_panel' for better portability

My configuration is as follows： lego_loam_ip: ros__parameters: laser: odom_type: "laser_odometry"
baselink_frame: "robot_center" num_vertical_scans: 32
num_horizontal_scans: 1800
ground_scan_index: 20
vertical_angle_bottom: -55.0
vertical_angle_top: 15.0
scan_period: 0.1

imageProjection:
    segment_valid_point_num: 5
    segment_valid_line_num: 3           
    segment_theta: 30.0                 
    maximum_detection_range: 150.0     
    minimum_detection_range: 0.5        
    distance_for_patch_between_rings: 5.0

lego_loam_fa: ros__parameters: featureAssociation: edge_threshold: 0.1
surf_threshold: 0.1
nearest_feature_search_distance: 3.0

mapping:
    to_map_optimization: true

lego_loam_mo: ros__parameters: mapping: distance_between_key_frame: 1.0
angle_between_key_frame: 1.0

    enable_loop_closure: true

    surrounding_keyframe_search_num: 10  
    history_keyframe_search_radius: 15.0 
    history_keyframe_search_num: 5      
    history_keyframe_fitness_score: 0.5 
    ground_voxel_size: 0.4              
    ground_edge_threshold_num: 600

Jul 18 '25 06:07 adsaosdi

Execution crashed. Could you send me a bag file of your point cloud, 1 miniute bag file is enough.

Jul 18 '25 10:07 tsengapola

I shared "rosbag2_2025_07_18-23_00_29.zip" via Quark Disk. Just click the link to save it. Link: https://pan.quark.cn/s/8c7ee90955d4

Jul 18 '25 15:07 adsaosdi

Thank you for your bag file. I have tested it, and look like it works fine.

https://github.com/user-attachments/assets/ffa20eba-3f15-44ed-82b8-fe51d0ad2df5

Please try branch: https://github.com/dfl-rlab/dddmr_navigation/tree/issue7 ''' ros2 launch lego_loam_bor lego_loam_bag.launch ''' A question: Does your crashing occur every time?

Jul 19 '25 06:07 tsengapola

Yes, such mistakes occur every time

Jul 19 '25 07:07 adsaosdi

Please try new branch, I want to reproduce the issue so i can fix it.

Jul 19 '25 07:07 tsengapola

lego_loam_ip: ros__parameters: laser: odom_type: "laser_odometry"
baselink_frame: "robot_center" num_vertical_scans: 32
num_horizontal_scans: 1800
ground_scan_index: 20
vertical_angle_bottom: -55.0
vertical_angle_top: 15.0
scan_period: 0.1

imageProjection:
    segment_valid_point_num: 5
    segment_valid_line_num: 2           
    segment_theta: 60.0                 
    maximum_detection_range: 120.0     
    minimum_detection_range: 0.5        
    distance_for_patch_between_rings: 1.0

lego_loam_fa: ros__parameters: featureAssociation: edge_threshold: 0.1
surf_threshold: 0.1
nearest_feature_search_distance: 3.0

mapping:
    to_map_optimization: true

lego_loam_mo: ros__parameters: mapping: distance_between_key_frame: 1.0
angle_between_key_frame: 1.0

    enable_loop_closure: true

    surrounding_keyframe_search_num: 10  
    history_keyframe_search_radius: 15.0 
    history_keyframe_search_num: 5      
    history_keyframe_fitness_score: 0.5 
    ground_voxel_size: 0.4              
    ground_edge_threshold_num: 60

I configured it this way and started the mapping function, but the same error message still appeared.

Jul 19 '25 07:07 adsaosdi

I think your tf is correct. Use new branch and compile it then run:

ros2 launch lego_loam_bor lego_loam.launch

I am online, keep me posted! By the way, do you use our docker image?

Jul 19 '25 07:07 tsengapola

I'm not very good at using github. What do you mean by trying new branches?
I didn't use your docker. I deployed your project locally on my computer

Jul 19 '25 07:07 adsaosdi

just cd to our repo and check out

cd dddmr
git pull
git checkout issue7
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

I already setup the params for your lidar, so you don't need to modify anything. 2. I recommend you to use our docker image because we found sometime PCL and GTSAM are not compatible if not build correctly.

Jul 19 '25 08:07 tsengapola

1.Can I directly copy your modified parameter configuration and startup file to my local project to run? I'm really not quite clear about the operation of pulling a new branch to run. When I followed your operation, the following error occurred: error: Your local modifications to the following files will be overridden by the detected operation: src/dddmr_lego_loam/lego_loam_bor/launch/lego_loam.launch Please submit or save your modifications before switching branches. So I directly copied your modified parameter configuration and startup file to my local project to run, and the same error occurred.

lego_loam_ip: ros__parameters: # leishen16 laser: odom_type: "laser_odometry" #wheel_odometry/laser_odometry baselink_frame: "robot_center" num_vertical_scans: 32 num_horizontal_scans: 1800 ground_scan_index: 20 vertical_angle_bottom: -55.0 # degrees vertical_angle_top: 15.0 # degrees scan_period: 0.1 # seconds

imageProjection:
    segment_valid_point_num: 5
    segment_valid_line_num: 2
    segment_theta: 60.0                      # decrese this value may improve accuracy
    maximum_detection_range: 120.0            # point cloud within this range will be used to project image
    distance_for_patch_between_rings: 1.0    # patch ground between rings within this range, i.e. ring0 and ring1 is smaller than 1 meter, patch them

lego_loam_fa: ros__parameters:

featureAssociation:
    edge_threshold: 0.1
    surf_threshold: 0.1
    nearest_feature_search_distance: 3.0
    
mapping:
    to_map_optimization: true

lego_loam_mo: ros__parameters: mapping:

    distance_between_key_frame: 1.0
    angle_between_key_frame: 1.0

    enable_loop_closure: true

    surrounding_keyframe_search_num: 10        
    history_keyframe_search_radius: 15.0  
    history_keyframe_search_num: 5
    history_keyframe_fitness_score: 0.5
    ground_voxel_size: 0.4
    ground_edge_threshold_num: 60

2.I saw that your docker dependencies include pcl1.15, Ros2, Humble, and gtsam4.2. I have all of these dependencies. Specifically as follows: unitree@unitree-b2-pc4:~/dddmr_navigation$ dpkg -l | grep gtsam ii ros-humble-gtsam 4.2.0-3jammy.20250522.073333 amd64 gtsam unitree@unitree-b2-pc4:~/dddmr_navigation$ pkg-config --modversion pcl_common 1.15.0

I really don't know where the problem lies.

I once used gdb for debugging and found that the problem occurred on line 1821 in mapOptimization.cpp isam->update(gtSAMgraph, initialEstimate);

Jul 19 '25 08:07 adsaosdi

There has been a lot of source code being optimized. Here is my suggestion:

Rename your current dddmr to dddmr_backup, so that yon can come back to your work later.
git clone -b issue7 https://github.com/dfl-rlab/dddmr_navigation.git

From your gdb log, I am confident that it is due to GTSAM and PCL building issue, you should use our docker image or rebuild your PCL and GTSAM with proper arguments!!! Because I meet similar issue before (source code were compiled and without any complaint), and it took me a lot of time to figure out that PCL and GTSAM have to be built be with proper arguments.

Noted that following argument for both PCL and GTSAM are must when you build: PCL: -DPCL_ENABLE_AVX=OFF -DPCL_ENABLE_SSE=OFF -DPCL_ENABLE_MARCHNATIVE=OFF GTSAM: -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_SYSTEM_METIS=ON -DPCL_ENABLE_MARCHNATIVE=OFF

You can check how we build PCL and GTSAM:

https://github.com/dfl-rlab/dddmr_navigation/blob/47a075dbe7bd0be2ece946a19632d7b709e61faa/dddmr_docker/docker_file/Dockerfile_x64#L43

https://github.com/dfl-rlab/dddmr_navigation/blob/47a075dbe7bd0be2ece946a19632d7b709e61faa/dddmr_docker/docker_file/Dockerfile_x64#L51

Jul 19 '25 09:07 tsengapola

Hello, I would like to ask, when I compile and install the PCL1.15 library, should I first replace the corresponding files in the PCL1.15 library with the two files "voxel_grid.h, voxel_grid.hpp" in docker pkgs, and then follow your steps for compilation and installation? Because I found that if I directly follow your steps to compile, an error message about using the relevant functions in these two files will occur. I want to know what the roles of these two files are in docker? How should it be deployed to my computer? Looking forward to your reply.

Jul 21 '25 07:07 adsaosdi

Hi, don't use them, they are deprecated. It was used to deal with super large point cloud and I think new PCL version should solve it already.

Jul 21 '25 07:07 tsengapola

Ok, thanks. But when I tried to run it again on my computer following the installation method of your docker dependencies, a new error occurred. It occurs near line 468 of imageProjection.cpp "dsf_patched_ground_.setInputCloud(patched_ground_);" dsf_patched_ground_.filter(*patched_ground_);” When I was debugging with gdb, I found that the getMinMax3D function of PCL received a null pointer or an invalid field name parameter, which caused a segment error in the strlen() function when calculating the string length. The error report is as follows: Point cloud fields: x y z intensity

Thread 23 "lego_loam" received signal SIGSEGV, Segmentation fault. [Switching to Thread 0x7fffb9ffb640 (LWP 923371)] __strlen_avx2 () at ../sysdeps/x86_64/multiarch/strlen-avx2.S:74 74 ../sysdeps/x86_64/multiarch/strlen-avx2.S: 没有那个文件或目录. (gdb) bt #0 __strlen_avx2 () at ../sysdeps/x86_64/multiarch/strlen-avx2.S:74 #1 0x00007ffff526bd31 in __vfprintf_internal (s=s@entry=0x7fffb9fef790, format=0x7ffff4df3920 "[pcl::getMinMax3D] Could not find field with name '%s'!\n", ap=0x7fffb9ff18d0, mode_flags=2) at ./stdio-common/vfprintf-internal.c:1517 #2 0x00007ffff526c665 in buffered_vfprintf (s=0x7ffff54106a0 <IO_2_1_stderr>, format=, args=, mode_flags=) at ./stdio-common/vfprintf-internal.c:2261 #3 0x00007ffff7ef0602 in pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...) () from /usr/local/lib/libpcl_common.so.1.15

Jul 21 '25 08:07 adsaosdi

Can you do:

cd dddmr_navigation
git log

Also make sure you did not use the voxel files in the docker folder.

Jul 21 '25 08:07 tsengapola

unitree@unitree-b2-pc4:~/dddmr_navigation$ git log commit 47a075dbe7bd0be2ece946a19632d7b709e61faa (HEAD -> main, origin/main, origin/HEAD) Author: apola [email protected] Date: Tue Jul 15 19:18:44 2025 +0400

update realsense lib/ros in l4t image

:

Is it done like this? I'm sure I didn't use the header files in docker

Jul 21 '25 09:07 adsaosdi

Then I will suggest to use docker. Which type of computer you are using? If it is x64 then please do:

/dddmr_navigation/dddmr_docker/docker_file
./build.bash

Jul 21 '25 09:07 pstsengb

If I use docker, can the radar and other data of my quadruped robot be transmitted to the dddmr system in docker? Can it control my robot?

Jul 21 '25 09:07 adsaosdi

Yes, you just need to run our code in docker. You can keep rest of your nodes running in the host. Once the docker is bring up, simply do ros2 topic list, and echo lidar topic, if you can see lidar topic in the docker, you should be fine.

Jul 21 '25 10:07 tsengapola

Hello, I still haven't solved the previous environmental problem, but I have reused a new location algorithm, avoiding files such as poses.pcd that must be used for your location algorithm; But I have encountered a new problem. When I have planned the path in the map, the output of the /cmd_vel topic is always 0.

Jul 28 '25 08:07 adsaosdi

Please provide the log shown in the terminal, there should be some complaints from p2p_move_base

Jul 28 '25 08:07 tsengapola

unitree@unitree-b2-pc4:~/dddmr_navigation$ ros2 launch p2p_move_base b2_nav3d.launch.py [INFO] [launch]: All log files can be found below /home/unitree/.ros/log/2025-07-28-16-50-55-004426-unitree-b2-pc4-6952 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [ros2-1]: process started with pid [6953] [INFO] [dog_odom_to_tf_node-2]: process started with pid [6955] [INFO] [pcl_publisher-3]: process started with pid [6957] [INFO] [global_planner_node-4]: process started with pid [6959] [INFO] [p2p_move_base_node-5]: process started with pid [6961] [INFO] [clicked2goal.py-6]: process started with pid [6963] [INFO] [rviz2-7]: process started with pid [6965] [global_planner_node-4] [INFO] [1753692655.109398814] [perception_3d_global]: global_frame: map [global_planner_node-4] [INFO] [1753692655.109495195] [perception_3d_global]: robot_base_frame: robot_footprint [global_planner_node-4] [INFO] [1753692655.109536050] [perception_3d_global]: max_obstacle_distance: 50.0 [global_planner_node-4] [INFO] [1753692655.109563854] [perception_3d_global]: inscribed_radius: 0.50 [global_planner_node-4] [INFO] [1753692655.109590449] [perception_3d_global]: inflation_descending_rate: 2.00 [global_planner_node-4] [INFO] [1753692655.109615727] [perception_3d_global]: inflation_radius: 0.50 [global_planner_node-4] [INFO] [1753692655.109640756] [perception_3d_global]: sensors_collected_frequency: 10.00 [global_planner_node-4] [INFO] [1753692655.109666389] [perception_3d_global]: dgraph_publish_frequency: 1.00 [global_planner_node-4] [INFO] [1753692655.112108307] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [p2p_move_base_node-5] [INFO] [1753692655.152059423] [trajectory_generators]: Use theory name: differential_drive_simple ---> trajectory_generators::DDSimpleTrajectoryGeneratorTheory [p2p_move_base_node-5] [INFO] [1753692655.155166001] [trajectory_generators.differential_drive_simple]: min_vel_x: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.155273748] [trajectory_generators.differential_drive_simple]: max_vel_x: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.155344103] [trajectory_generators.differential_drive_simple]: min_vel_theta: 0.15 [p2p_move_base_node-5] [INFO] [1753692655.155404351] [trajectory_generators.differential_drive_simple]: max_vel_theta: 0.60 [p2p_move_base_node-5] [INFO] [1753692655.155461674] [trajectory_generators.differential_drive_simple]: acc_lim_x: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.155517473] [trajectory_generators.differential_drive_simple]: acc_lim_theta: 3.00 [p2p_move_base_node-5] [INFO] [1753692655.155575507] [trajectory_generators.differential_drive_simple]: prune_forward: 3.00 [p2p_move_base_node-5] [INFO] [1753692655.155649961] [trajectory_generators.differential_drive_simple]: prune_backward: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.155717118] [trajectory_generators.differential_drive_simple]: deceleration_ratio: 2.00 [p2p_move_base_node-5] [INFO] [1753692655.155781498] [trajectory_generators.differential_drive_simple]: max_motor_shaft_rpm: 3000.00 [p2p_move_base_node-5] [INFO] [1753692655.155844957] [trajectory_generators.differential_drive_simple]: wheel_diameter: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.155911730] [trajectory_generators.differential_drive_simple]: gear_ratio: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.155977532] [trajectory_generators.differential_drive_simple]: robot_radius: 0.60 [p2p_move_base_node-5] [INFO] [1753692655.156207773] [trajectory_generators.differential_drive_simple]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.156274575] [trajectory_generators.differential_drive_simple]: sim_time: 2.00 [p2p_move_base_node-5] [INFO] [1753692655.156336751] [trajectory_generators.differential_drive_simple]: linear_x_sample: 5.00 [p2p_move_base_node-5] [INFO] [1753692655.156393003] [trajectory_generators.differential_drive_simple]: angular_z_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.156452640] [trajectory_generators.differential_drive_simple]: sim_granularity: 0.05 [p2p_move_base_node-5] [INFO] [1753692655.156509774] [trajectory_generators.differential_drive_simple]: angular_sim_granularity: 0.03 [p2p_move_base_node-5] [INFO] [1753692655.156518407] [trajectory_generators.differential_drive_simple]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753692655.156642227] [trajectory_generators.differential_drive_simple]: Cuboid flb: 0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.156734798] [trajectory_generators.differential_drive_simple]: Cuboid frb: 0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.156800713] [trajectory_generators.differential_drive_simple]: Cuboid flt: 0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.156861708] [trajectory_generators.differential_drive_simple]: Cuboid frt: 0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.156943889] [trajectory_generators.differential_drive_simple]: Cuboid blb: -0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.157010808] [trajectory_generators.differential_drive_simple]: Cuboid brb: -0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.157069710] [trajectory_generators.differential_drive_simple]: Cuboid blt: -0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.157130672] [trajectory_generators.differential_drive_simple]: Cuboid brt: -0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.157357767] [trajectory_generators]: Use theory name: differential_drive_rotate_inplace ---> trajectory_generators::DDRotateInplaceTheory [p2p_move_base_node-5] [INFO] [1753692655.157620728] [trajectory_generators.differential_drive_rotate_inplace]: min_vel_x: 0.01 [p2p_move_base_node-5] [INFO] [1753692655.157688240] [trajectory_generators.differential_drive_rotate_inplace]: max_vel_x: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.157754189] [trajectory_generators.differential_drive_rotate_inplace]: min_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.157812916] [trajectory_generators.differential_drive_rotate_inplace]: max_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.157873820] [trajectory_generators.differential_drive_rotate_inplace]: acc_lim_x: 0.30 [p2p_move_base_node-5] [INFO] [1753692655.157947447] [trajectory_generators.differential_drive_rotate_inplace]: acc_lim_theta: 0.50 [p2p_move_base_node-5] [INFO] [1753692655.158013244] [trajectory_generators.differential_drive_rotate_inplace]: prune_forward: 3.00 [p2p_move_base_node-5] [INFO] [1753692655.158069980] [trajectory_generators.differential_drive_rotate_inplace]: prune_backward: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.158332335] [trajectory_generators.differential_drive_rotate_inplace]: max_motor_shaft_rpm: 3000.00 [p2p_move_base_node-5] [INFO] [1753692655.158395321] [trajectory_generators.differential_drive_rotate_inplace]: wheel_diameter: 0.15 [p2p_move_base_node-5] [INFO] [1753692655.158452016] [trajectory_generators.differential_drive_rotate_inplace]: gear_ratio: 30.00 [p2p_move_base_node-5] [INFO] [1753692655.158512297] [trajectory_generators.differential_drive_rotate_inplace]: robot_radius: 0.25 [p2p_move_base_node-5] [INFO] [1753692655.158592892] [trajectory_generators.differential_drive_rotate_inplace]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.158654216] [trajectory_generators.differential_drive_rotate_inplace]: sim_time: 2.00 [p2p_move_base_node-5] [INFO] [1753692655.158712553] [trajectory_generators.differential_drive_rotate_inplace]: linear_x_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.158772563] [trajectory_generators.differential_drive_rotate_inplace]: angular_z_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.158828956] [trajectory_generators.differential_drive_rotate_inplace]: sim_granularity: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.158889988] [trajectory_generators.differential_drive_rotate_inplace]: angular_sim_granularity: 0.05 [p2p_move_base_node-5] [INFO] [1753692655.158949468] [trajectory_generators.differential_drive_rotate_inplace]: rotation_speed: 0.50 [p2p_move_base_node-5] [INFO] [1753692655.158958336] [trajectory_generators.differential_drive_rotate_inplace]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753692655.159021109] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid flb: 0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.159098181] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid frb: 0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.159158938] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid flt: 0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.159219500] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid frt: 0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.159276019] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid blb: -0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.159335922] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid brb: -0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.159390884] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid blt: -0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.159449064] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid brt: -0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.159532986] [trajectory_generators]: Use theory name: differential_drive_rotate_shortest_angle ---> trajectory_generators::DDRotateInplaceTheory [p2p_move_base_node-5] [INFO] [1753692655.159720100] [trajectory_generators.differential_drive_rotate_shortest_angle]: min_vel_x: 0.01 [p2p_move_base_node-5] [INFO] [1753692655.159783183] [trajectory_generators.differential_drive_rotate_shortest_angle]: max_vel_x: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.159844573] [trajectory_generators.differential_drive_rotate_shortest_angle]: min_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.159907104] [trajectory_generators.differential_drive_rotate_shortest_angle]: max_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.159967131] [trajectory_generators.differential_drive_rotate_shortest_angle]: acc_lim_x: 0.30 [p2p_move_base_node-5] [INFO] [1753692655.160023972] [trajectory_generators.differential_drive_rotate_shortest_angle]: acc_lim_theta: 0.50 [p2p_move_base_node-5] [INFO] [1753692655.160079694] [trajectory_generators.differential_drive_rotate_shortest_angle]: prune_forward: 3.00 [p2p_move_base_node-5] [INFO] [1753692655.160141689] [trajectory_generators.differential_drive_rotate_shortest_angle]: prune_backward: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.160205898] [trajectory_generators.differential_drive_rotate_shortest_angle]: max_motor_shaft_rpm: 3000.00 [p2p_move_base_node-5] [INFO] [1753692655.160264311] [trajectory_generators.differential_drive_rotate_shortest_angle]: wheel_diameter: 0.15 [p2p_move_base_node-5] [INFO] [1753692655.160324240] [trajectory_generators.differential_drive_rotate_shortest_angle]: gear_ratio: 30.00 [p2p_move_base_node-5] [INFO] [1753692655.160379875] [trajectory_generators.differential_drive_rotate_shortest_angle]: robot_radius: 0.25 [p2p_move_base_node-5] [INFO] [1753692655.160457471] [trajectory_generators.differential_drive_rotate_shortest_angle]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.160521489] [trajectory_generators.differential_drive_rotate_shortest_angle]: sim_time: 2.00 [p2p_move_base_node-5] [INFO] [1753692655.160580116] [trajectory_generators.differential_drive_rotate_shortest_angle]: linear_x_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.160669233] [trajectory_generators.differential_drive_rotate_shortest_angle]: angular_z_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.160735317] [trajectory_generators.differential_drive_rotate_shortest_angle]: sim_granularity: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.160806984] [trajectory_generators.differential_drive_rotate_shortest_angle]: angular_sim_granularity: 0.05 [p2p_move_base_node-5] [INFO] [1753692655.160869024] [trajectory_generators.differential_drive_rotate_shortest_angle]: rotation_speed: 0.50 [p2p_move_base_node-5] [INFO] [1753692655.160878713] [trajectory_generators.differential_drive_rotate_shortest_angle]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753692655.160945822] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid flb: 0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.161009699] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid frb: 0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.161072573] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid flt: 0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.161133737] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid frt: 0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.161213372] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid blb: -0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.161273649] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid brb: -0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753692655.161335774] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid blt: -0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.161396815] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid brt: -0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753692655.164498624] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::CollisionModel [p2p_move_base_node-5] [INFO] [1753692655.168205128] [mpc_critics.collision]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.168310963] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::StickPathModel [p2p_move_base_node-5] [INFO] [1753692655.168418905] [mpc_critics.stick_path]: weight: 0.10 [p2p_move_base_node-5] [INFO] [1753692655.168465945] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::PurePursuitModel [p2p_move_base_node-5] [INFO] [1753692655.168509412] [mpc_critics.pure_pursuit]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.168540987] [mpc_critics.pure_pursuit]: translation_weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.168566911] [mpc_critics.pure_pursuit]: orientation_weight: 0.01 [p2p_move_base_node-5] [INFO] [1753692655.168615059] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::TowardGlobalPlanModel [p2p_move_base_node-5] [INFO] [1753692655.168668648] [mpc_critics.toward_global_plan]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.168735641] [mpc_critics]: Trajectory generator: differential_drive_rotate_inplace using ---> mpc_critics::CollisionModel [p2p_move_base_node-5] [INFO] [1753692655.168811237] [mpc_critics.collision_rotate]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.168902725] [mpc_critics]: Trajectory generator: differential_drive_rotate_shortest_angle using ---> mpc_critics::CollisionModel [p2p_move_base_node-5] [INFO] [1753692655.168983771] [mpc_critics.collision_rotate_shortest]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.169053936] [mpc_critics]: Trajectory generator: differential_drive_rotate_shortest_angle using ---> mpc_critics::ShortestAngleModel [p2p_move_base_node-5] [INFO] [1753692655.169135202] [mpc_critics.prefer_rotate_shortest]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753692655.169217974] [perception_3d_local]: global_frame: map [p2p_move_base_node-5] [INFO] [1753692655.169291184] [perception_3d_local]: robot_base_frame: robot_footprint [p2p_move_base_node-5] [INFO] [1753692655.169340886] [perception_3d_local]: max_obstacle_distance: 50.0 [p2p_move_base_node-5] [INFO] [1753692655.169385708] [perception_3d_local]: inscribed_radius: 0.50 [p2p_move_base_node-5] [INFO] [1753692655.169429685] [perception_3d_local]: inflation_descending_rate: 2.00 [p2p_move_base_node-5] [INFO] [1753692655.169469174] [perception_3d_local]: inflation_radius: 0.50 [p2p_move_base_node-5] [INFO] [1753692655.169515252] [perception_3d_local]: sensors_collected_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692655.169548538] [perception_3d_local]: dgraph_publish_frequency: 0.00 [p2p_move_base_node-5] [INFO] [1753692655.171535302] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [ros2-1] [INFO] [launch]: All log files can be found below /home/unitree/.ros/log/2025-07-28-16-50-55-288542-unitree-b2-pc4-6953 [ros2-1] [INFO] [launch]: Default logging verbosity is set to INFO [pcl_publisher-3] [INFO] [1753692655.346406567] [pcl_publisher]: map_dir: /home/unitree/dddmr_navigation/install/global_planner/share/global_planner/data/map.pcd [pcl_publisher-3] [INFO] [1753692655.346562338] [pcl_publisher]: ground_dir: /home/unitree/dddmr_navigation/install/global_planner/share/global_planner/data/ground.pcd [pcl_publisher-3] [INFO] [1753692655.346621102] [pcl_publisher]: global_frame: map [pcl_publisher-3] [INFO] [1753692655.346854793] [pcl_publisher]: map_rotate_around_x: 0.00 [pcl_publisher-3] [INFO] [1753692655.346930899] [pcl_publisher]: map_rotate_around_y: 0.00 [pcl_publisher-3] [INFO] [1753692655.346994314] [pcl_publisher]: map_rotate_around_z: 0.00 [pcl_publisher-3] [INFO] [1753692655.347055457] [pcl_publisher]: map_translate_x: 0.00 [pcl_publisher-3] [INFO] [1753692655.347146146] [pcl_publisher]: map_translate_y: 0.00 [pcl_publisher-3] [INFO] [1753692655.347202201] [pcl_publisher]: map_translate_z: 0.00 [pcl_publisher-3] [INFO] [1753692655.347257325] [pcl_publisher]: ground_rotate_around_x: 0.00 [pcl_publisher-3] [INFO] [1753692655.347312765] [pcl_publisher]: ground_rotate_around_y: 0.00 [pcl_publisher-3] [INFO] [1753692655.347366922] [pcl_publisher]: ground_rotate_around_z: 0.00 [pcl_publisher-3] [INFO] [1753692655.347420155] [pcl_publisher]: ground_translate_x: 0.00 [pcl_publisher-3] [INFO] [1753692655.347476326] [pcl_publisher]: ground_translate_y: 0.00 [pcl_publisher-3] [INFO] [1753692655.347534414] [pcl_publisher]: ground_translate_z: 0.00 [pcl_publisher-3] [INFO] [1753692655.347593103] [pcl_publisher]: map_down_sample: 0.30 [pcl_publisher-3] [INFO] [1753692655.347649719] [pcl_publisher]: ground_down_sample: 0.40 [rviz2-7] [INFO] [1753692655.440383971] [rviz2]: Stereo is NOT SUPPORTED [rviz2-7] [INFO] [1753692655.440444247] [rviz2]: OpenGl version: 4.5 (GLSL 4.5) [rviz2-7] [INFO] [1753692655.471455481] [rviz2]: Stereo is NOT SUPPORTED [global_planner_node-4] [INFO] [1753692655.612238721] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [pcl_publisher-3] [INFO] [1753692655.630427103] [pcl_publisher]: Map pointcloud size: 3467288 [p2p_move_base_node-5] [INFO] [1753692655.671619903] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [pcl_publisher-3] [INFO] [1753692655.752700087] [pcl_publisher]: Map pointcloud size after down size: 12467 [pcl_publisher-3] [INFO] [1753692655.818964798] [pcl_publisher]: Ground pointcloud size: 815624 [pcl_publisher-3] [INFO] [1753692655.848033339] [pcl_publisher]: Ground pointcloud size after down size: 1743 [global_planner_node-4] [INFO] [1753692656.112170228] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [p2p_move_base_node-5] [INFO] [1753692656.171603792] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [global_planner_node-4] [INFO] [1753692656.612166817] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [p2p_move_base_node-5] [INFO] [1753692656.671607650] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [ros2-1] [INFO] [icp_localization-1]: process started with pid [7157] [ros2-1] [icp_localization-1] [INFO] [1753692657.037457982] [pcl_loader_node]: Received and converted map cloud from /mapcloud. Points: 12467, Width: 12467, Frame: map [ros2-1] [icp_localization-1] [INFO] [1753692657.037605389] [pcl_loader_node]: Map received immediately from latched queue [ros2-1] [icp_localization-1] Map size is: 12467(befor filter), 12467(after filter) [ros2-1] [icp_localization-1] Init pose set to, xyz: 0 0 0, q: 0 0 0 1, rpy: 0 0 0 deg [ros2-1] [icp_localization-1] odometry data topic: /dog_odom [ros2-1] [icp_localization-1] imu data topic: /dog_imu_raw [ros2-1] [icp_localization-1] Is use odometry: true [ros2-1] [icp_localization-1] Gravity vector filter time constant: 0.01 [ros2-1] [icp_localization-1] Min num odom measurements before ready: 5 [ros2-1] [icp_localization-1] Calibration: [ros2-1] [icp_localization-1] imu to range sensor: t:[0.342180, 0.023410, 0.129240] ; q:[0.000000, 0.000000, 0.000000, 1.000000] ; rpy (deg):[0.000000, 0.000000, 0.000000] [ros2-1] [icp_localization-1] [ros2-1] [icp_localization-1] odometry source to range sensor: t:[0.000000, 0.000000, 0.000000] ; q:[0.000000, 0.000000, 0.000000, 1.000000] ; rpy (deg):[0.000000, 0.000000, 0.000000] [ros2-1] [icp_localization-1] [ros2-1] [icp_localization-1] range data parameters: [ros2-1] [icp_localization-1] topic: /rslidar_points [ros2-1] [icp_localization-1] num range data accumulated: 1 [ros2-1] [icp_localization-1]
[ros2-1] [icp_localization-1] Setting fixed frame to: map [ros2-1] [icp_localization-1] ICPlocalization: Initialized [ros2-1] [icp_localization-1] succesfully initialized icp [global_planner_node-4] [INFO] [1753692657.112184131] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Could not find a connection between 'map' and 'robot_footprint' because they are not part of the same tree.Tf has two or more unconnected trees. [p2p_move_base_node-5] [INFO] [1753692657.171621459] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Could not find a connection between 'map' and 'robot_footprint' because they are not part of the same tree.Tf has two or more unconnected trees. [global_planner_node-4] [INFO] [1753692657.612787593] [perception_3d_global]: Use plugin name: map ---> perception_3d::StaticLayer [p2p_move_base_node-5] [INFO] [1753692657.672158447] [perception_3d_local]: Use plugin name: map ---> perception_3d::StaticLayer [global_planner_node-4] [INFO] [1753692657.843805764] [perception_3d_global.map]: radius_of_ground_connection: 1.50 [global_planner_node-4] [INFO] [1753692657.844310625] [perception_3d_global.map]: use_adaptive_connection: 0 [global_planner_node-4] [INFO] [1753692657.844743039] [perception_3d_global.map]: adaptive_connection_number: 20 [global_planner_node-4] [INFO] [1753692657.845145157] [perception_3d_global.map]: turning_weight: 0.10 [global_planner_node-4] [INFO] [1753692657.845608291] [perception_3d_global.map]: intensity_search_radius: 1.00 [global_planner_node-4] [INFO] [1753692657.845944228] [perception_3d_global.map]: intensity_search_punish_weight: 0.10 [global_planner_node-4] [INFO] [1753692657.846221204] [perception_3d_global.map]: static_imposing_radius: 1.50 [global_planner_node-4] [INFO] [1753692657.846588004] [perception_3d_global.map]: is_local_planner: 0 [global_planner_node-4] [INFO] [1753692657.847201417] [perception_3d_global.map]: map_topic: mapcloud [global_planner_node-4] [INFO] [1753692657.847488685] [perception_3d_global.map]: ground_topic: mapground [global_planner_node-4] [INFO] [1753692657.847826778] [perception_3d_global.map]: mapping_mode: 0 [global_planner_node-4] [INFO] [1753692657.862187379] [perception_3d_global]: Use plugin name: robosence ---> perception_3d::MultiLayerSpinningLidar [global_planner_node-4] [INFO] [1753692657.872626735] [perception_3d_global.robosence]: topic: /registered_cloud [global_planner_node-4] [INFO] [1753692657.873911381] [perception_3d_global.robosence]: vertical_FOV_top: 15.0 [global_planner_node-4] [INFO] [1753692657.874499446] [perception_3d_global.robosence]: vertical_FOV_bottom: -55.0 [global_planner_node-4] [INFO] [1753692657.875468442] [perception_3d_global.robosence]: scan_effective_positive_start: 0.0 [global_planner_node-4] [INFO] [1753692657.876052247] [perception_3d_global.robosence]: scan_effective_positive_end: 135.0 [global_planner_node-4] [INFO] [1753692657.877216674] [perception_3d_global.robosence]: scan_effective_negative_start: 0.0 [global_planner_node-4] [INFO] [1753692657.877954544] [perception_3d_global.robosence]: scan_effective_negative_end: -135.0 [global_planner_node-4] [INFO] [1753692657.879235677] [perception_3d_global.robosence]: euclidean_cluster_extraction_tolerance: 0.1 [global_planner_node-4] [INFO] [1753692657.880014556] [perception_3d_global.robosence]: euclidean_cluster_extraction_min_cluster_size: 3 [global_planner_node-4] [INFO] [1753692657.880567902] [perception_3d_global.robosence]: xy_resolution: 0.10 [global_planner_node-4] [INFO] [1753692657.881732894] [perception_3d_global.robosence]: height_resolution: 0.10 [global_planner_node-4] [INFO] [1753692657.882106527] [perception_3d_global.robosence]: marking_height: 2.00 [global_planner_node-4] [INFO] [1753692657.882413739] [perception_3d_global.robosence]: perception_window_size: 5.00 [global_planner_node-4] [INFO] [1753692657.882745311] [perception_3d_global.robosence]: segmentation_ignore_ratio: 0.90 [global_planner_node-4] [INFO] [1753692657.883439145] [perception_3d_global.robosence]: is_local_planner: 0 [global_planner_node-4] [INFO] [1753692657.883748027] [perception_3d_global.robosence]: expected_sensor_time: 0.10 [global_planner_node-4] [INFO] [1753692657.884352127] [perception_3d_global.robosence]: pub_gbl_marking_for_visualization: 1 [global_planner_node-4] [INFO] [1753692657.885166544] [perception_3d_global.robosence]: stitcher_num: 10 [global_planner_node-4] [WARN] [1753692657.900351009] [perception_3d_global]: Loop time: 100 ms [global_planner_node-4] [WARN] [1753692657.901834721] [perception_3d_global]: dGraph publish time: 1000 ms [global_planner_node-4] [INFO] [1753692657.907127288] [global_planner]: turning_weight: 0.10 [global_planner_node-4] [INFO] [1753692657.934945429] [dynamic_window_aware_global_planner]: look_ahead_distance: 5.00 [global_planner_node-4] [WARN] [1753692657.943369667] [perception_3d_global.map]: map receive new Map with size: 12467 [global_planner_node-4] [WARN] [1753692657.952120961] [perception_3d_global.map]: map receive new Ground with size: 1743 [p2p_move_base_node-5] [INFO] [1753692657.961555258] [perception_3d_local.map]: radius_of_ground_connection: 1.00 [p2p_move_base_node-5] [INFO] [1753692657.962428859] [perception_3d_local.map]: use_adaptive_connection: 1 [p2p_move_base_node-5] [INFO] [1753692657.962627348] [perception_3d_local.map]: adaptive_connection_number: 20 [p2p_move_base_node-5] [INFO] [1753692657.962777012] [perception_3d_local.map]: turning_weight: 0.10 [p2p_move_base_node-5] [INFO] [1753692657.962939978] [perception_3d_local.map]: intensity_search_radius: 1.00 [p2p_move_base_node-5] [INFO] [1753692657.963084925] [perception_3d_local.map]: intensity_search_punish_weight: 0.10 [p2p_move_base_node-5] [INFO] [1753692657.963245774] [perception_3d_local.map]: static_imposing_radius: 0.25 [p2p_move_base_node-5] [INFO] [1753692657.963409907] [perception_3d_local.map]: is_local_planner: 1 [p2p_move_base_node-5] [INFO] [1753692657.963574643] [perception_3d_local.map]: map_topic: mapcloud [global_planner_node-4] [INFO] [1753692657.963609325] [perception_3d_global.robosence]: Receiving Lidar topic: /registered_cloud [p2p_move_base_node-5] [INFO] [1753692657.963743735] [perception_3d_local.map]: ground_topic: mapground [p2p_move_base_node-5] [INFO] [1753692657.963915356] [perception_3d_local.map]: mapping_mode: 0 [p2p_move_base_node-5] [INFO] [1753692657.971753189] [perception_3d_local]: Use plugin name: path_blocked_strategy ---> perception_3d::PathBlockedStrategy [p2p_move_base_node-5] [INFO] [1753692657.973251223] [perception_3d_local.path_blocked_strategy]: check_radius: 0.30 [p2p_move_base_node-5] [INFO] [1753692657.973359240] [perception_3d_local]: Use plugin name: robosence ---> perception_3d::MultiLayerSpinningLidar [p2p_move_base_node-5] [INFO] [1753692657.973937728] [perception_3d_local.robosence]: topic: /registered_cloud [p2p_move_base_node-5] [INFO] [1753692657.974050902] [perception_3d_local.robosence]: vertical_FOV_top: 15.0 [p2p_move_base_node-5] [INFO] [1753692657.974163153] [perception_3d_local.robosence]: vertical_FOV_bottom: -55.0 [p2p_move_base_node-5] [INFO] [1753692657.974255601] [perception_3d_local.robosence]: scan_effective_positive_start: 0.0 [p2p_move_base_node-5] [INFO] [1753692657.974353438] [perception_3d_local.robosence]: scan_effective_positive_end: 135.0 [p2p_move_base_node-5] [INFO] [1753692657.974482354] [perception_3d_local.robosence]: scan_effective_negative_start: 0.0 [p2p_move_base_node-5] [INFO] [1753692657.974594880] [perception_3d_local.robosence]: scan_effective_negative_end: -135.0 [p2p_move_base_node-5] [INFO] [1753692657.974688845] [perception_3d_local.robosence]: euclidean_cluster_extraction_tolerance: 0.1 [p2p_move_base_node-5] [INFO] [1753692657.974945013] [perception_3d_local.robosence]: euclidean_cluster_extraction_min_cluster_size: 3 [p2p_move_base_node-5] [INFO] [1753692657.975158316] [perception_3d_local.robosence]: xy_resolution: 0.10 [p2p_move_base_node-5] [INFO] [1753692657.975246433] [perception_3d_local.robosence]: height_resolution: 0.10 [p2p_move_base_node-5] [INFO] [1753692657.975331224] [perception_3d_local.robosence]: marking_height: 2.00 [p2p_move_base_node-5] [INFO] [1753692657.975402121] [perception_3d_local.robosence]: perception_window_size: 5.00 [p2p_move_base_node-5] [INFO] [1753692657.975490380] [perception_3d_local.robosence]: segmentation_ignore_ratio: 1.10 [p2p_move_base_node-5] [INFO] [1753692657.975587028] [perception_3d_local.robosence]: is_local_planner: 1 [p2p_move_base_node-5] [INFO] [1753692657.975660587] [perception_3d_local.robosence]: expected_sensor_time: 0.10 [p2p_move_base_node-5] [INFO] [1753692657.975736935] [perception_3d_local.robosence]: pub_gbl_marking_for_visualization: 0 [p2p_move_base_node-5] [INFO] [1753692657.975840026] [perception_3d_local.robosence]: stitcher_num: 10 [p2p_move_base_node-5] [WARN] [1753692657.993422579] [perception_3d_local]: Loop time: 100 ms [p2p_move_base_node-5] [INFO] [1753692657.996775677] [recovery_behaviors]: odom_topic: odom [global_planner_node-4] [WARN] [1753692658.001966697] [perception_3d_global.map]: map has already received two msg. [global_planner_node-4] [INFO] [1753692658.002047925] [perception_3d_global.map]: map starts to reset dynamic graph. [p2p_move_base_node-5] [INFO] [1753692658.004059721] [recovery_behaviors]: Use behavior name: rotate_inplace ---> recovery_behaviors::RotateInPlaceBehavior [p2p_move_base_node-5] [INFO] [1753692658.008669387] [recovery_behaviors.rotate_inplace]: frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.008953916] [recovery_behaviors.rotate_inplace]: tolerance: 0.30 [p2p_move_base_node-5] [INFO] [1753692658.009064375] [recovery_behaviors.rotate_inplace]: trajectory_generator_name: differential_drive_rotate_inplace [p2p_move_base_node-5] [INFO] [1753692658.014483539] [local_planner]: odom_topic: /range_sensor_pose [p2p_move_base_node-5] [INFO] [1753692658.014675020] [local_planner]: odom_topic_qos: reliable [p2p_move_base_node-5] [INFO] [1753692658.014808629] [local_planner]: compute_best_trajectory_in_odomCb: 0 [p2p_move_base_node-5] [INFO] [1753692658.014945125] [local_planner]: forward_prune: 3.00 [p2p_move_base_node-5] [INFO] [1753692658.015076065] [local_planner]: backward_prune: 1.00 [p2p_move_base_node-5] [INFO] [1753692658.015665985] [local_planner]: heading_tracking_distance: 0.50 [p2p_move_base_node-5] [INFO] [1753692658.015804949] [local_planner]: heading_align_angle: 0.50 [p2p_move_base_node-5] [INFO] [1753692658.015933216] [local_planner]: prune_plane_timeout: 3.00 [p2p_move_base_node-5] [INFO] [1753692658.016067552] [local_planner]: xy_goal_tolerance: 0.30 [p2p_move_base_node-5] [INFO] [1753692658.016211926] [local_planner]: yaw_goal_tolerance: 0.30 [p2p_move_base_node-5] [INFO] [1753692658.016350167] [local_planner]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.020113152] [local_planner.local_planner]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753692658.021301859] [local_planner.local_planner]: Cuboid vertex are loaded, start to connect edges. [p2p_move_base_node-5] [INFO] [1753692658.025475082] [global_plan_manager]: P2P global plan manager uses get_dwa_plan service to query global plan. [p2p_move_base_node-5] [INFO] [1753692658.025599478] [global_plan_manager]: global_plan_query_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.031739254] [global_plan_manager]: Global plan manager is stopped [p2p_move_base_node-5] [INFO] [1753692658.032031391] [p2p_move_base]: planner_patience: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.032146542] [p2p_move_base]: oscillation_distance: 2.00 [p2p_move_base_node-5] [INFO] [1753692658.032217889] [p2p_move_base]: oscillation_angle: 1.00 [p2p_move_base_node-5] [INFO] [1753692658.032280589] [p2p_move_base]: oscillation_patience: 20.00 [p2p_move_base_node-5] [INFO] [1753692658.032348315] [p2p_move_base]: controller_patience: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.032421216] [p2p_move_base]: no_plan_retry_num: 10 [p2p_move_base_node-5] [INFO] [1753692658.032475936] [p2p_move_base]: waiting_patience: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.032529448] [p2p_move_base]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753692658.032576484] [p2p_move_base]: use_stamped_twist: 0 [p2p_move_base_node-5] [INFO] [1753692658.032636495] [p2p_move_base]: twist_frame_id: base_link [p2p_move_base_node-5] [INFO] [1753692658.041679782] [p2p_move_base]: ----> P2P move base launched. [p2p_move_base_node-5] [WARN] [1753692658.043851252] [perception_3d_local.map]: map receive new Map with size: 12467 [p2p_move_base_node-5] [WARN] [1753692658.046091134] [perception_3d_local.map]: map receive new Ground with size: 1743 [p2p_move_base_node-5] [WARN] [1753692658.094326857] [perception_3d_local.map]: map has already received two msg. [p2p_move_base_node-5] [INFO] [1753692658.094408658] [perception_3d_local.map]: map starts to reset dynamic graph. [p2p_move_base_node-5] [INFO] [1753692658.187477454] [perception_3d_local.robosence]: Receiving Lidar topic: /registered_cloud [global_planner_node-4] [INFO] [1753692658.418159059] [global_planner]: Waiting for static layer [global_planner_node-4] [INFO] [1753692658.638814210] [perception_3d_global.robosence]: robosence starts to reset dynamic graph. [global_planner_node-4] [INFO] [1753692658.639375537] [perception_3d_global.robosence]: robosence done dynamic graph regeneration. [global_planner_node-4] [WARN] [1753692658.669675414] [perception_3d_global]: Map update loop missed its desired rate of 10.0000Hz... the loop actually took 0.6677 seconds [global_planner_node-4] [INFO] [1753692658.918917081] [global_planner]: Ground and Kd-tree ground have been received from perception_3d. [global_planner_node-4] [INFO] [1753692658.920606429] [global_planner]: Publish weighted pc. [rviz2-7] [INFO] [1753692664.188200095] [rviz2]: Setting estimate pose: Frame:map, Position(6.12848, -2.56457, -0.854017), Orientation(0, 0, -0.825961, 0.563727) = Angle: -1.94381 [ros2-1] [icp_localization-1] User set pose to :t:[6.128476, -2.564567, -0.854017] ; q:[0.000000, 0.000000, -0.825961, 0.563727] ; rpy (deg):[0.000000, 0.000000, -111.372143] [global_planner_node-4] [INFO] [1753692717.965660740] [perception_3d_global.robosence]: Receiving Lidar topic: /registered_cloud [p2p_move_base_node-5] [INFO] [1753692718.224676230] [perception_3d_local.robosence]: Receiving Lidar topic: /registered_cloud

Jul 28 '25 08:07 adsaosdi

Can you also provide the logs after you give a goal?

Jul 28 '25 08:07 tsengapola

unitree@unitree-b2-pc4:~/dddmr_navigation$ ros2 launch p2p_move_base b2_nav3d.launch.py [INFO] [launch]: All log files can be found below /home/unitree/.ros/log/2025-07-28-16-56-42-256522-unitree-b2-pc4-21408 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [ros2-1]: process started with pid [21409] [INFO] [dog_odom_to_tf_node-2]: process started with pid [21411] [INFO] [pcl_publisher-3]: process started with pid [21413] [INFO] [global_planner_node-4]: process started with pid [21415] [INFO] [p2p_move_base_node-5]: process started with pid [21417] [INFO] [clicked2goal.py-6]: process started with pid [21419] [INFO] [rviz2-7]: process started with pid [21421] [global_planner_node-4] [INFO] [1753693002.338932470] [perception_3d_global]: global_frame: map [global_planner_node-4] [INFO] [1753693002.340059958] [perception_3d_global]: robot_base_frame: robot_footprint [global_planner_node-4] [INFO] [1753693002.340515341] [perception_3d_global]: max_obstacle_distance: 50.0 [global_planner_node-4] [INFO] [1753693002.340852440] [perception_3d_global]: inscribed_radius: 0.50 [global_planner_node-4] [INFO] [1753693002.341331473] [perception_3d_global]: inflation_descending_rate: 2.00 [global_planner_node-4] [INFO] [1753693002.341723644] [perception_3d_global]: inflation_radius: 0.50 [global_planner_node-4] [INFO] [1753693002.342091519] [perception_3d_global]: sensors_collected_frequency: 10.00 [global_planner_node-4] [INFO] [1753693002.342447650] [perception_3d_global]: dgraph_publish_frequency: 1.00 [global_planner_node-4] [INFO] [1753693002.346505838] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [p2p_move_base_node-5] [INFO] [1753693002.381623183] [trajectory_generators]: Use theory name: differential_drive_simple ---> trajectory_generators::DDSimpleTrajectoryGeneratorTheory [p2p_move_base_node-5] [INFO] [1753693002.383244566] [trajectory_generators.differential_drive_simple]: min_vel_x: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.383322648] [trajectory_generators.differential_drive_simple]: max_vel_x: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.383380798] [trajectory_generators.differential_drive_simple]: min_vel_theta: 0.15 [p2p_move_base_node-5] [INFO] [1753693002.383436057] [trajectory_generators.differential_drive_simple]: max_vel_theta: 0.60 [p2p_move_base_node-5] [INFO] [1753693002.383489792] [trajectory_generators.differential_drive_simple]: acc_lim_x: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.383543870] [trajectory_generators.differential_drive_simple]: acc_lim_theta: 3.00 [p2p_move_base_node-5] [INFO] [1753693002.383597633] [trajectory_generators.differential_drive_simple]: prune_forward: 3.00 [p2p_move_base_node-5] [INFO] [1753693002.383651663] [trajectory_generators.differential_drive_simple]: prune_backward: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.383707093] [trajectory_generators.differential_drive_simple]: deceleration_ratio: 2.00 [p2p_move_base_node-5] [INFO] [1753693002.383761052] [trajectory_generators.differential_drive_simple]: max_motor_shaft_rpm: 3000.00 [p2p_move_base_node-5] [INFO] [1753693002.383815897] [trajectory_generators.differential_drive_simple]: wheel_diameter: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.383869380] [trajectory_generators.differential_drive_simple]: gear_ratio: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.383926830] [trajectory_generators.differential_drive_simple]: robot_radius: 0.60 [p2p_move_base_node-5] [INFO] [1753693002.384106264] [trajectory_generators.differential_drive_simple]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.384162836] [trajectory_generators.differential_drive_simple]: sim_time: 2.00 [p2p_move_base_node-5] [INFO] [1753693002.384220432] [trajectory_generators.differential_drive_simple]: linear_x_sample: 5.00 [p2p_move_base_node-5] [INFO] [1753693002.384273855] [trajectory_generators.differential_drive_simple]: angular_z_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.384329788] [trajectory_generators.differential_drive_simple]: sim_granularity: 0.05 [p2p_move_base_node-5] [INFO] [1753693002.384390829] [trajectory_generators.differential_drive_simple]: angular_sim_granularity: 0.03 [p2p_move_base_node-5] [INFO] [1753693002.384399866] [trajectory_generators.differential_drive_simple]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753693002.384513376] [trajectory_generators.differential_drive_simple]: Cuboid flb: 0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.385328330] [trajectory_generators.differential_drive_simple]: Cuboid frb: 0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.385477572] [trajectory_generators.differential_drive_simple]: Cuboid flt: 0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.385568350] [trajectory_generators.differential_drive_simple]: Cuboid frt: 0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.385633188] [trajectory_generators.differential_drive_simple]: Cuboid blb: -0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.385703927] [trajectory_generators.differential_drive_simple]: Cuboid brb: -0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.385768471] [trajectory_generators.differential_drive_simple]: Cuboid blt: -0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.385835153] [trajectory_generators.differential_drive_simple]: Cuboid brt: -0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.386066452] [trajectory_generators]: Use theory name: differential_drive_rotate_inplace ---> trajectory_generators::DDRotateInplaceTheory [p2p_move_base_node-5] [INFO] [1753693002.386272764] [trajectory_generators.differential_drive_rotate_inplace]: min_vel_x: 0.01 [p2p_move_base_node-5] [INFO] [1753693002.386316551] [trajectory_generators.differential_drive_rotate_inplace]: max_vel_x: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.386358765] [trajectory_generators.differential_drive_rotate_inplace]: min_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.386400036] [trajectory_generators.differential_drive_rotate_inplace]: max_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.386431234] [trajectory_generators.differential_drive_rotate_inplace]: acc_lim_x: 0.30 [p2p_move_base_node-5] [INFO] [1753693002.386463410] [trajectory_generators.differential_drive_rotate_inplace]: acc_lim_theta: 0.50 [p2p_move_base_node-5] [INFO] [1753693002.386495424] [trajectory_generators.differential_drive_rotate_inplace]: prune_forward: 3.00 [p2p_move_base_node-5] [INFO] [1753693002.386530410] [trajectory_generators.differential_drive_rotate_inplace]: prune_backward: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.386589743] [trajectory_generators.differential_drive_rotate_inplace]: max_motor_shaft_rpm: 3000.00 [p2p_move_base_node-5] [INFO] [1753693002.386622223] [trajectory_generators.differential_drive_rotate_inplace]: wheel_diameter: 0.15 [p2p_move_base_node-5] [INFO] [1753693002.386652187] [trajectory_generators.differential_drive_rotate_inplace]: gear_ratio: 30.00 [p2p_move_base_node-5] [INFO] [1753693002.386682546] [trajectory_generators.differential_drive_rotate_inplace]: robot_radius: 0.25 [p2p_move_base_node-5] [INFO] [1753693002.386733184] [trajectory_generators.differential_drive_rotate_inplace]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.386764079] [trajectory_generators.differential_drive_rotate_inplace]: sim_time: 2.00 [p2p_move_base_node-5] [INFO] [1753693002.386801964] [trajectory_generators.differential_drive_rotate_inplace]: linear_x_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.386831358] [trajectory_generators.differential_drive_rotate_inplace]: angular_z_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.386862158] [trajectory_generators.differential_drive_rotate_inplace]: sim_granularity: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.386902655] [trajectory_generators.differential_drive_rotate_inplace]: angular_sim_granularity: 0.05 [p2p_move_base_node-5] [INFO] [1753693002.386935484] [trajectory_generators.differential_drive_rotate_inplace]: rotation_speed: 0.50 [p2p_move_base_node-5] [INFO] [1753693002.386939067] [trajectory_generators.differential_drive_rotate_inplace]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753693002.386977718] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid flb: 0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.387013611] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid frb: 0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.387047602] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid flt: 0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.387078972] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid frt: 0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.387110528] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid blb: -0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.387140800] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid brb: -0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.387171928] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid blt: -0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.387202091] [trajectory_generators.differential_drive_rotate_inplace]: Cuboid brt: -0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.387252972] [trajectory_generators]: Use theory name: differential_drive_rotate_shortest_angle ---> trajectory_generators::DDRotateInplaceTheory [p2p_move_base_node-5] [INFO] [1753693002.387392157] [trajectory_generators.differential_drive_rotate_shortest_angle]: min_vel_x: 0.01 [p2p_move_base_node-5] [INFO] [1753693002.387433477] [trajectory_generators.differential_drive_rotate_shortest_angle]: max_vel_x: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.387477987] [trajectory_generators.differential_drive_rotate_shortest_angle]: min_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.387513290] [trajectory_generators.differential_drive_rotate_shortest_angle]: max_vel_theta: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.387547512] [trajectory_generators.differential_drive_rotate_shortest_angle]: acc_lim_x: 0.30 [p2p_move_base_node-5] [INFO] [1753693002.387577518] [trajectory_generators.differential_drive_rotate_shortest_angle]: acc_lim_theta: 0.50 [p2p_move_base_node-5] [INFO] [1753693002.387612925] [trajectory_generators.differential_drive_rotate_shortest_angle]: prune_forward: 3.00 [p2p_move_base_node-5] [INFO] [1753693002.387650201] [trajectory_generators.differential_drive_rotate_shortest_angle]: prune_backward: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.387683044] [trajectory_generators.differential_drive_rotate_shortest_angle]: max_motor_shaft_rpm: 3000.00 [p2p_move_base_node-5] [INFO] [1753693002.387718792] [trajectory_generators.differential_drive_rotate_shortest_angle]: wheel_diameter: 0.15 [p2p_move_base_node-5] [INFO] [1753693002.387750541] [trajectory_generators.differential_drive_rotate_shortest_angle]: gear_ratio: 30.00 [p2p_move_base_node-5] [INFO] [1753693002.387784618] [trajectory_generators.differential_drive_rotate_shortest_angle]: robot_radius: 0.25 [p2p_move_base_node-5] [INFO] [1753693002.387829376] [trajectory_generators.differential_drive_rotate_shortest_angle]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.387865073] [trajectory_generators.differential_drive_rotate_shortest_angle]: sim_time: 2.00 [p2p_move_base_node-5] [INFO] [1753693002.387906809] [trajectory_generators.differential_drive_rotate_shortest_angle]: linear_x_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.387967470] [trajectory_generators.differential_drive_rotate_shortest_angle]: angular_z_sample: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.388003326] [trajectory_generators.differential_drive_rotate_shortest_angle]: sim_granularity: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.388036892] [trajectory_generators.differential_drive_rotate_shortest_angle]: angular_sim_granularity: 0.05 [p2p_move_base_node-5] [INFO] [1753693002.388070304] [trajectory_generators.differential_drive_rotate_shortest_angle]: rotation_speed: 0.50 [p2p_move_base_node-5] [INFO] [1753693002.388074510] [trajectory_generators.differential_drive_rotate_shortest_angle]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753693002.388112030] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid flb: 0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.388150635] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid frb: 0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.388183331] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid flt: 0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.388219584] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid frt: 0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.388252421] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid blb: -0.60, 0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.388286953] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid brb: -0.60, -0.25, 0.00 [p2p_move_base_node-5] [INFO] [1753693002.388323055] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid blt: -0.60, 0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.388362466] [trajectory_generators.differential_drive_rotate_shortest_angle]: Cuboid brt: -0.60, -0.25, 0.70 [p2p_move_base_node-5] [INFO] [1753693002.390652661] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::CollisionModel [p2p_move_base_node-5] [INFO] [1753693002.391691725] [mpc_critics.collision]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.391789618] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::StickPathModel [p2p_move_base_node-5] [INFO] [1753693002.391908250] [mpc_critics.stick_path]: weight: 0.10 [p2p_move_base_node-5] [INFO] [1753693002.391996537] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::PurePursuitModel [p2p_move_base_node-5] [INFO] [1753693002.392088716] [mpc_critics.pure_pursuit]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.392124029] [mpc_critics.pure_pursuit]: translation_weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.392157221] [mpc_critics.pure_pursuit]: orientation_weight: 0.01 [p2p_move_base_node-5] [INFO] [1753693002.392219557] [mpc_critics]: Trajectory generator: differential_drive_simple using ---> mpc_critics::TowardGlobalPlanModel [p2p_move_base_node-5] [INFO] [1753693002.392282060] [mpc_critics.toward_global_plan]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.392345972] [mpc_critics]: Trajectory generator: differential_drive_rotate_inplace using ---> mpc_critics::CollisionModel [p2p_move_base_node-5] [INFO] [1753693002.392407471] [mpc_critics.collision_rotate]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.392471273] [mpc_critics]: Trajectory generator: differential_drive_rotate_shortest_angle using ---> mpc_critics::CollisionModel [p2p_move_base_node-5] [INFO] [1753693002.392532885] [mpc_critics.collision_rotate_shortest]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.392622069] [mpc_critics]: Trajectory generator: differential_drive_rotate_shortest_angle using ---> mpc_critics::ShortestAngleModel [p2p_move_base_node-5] [INFO] [1753693002.392686576] [mpc_critics.prefer_rotate_shortest]: weight: 1.00 [p2p_move_base_node-5] [INFO] [1753693002.392761295] [perception_3d_local]: global_frame: map [p2p_move_base_node-5] [INFO] [1753693002.392815269] [perception_3d_local]: robot_base_frame: robot_footprint [p2p_move_base_node-5] [INFO] [1753693002.392857481] [perception_3d_local]: max_obstacle_distance: 50.0 [p2p_move_base_node-5] [INFO] [1753693002.392920657] [perception_3d_local]: inscribed_radius: 0.50 [p2p_move_base_node-5] [INFO] [1753693002.392960451] [perception_3d_local]: inflation_descending_rate: 2.00 [p2p_move_base_node-5] [INFO] [1753693002.392994821] [perception_3d_local]: inflation_radius: 0.50 [p2p_move_base_node-5] [INFO] [1753693002.393033131] [perception_3d_local]: sensors_collected_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693002.393075004] [perception_3d_local]: dgraph_publish_frequency: 0.00 [p2p_move_base_node-5] [INFO] [1753693002.396451378] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [pcl_publisher-3] [INFO] [1753693002.422046155] [pcl_publisher]: map_dir: /home/unitree/dddmr_navigation/install/global_planner/share/global_planner/data/map.pcd [pcl_publisher-3] [INFO] [1753693002.422182265] [pcl_publisher]: ground_dir: /home/unitree/dddmr_navigation/install/global_planner/share/global_planner/data/ground.pcd [pcl_publisher-3] [INFO] [1753693002.422217456] [pcl_publisher]: global_frame: map [pcl_publisher-3] [INFO] [1753693002.422249363] [pcl_publisher]: map_rotate_around_x: 0.00 [pcl_publisher-3] [INFO] [1753693002.422282220] [pcl_publisher]: map_rotate_around_y: 0.00 [pcl_publisher-3] [INFO] [1753693002.422313359] [pcl_publisher]: map_rotate_around_z: 0.00 [pcl_publisher-3] [INFO] [1753693002.422343621] [pcl_publisher]: map_translate_x: 0.00 [pcl_publisher-3] [INFO] [1753693002.422373944] [pcl_publisher]: map_translate_y: 0.00 [pcl_publisher-3] [INFO] [1753693002.422403787] [pcl_publisher]: map_translate_z: 0.00 [pcl_publisher-3] [INFO] [1753693002.422434184] [pcl_publisher]: ground_rotate_around_x: 0.00 [pcl_publisher-3] [INFO] [1753693002.422465103] [pcl_publisher]: ground_rotate_around_y: 0.00 [pcl_publisher-3] [INFO] [1753693002.422495559] [pcl_publisher]: ground_rotate_around_z: 0.00 [pcl_publisher-3] [INFO] [1753693002.422526001] [pcl_publisher]: ground_translate_x: 0.00 [pcl_publisher-3] [INFO] [1753693002.422556378] [pcl_publisher]: ground_translate_y: 0.00 [pcl_publisher-3] [INFO] [1753693002.422586807] [pcl_publisher]: ground_translate_z: 0.00 [pcl_publisher-3] [INFO] [1753693002.422617102] [pcl_publisher]: map_down_sample: 0.30 [pcl_publisher-3] [INFO] [1753693002.422648545] [pcl_publisher]: ground_down_sample: 0.40 [ros2-1] [INFO] [launch]: All log files can be found below /home/unitree/.ros/log/2025-07-28-16-56-42-597675-unitree-b2-pc4-21409 [ros2-1] [INFO] [launch]: Default logging verbosity is set to INFO [rviz2-7] [INFO] [1753693002.667794839] [rviz2]: Stereo is NOT SUPPORTED [rviz2-7] [INFO] [1753693002.667872781] [rviz2]: OpenGl version: 4.5 (GLSL 4.5) [rviz2-7] [INFO] [1753693002.684795205] [rviz2]: Stereo is NOT SUPPORTED [pcl_publisher-3] [INFO] [1753693002.756097324] [pcl_publisher]: Map pointcloud size: 3467288 [global_planner_node-4] [INFO] [1753693002.846595377] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [pcl_publisher-3] [INFO] [1753693002.882128321] [pcl_publisher]: Map pointcloud size after down size: 12467 [p2p_move_base_node-5] [INFO] [1753693002.896530114] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [pcl_publisher-3] [INFO] [1753693002.948876987] [pcl_publisher]: Ground pointcloud size: 815624 [pcl_publisher-3] [INFO] [1753693002.972615730] [pcl_publisher]: Ground pointcloud size after down size: 1743 [global_planner_node-4] [INFO] [1753693003.346567012] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [p2p_move_base_node-5] [INFO] [1753693003.396520857] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [global_planner_node-4] [INFO] [1753693003.846632293] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [p2p_move_base_node-5] [INFO] [1753693003.896541590] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist [ros2-1] [INFO] [icp_localization-1]: process started with pid [21626] [ros2-1] [icp_localization-1] [INFO] [1753693004.323421250] [pcl_loader_node]: Received and converted map cloud from /mapcloud. Points: 12467, Width: 12467, Frame: map [ros2-1] [icp_localization-1] [INFO] [1753693004.323514131] [pcl_loader_node]: Map received immediately from latched queue [ros2-1] [icp_localization-1] Map size is: 12467(befor filter), 12467(after filter) [ros2-1] [icp_localization-1] Init pose set to, xyz: 0 0 0, q: 0 0 0 1, rpy: 0 0 0 deg [ros2-1] [icp_localization-1] odometry data topic: /dog_odom [ros2-1] [icp_localization-1] imu data topic: /dog_imu_raw [ros2-1] [icp_localization-1] Is use odometry: true [ros2-1] [icp_localization-1] Gravity vector filter time constant: 0.01 [ros2-1] [icp_localization-1] Min num odom measurements before ready: 5 [ros2-1] [icp_localization-1] Calibration: [ros2-1] [icp_localization-1] imu to range sensor: t:[0.342180, 0.023410, 0.129240] ; q:[0.000000, 0.000000, 0.000000, 1.000000] ; rpy (deg):[0.000000, 0.000000, 0.000000] [ros2-1] [icp_localization-1] [ros2-1] [icp_localization-1] odometry source to range sensor: t:[0.000000, 0.000000, 0.000000] ; q:[0.000000, 0.000000, 0.000000, 1.000000] ; rpy (deg):[0.000000, 0.000000, 0.000000] [ros2-1] [icp_localization-1] [ros2-1] [icp_localization-1] range data parameters: [ros2-1] [icp_localization-1] topic: /rslidar_points [ros2-1] [icp_localization-1] num range data accumulated: 1 [ros2-1] [icp_localization-1]
[ros2-1] [icp_localization-1] Setting fixed frame to: map [ros2-1] [icp_localization-1] ICPlocalization: Initialized [ros2-1] [icp_localization-1] succesfully initialized icp [global_planner_node-4] [INFO] [1753693004.346562394] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Could not find a connection between 'map' and 'robot_footprint' because they are not part of the same tree.Tf has two or more unconnected trees. [p2p_move_base_node-5] [INFO] [1753693004.396513629] [perception_3d_local]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Could not find a connection between 'map' and 'robot_footprint' because they are not part of the same tree.Tf has two or more unconnected trees. [global_planner_node-4] [INFO] [1753693004.846570334] [perception_3d_global]: Timed out waiting for transform from robot_footprint to map to become available, tf error: Could not find a connection between 'map' and 'robot_footprint' because they are not part of the same tree.Tf has two or more unconnected trees. [p2p_move_base_node-5] [INFO] [1753693004.896847076] [perception_3d_local]: Use plugin name: map ---> perception_3d::StaticLayer [p2p_move_base_node-5] [INFO] [1753693005.051393344] [perception_3d_local.map]: radius_of_ground_connection: 1.00 [p2p_move_base_node-5] [INFO] [1753693005.051473638] [perception_3d_local.map]: use_adaptive_connection: 1 [p2p_move_base_node-5] [INFO] [1753693005.051528436] [perception_3d_local.map]: adaptive_connection_number: 20 [p2p_move_base_node-5] [INFO] [1753693005.051563060] [perception_3d_local.map]: turning_weight: 0.10 [p2p_move_base_node-5] [INFO] [1753693005.051690936] [perception_3d_local.map]: intensity_search_radius: 1.00 [p2p_move_base_node-5] [INFO] [1753693005.051776567] [perception_3d_local.map]: intensity_search_punish_weight: 0.10 [p2p_move_base_node-5] [INFO] [1753693005.051851136] [perception_3d_local.map]: static_imposing_radius: 0.25 [p2p_move_base_node-5] [INFO] [1753693005.051925598] [perception_3d_local.map]: is_local_planner: 1 [p2p_move_base_node-5] [INFO] [1753693005.052002303] [perception_3d_local.map]: map_topic: mapcloud [p2p_move_base_node-5] [INFO] [1753693005.052072900] [perception_3d_local.map]: ground_topic: mapground [p2p_move_base_node-5] [INFO] [1753693005.052137821] [perception_3d_local.map]: mapping_mode: 0 [p2p_move_base_node-5] [INFO] [1753693005.057302254] [perception_3d_local]: Use plugin name: path_blocked_strategy ---> perception_3d::PathBlockedStrategy [p2p_move_base_node-5] [INFO] [1753693005.057617251] [perception_3d_local.path_blocked_strategy]: check_radius: 0.30 [p2p_move_base_node-5] [INFO] [1753693005.057723297] [perception_3d_local]: Use plugin name: robosence ---> perception_3d::MultiLayerSpinningLidar [p2p_move_base_node-5] [INFO] [1753693005.058230970] [perception_3d_local.robosence]: topic: /registered_cloud [p2p_move_base_node-5] [INFO] [1753693005.058339282] [perception_3d_local.robosence]: vertical_FOV_top: 15.0 [p2p_move_base_node-5] [INFO] [1753693005.058431222] [perception_3d_local.robosence]: vertical_FOV_bottom: -55.0 [p2p_move_base_node-5] [INFO] [1753693005.058520353] [perception_3d_local.robosence]: scan_effective_positive_start: 0.0 [p2p_move_base_node-5] [INFO] [1753693005.058606319] [perception_3d_local.robosence]: scan_effective_positive_end: 135.0 [p2p_move_base_node-5] [INFO] [1753693005.058695302] [perception_3d_local.robosence]: scan_effective_negative_start: 0.0 [p2p_move_base_node-5] [INFO] [1753693005.058783852] [perception_3d_local.robosence]: scan_effective_negative_end: -135.0 [p2p_move_base_node-5] [INFO] [1753693005.058871275] [perception_3d_local.robosence]: euclidean_cluster_extraction_tolerance: 0.1 [p2p_move_base_node-5] [INFO] [1753693005.058980199] [perception_3d_local.robosence]: euclidean_cluster_extraction_min_cluster_size: 3 [p2p_move_base_node-5] [INFO] [1753693005.059097995] [perception_3d_local.robosence]: xy_resolution: 0.10 [p2p_move_base_node-5] [INFO] [1753693005.059190658] [perception_3d_local.robosence]: height_resolution: 0.10 [p2p_move_base_node-5] [INFO] [1753693005.059281152] [perception_3d_local.robosence]: marking_height: 2.00 [p2p_move_base_node-5] [INFO] [1753693005.059377988] [perception_3d_local.robosence]: perception_window_size: 5.00 [p2p_move_base_node-5] [INFO] [1753693005.059468977] [perception_3d_local.robosence]: segmentation_ignore_ratio: 1.10 [p2p_move_base_node-5] [INFO] [1753693005.059566247] [perception_3d_local.robosence]: is_local_planner: 1 [p2p_move_base_node-5] [INFO] [1753693005.059665979] [perception_3d_local.robosence]: expected_sensor_time: 0.10 [p2p_move_base_node-5] [INFO] [1753693005.059766347] [perception_3d_local.robosence]: pub_gbl_marking_for_visualization: 0 [p2p_move_base_node-5] [INFO] [1753693005.059919636] [perception_3d_local.robosence]: stitcher_num: 10 [p2p_move_base_node-5] [WARN] [1753693005.065569469] [perception_3d_local]: Loop time: 100 ms [p2p_move_base_node-5] [INFO] [1753693005.067012932] [recovery_behaviors]: odom_topic: odom [p2p_move_base_node-5] [INFO] [1753693005.075072542] [recovery_behaviors]: Use behavior name: rotate_inplace ---> recovery_behaviors::RotateInPlaceBehavior [p2p_move_base_node-5] [INFO] [1753693005.076961579] [recovery_behaviors.rotate_inplace]: frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.077571042] [recovery_behaviors.rotate_inplace]: tolerance: 0.30 [p2p_move_base_node-5] [INFO] [1753693005.077766142] [recovery_behaviors.rotate_inplace]: trajectory_generator_name: differential_drive_rotate_inplace [p2p_move_base_node-5] [INFO] [1753693005.087103592] [local_planner]: odom_topic: /dog_odom [p2p_move_base_node-5] [INFO] [1753693005.087330084] [local_planner]: odom_topic_qos: reliable [p2p_move_base_node-5] [INFO] [1753693005.087523445] [local_planner]: compute_best_trajectory_in_odomCb: 0 [p2p_move_base_node-5] [INFO] [1753693005.087718879] [local_planner]: forward_prune: 3.00 [p2p_move_base_node-5] [INFO] [1753693005.087915296] [local_planner]: backward_prune: 1.00 [p2p_move_base_node-5] [INFO] [1753693005.088110430] [local_planner]: heading_tracking_distance: 0.50 [p2p_move_base_node-5] [INFO] [1753693005.088299344] [local_planner]: heading_align_angle: 0.50 [p2p_move_base_node-5] [INFO] [1753693005.088479229] [local_planner]: prune_plane_timeout: 3.00 [p2p_move_base_node-5] [INFO] [1753693005.089031654] [local_planner]: xy_goal_tolerance: 0.30 [p2p_move_base_node-5] [INFO] [1753693005.089233739] [local_planner]: yaw_goal_tolerance: 0.30 [p2p_move_base_node-5] [INFO] [1753693005.089423468] [local_planner]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.093212988] [local_planner.local_planner]: Start to parse cuboid. [p2p_move_base_node-5] [INFO] [1753693005.095844178] [local_planner.local_planner]: Cuboid vertex are loaded, start to connect edges. [p2p_move_base_node-5] [INFO] [1753693005.105143130] [global_plan_manager]: P2P global plan manager uses get_dwa_plan service to query global plan. [p2p_move_base_node-5] [INFO] [1753693005.105370692] [global_plan_manager]: global_plan_query_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.114250864] [global_plan_manager]: Global plan manager is stopped [p2p_move_base_node-5] [INFO] [1753693005.115017301] [p2p_move_base]: planner_patience: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.115298987] [p2p_move_base]: oscillation_distance: 2.00 [p2p_move_base_node-5] [INFO] [1753693005.115363030] [p2p_move_base]: oscillation_angle: 1.00 [p2p_move_base_node-5] [INFO] [1753693005.115415522] [p2p_move_base]: oscillation_patience: 20.00 [p2p_move_base_node-5] [INFO] [1753693005.115466818] [p2p_move_base]: controller_patience: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.115530793] [p2p_move_base]: no_plan_retry_num: 10 [p2p_move_base_node-5] [INFO] [1753693005.115588015] [p2p_move_base]: waiting_patience: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.115645607] [p2p_move_base]: controller_frequency: 10.00 [p2p_move_base_node-5] [INFO] [1753693005.115699684] [p2p_move_base]: use_stamped_twist: 0 [p2p_move_base_node-5] [INFO] [1753693005.115749275] [p2p_move_base]: twist_frame_id: base_link [p2p_move_base_node-5] [INFO] [1753693005.130561492] [p2p_move_base]: ----> P2P move base launched. [p2p_move_base_node-5] [WARN] [1753693005.166200531] [perception_3d_local.map]: map receive new Map with size: 12467 [p2p_move_base_node-5] [WARN] [1753693005.171323736] [perception_3d_local.map]: map receive new Ground with size: 1743 [p2p_move_base_node-5] [WARN] [1753693005.265866009] [perception_3d_local.map]: map has already received two msg. [p2p_move_base_node-5] [INFO] [1753693005.265935430] [perception_3d_local.map]: map starts to reset dynamic graph. [global_planner_node-4] [INFO] [1753693005.347099144] [perception_3d_global]: Use plugin name: map ---> perception_3d::StaticLayer [p2p_move_base_node-5] [INFO] [1753693005.511827873] [perception_3d_local.robosence]: Receiving Lidar topic: /registered_cloud [global_planner_node-4] [INFO] [1753693005.599891584] [perception_3d_global.map]: radius_of_ground_connection: 1.50 [global_planner_node-4] [INFO] [1753693005.600011022] [perception_3d_global.map]: use_adaptive_connection: 0 [global_planner_node-4] [INFO] [1753693005.600062779] [perception_3d_global.map]: adaptive_connection_number: 20 [global_planner_node-4] [INFO] [1753693005.600103043] [perception_3d_global.map]: turning_weight: 0.10 [global_planner_node-4] [INFO] [1753693005.600157964] [perception_3d_global.map]: intensity_search_radius: 1.00 [global_planner_node-4] [INFO] [1753693005.600213317] [perception_3d_global.map]: intensity_search_punish_weight: 0.10 [global_planner_node-4] [INFO] [1753693005.600257053] [perception_3d_global.map]: static_imposing_radius: 1.50 [global_planner_node-4] [INFO] [1753693005.600299317] [perception_3d_global.map]: is_local_planner: 0 [global_planner_node-4] [INFO] [1753693005.600342401] [perception_3d_global.map]: map_topic: mapcloud [global_planner_node-4] [INFO] [1753693005.600394691] [perception_3d_global.map]: ground_topic: mapground [global_planner_node-4] [INFO] [1753693005.600452894] [perception_3d_global.map]: mapping_mode: 0 [global_planner_node-4] [INFO] [1753693005.603686473] [perception_3d_global]: Use plugin name: robosence ---> perception_3d::MultiLayerSpinningLidar [global_planner_node-4] [INFO] [1753693005.604031062] [perception_3d_global.robosence]: topic: /registered_cloud [global_planner_node-4] [INFO] [1753693005.604096985] [perception_3d_global.robosence]: vertical_FOV_top: 15.0 [global_planner_node-4] [INFO] [1753693005.604152500] [perception_3d_global.robosence]: vertical_FOV_bottom: -55.0 [global_planner_node-4] [INFO] [1753693005.604203972] [perception_3d_global.robosence]: scan_effective_positive_start: 0.0 [global_planner_node-4] [INFO] [1753693005.604250270] [perception_3d_global.robosence]: scan_effective_positive_end: 135.0 [global_planner_node-4] [INFO] [1753693005.604299676] [perception_3d_global.robosence]: scan_effective_negative_start: 0.0 [global_planner_node-4] [INFO] [1753693005.604355362] [perception_3d_global.robosence]: scan_effective_negative_end: -135.0 [global_planner_node-4] [INFO] [1753693005.604405546] [perception_3d_global.robosence]: euclidean_cluster_extraction_tolerance: 0.1 [global_planner_node-4] [INFO] [1753693005.604489365] [perception_3d_global.robosence]: euclidean_cluster_extraction_min_cluster_size: 3 [global_planner_node-4] [INFO] [1753693005.604547399] [perception_3d_global.robosence]: xy_resolution: 0.10 [global_planner_node-4] [INFO] [1753693005.604594949] [perception_3d_global.robosence]: height_resolution: 0.10 [global_planner_node-4] [INFO] [1753693005.604648165] [perception_3d_global.robosence]: marking_height: 2.00 [global_planner_node-4] [INFO] [1753693005.604711289] [perception_3d_global.robosence]: perception_window_size: 5.00 [global_planner_node-4] [INFO] [1753693005.604767458] [perception_3d_global.robosence]: segmentation_ignore_ratio: 0.90 [global_planner_node-4] [INFO] [1753693005.604816561] [perception_3d_global.robosence]: is_local_planner: 0 [global_planner_node-4] [INFO] [1753693005.604869497] [perception_3d_global.robosence]: expected_sensor_time: 0.10 [global_planner_node-4] [INFO] [1753693005.604944999] [perception_3d_global.robosence]: pub_gbl_marking_for_visualization: 1 [global_planner_node-4] [INFO] [1753693005.604993068] [perception_3d_global.robosence]: stitcher_num: 10 [global_planner_node-4] [WARN] [1753693005.609604893] [perception_3d_global]: Loop time: 100 ms [global_planner_node-4] [WARN] [1753693005.609743437] [perception_3d_global]: dGraph publish time: 1000 ms [global_planner_node-4] [INFO] [1753693005.611536678] [global_planner]: turning_weight: 0.10 [global_planner_node-4] [INFO] [1753693005.624616419] [dynamic_window_aware_global_planner]: look_ahead_distance: 5.00 [global_planner_node-4] [WARN] [1753693005.631615534] [perception_3d_global.map]: map receive new Map with size: 12467 [global_planner_node-4] [WARN] [1753693005.634981737] [perception_3d_global.map]: map receive new Ground with size: 1743 [global_planner_node-4] [WARN] [1753693005.710026975] [perception_3d_global.map]: map has already received two msg. [global_planner_node-4] [INFO] [1753693005.710105625] [perception_3d_global.map]: map starts to reset dynamic graph. [global_planner_node-4] [INFO] [1753693005.767641455] [perception_3d_global.robosence]: Receiving Lidar topic: /registered_cloud [global_planner_node-4] [INFO] [1753693006.115929954] [global_planner]: Waiting for static layer [global_planner_node-4] [INFO] [1753693006.235477835] [perception_3d_global.robosence]: robosence starts to reset dynamic graph. [global_planner_node-4] [INFO] [1753693006.242220940] [perception_3d_global.robosence]: robosence done dynamic graph regeneration. [global_planner_node-4] [WARN] [1753693006.302602267] [perception_3d_global]: Map update loop missed its desired rate of 10.0000Hz... the loop actually took 0.5927 seconds [global_planner_node-4] [INFO] [1753693006.616673341] [global_planner]: Ground and Kd-tree ground have been received from perception_3d. [global_planner_node-4] [INFO] [1753693006.619701180] [global_planner]: Publish weighted pc. [rviz2-7] [INFO] [1753693016.737844145] [rviz2]: Setting estimate pose: Frame:map, Position(6.62939, -2.00798, -0.854748), Orientation(0, 0, -0.839556, 0.543273) = Angle: -1.99293 [ros2-1] [icp_localization-1] User set pose to :t:[6.629386, -2.007981, -0.854748] ; q:[0.000000, 0.000000, -0.839556, 0.543273] ; rpy (deg):[0.000000, 0.000000, -114.186495] [rviz2-7] [INFO] [1753693022.474194271] [rviz2]: Setting goal pose: Frame:map, Position(5.78279, -3.50524, -0.853094), Orientation(0, 0, -0.854055, 0.520182) = Angle: -2.04746 [clicked2goal.py-6] [INFO] [1753693022.488150336] [clicked2p2p]: Got goal at: 5.78, -3.51, -0.85 [clicked2goal.py-6] [INFO] [1753693022.488690513] [clicked2p2p]: Sending Goal [clicked2goal.py-6] [INFO] [1753693022.489668708] [clicked2p2p]: Goal sent. Waiting response [clicked2goal.py-6] [INFO] [1753693022.490735648] [clicked2p2p]: Goal accepted :) [p2p_move_base_node-5] [INFO] [1753693022.491562082] [global_plan_manager]: Global plan manager is resumed [global_planner_node-4] [INFO] [1753693022.593801404] [dynamic_window_aware_global_planner]: Received new goal at: 5.78, -3.51, -0.85 [global_planner_node-4] [WARN] [1753693022.594414300] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.594444290] [global_planner]: Selected start: 6.83, -1.76, -0.66, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [p2p_move_base_node-5] [INFO] [1753693022.594694138] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [INFO] [1753693022.596282022] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693022.647007142] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.647060443] [global_planner]: Selected start: 6.83, -1.76, -0.66, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.649578472] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693022.691937967] [local_planner.local_planner]: Recieve new global plan. [p2p_move_base_node-5] [INFO] [1753693022.692148294] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693022.696973109] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.697032782] [global_planner]: Selected start: 6.83, -1.76, -0.66, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.698243564] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693022.748089781] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.748167796] [global_planner]: Selected start: 6.83, -1.76, -0.66, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.750853354] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693022.792121757] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693022.797015033] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.797082798] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.799818436] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693022.846971153] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.847031692] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.850767332] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693022.892096482] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693022.897015800] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.897094138] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.898665406] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [WARN] [1753693022.905476359] [local_planner.local_planner]: All trajectories are rejected by critics. [global_planner_node-4] [WARN] [1753693022.947016858] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.947078595] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.949664332] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693022.993506357] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693022.996862929] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693022.996917515] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693022.998857933] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.047007192] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.047084445] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.050167153] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.092312647] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.097042668] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.097108607] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.098290060] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.147014568] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.147083121] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.150475886] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.192207313] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.197000938] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.197078386] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.198428497] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.246992835] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.247058893] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.250694246] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.292120550] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.296979354] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.297060604] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.299234530] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.347018854] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.347093691] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.351655936] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.392580041] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.397029096] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.397082680] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.398202949] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.447043058] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.447105519] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.449867283] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.492837555] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.497130434] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.497216267] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.500505658] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.547004718] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.547070807] [global_planner]: Selected start: 6.83, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.554848656] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.593333706] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.596999505] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.597068657] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.598736681] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.647007360] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.647077223] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.650945569] [global_planner]: Path found from: 567 to 449 [p2p_move_base_node-5] [INFO] [1753693023.692907040] [global_plan_manager]: Goal accepted by: get_dwa_plan, waiting for result [global_planner_node-4] [WARN] [1753693023.696992155] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.697061401] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.699865412] [global_planner]: Path found from: 567 to 449 [global_planner_node-4] [WARN] [1753693023.746992707] [global_planner]: Selected goal: 5.78, -3.51, -0.85, Nearest-> id: 449, x: 5.79, y: -3.41, z: -0.86 [global_planner_node-4] [WARN] [1753693023.747059169] [global_planner]: Selected start: 6.84, -1.76, -0.67, Nearest-> id: 567, x: 7.02, y: -1.88, z: -0.87 [global_planner_node-4] [INFO] [1753693023.749869658] [global_planner]: Path found from: 567 to 449 ^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)

Jul 28 '25 08:07 adsaosdi

The log shows

[p2p_move_base_node-5] [WARN] [1753693022.905476359] [local_planner.local_planner]: All trajectories are rejected by critics.

There is obstacle around your robot. Please check following:

Is there anything around your lidar? You can cut your effective FOV by setting these parameters: https://github.com/dfl-rlab/dddmr_navigation/blob/20d6882df08f1c3ecbb6ae10f604d1614dc34343/src/dddmr_p2p_move_base/config/p2p_move_base_localization.yaml#L275
Does your point cloud include ground? If yes, you have to remove them. Or you can modify this line to temporary remove them: https://github.com/dfl-rlab/dddmr_navigation/blob/20d6882df08f1c3ecbb6ae10f604d1614dc34343/src/dddmr_perception_3d/plugins/multilayer_spinning_lidar.cpp#L243 You can set minimum height from 0.0 to 0.1.
Quick check is that using Rviz to display your pointcoud that sends to the plugin. Make sure there is no pointcloud in your robot cuboid: https://github.com/dfl-rlab/dddmr_navigation/blob/20d6882df08f1c3ecbb6ae10f604d1614dc34343/src/dddmr_p2p_move_base/config/p2p_move_base_localization.yaml#L271

Jul 29 '25 05:07 tsengapola

1.There is indeed a box around my radar on the back of my robot, which causes the inside of my robot's box to contain radar point clouds. How should I remove the point cloud inside the box? 2. My point cloud does indeed include ground point clouds. I have adjusted the minimum height to 0.0 to 0.1 as per your suggestion to temporarily avoid this issue. 3. This is the terminal display that supplements the previous p2p_move_base_node after I publish the target point: [p2p_move_base_node-5] [ERROR] [1753777065.696864704] [local_planner.local_planner]: Perception 3D is not ok. [p2p_move_base_node-5] [WARN] [1753777065.696894114] [p2p_move_base]: Sensor data is out of date, we're not going to allow commanding of the base for safety [p2p_move_base_node-5] [WARN] [1753777066.399108744] [local_planner.local_planner]: Local planner control time exceed expect time: 0.10 but is 0.10 [p2p_move_base_node-5] [WARN] [1753777066.836665094] [local_planner.local_planner]: Local planner control time exceed expect time: 0.10 but is 0.14

[p2p_move_base_node-5] [WARN] [1753777068.564740754] [local_planner.local_planner]: Local planner control time exceed expect time: 0.10 but is 0.22 [p2p_move_base_node-5] [WARN] [1753777068.564798373] [p2p_move_base]: Controller time out, go to recovery [p2p_move_base_node-5] [INFO] [1753777068.567613710] [p2p_move_base]: Goal accepted by recovery behaviors server, waiting for result [p2p_move_base_node-5] [WARN] [1753777068.570718723] [recovery_behaviors.rotate_inplace]: Behavior is running with thread ID : 140562325825088 [p2p_move_base_node-5] [WARN] [1753777068.589892394] [recovery_behaviors.rotate_inplace]: rotate_inplace: All trajectories are rejected by critics. [p2p_move_base_node-5] [INFO] [1753777073.578912938] [recovery_behaviors.rotate_inplace]: Behavior abort. [p2p_move_base_node-5] [ERROR] [1753777073.579457858] [p2p_move_base]: Recovery Behaviors: Goal was aborted [p2p_move_base_node-5] [ERROR] [1753777073.675256013] [p2p_move_base]: The potential collision has been detected when doing recovery. [p2p_move_base_node-5] [INFO] [1753777073.675539695] [global_plan_manager]: Global plan manager is stopped

Jul 29 '25 09:07 adsaosdi

Please provide me the param.yaml of p2p_move_base so I can check. Please also provide me following information:

Your lidar topic
Your TF setup in Rviz (not tf tree), I need to see what is your lidar orientation, so we can cut the backward part.
Sensor data is out of date, we're not going to allow commanding of the base for safety ---> means your point cloud topic is lost, please check you setup topic in parameters correctly.

Jul 29 '25 09:07 tsengapola

p2p_move_base: ros__parameters: controller_frequency: 10.0 planner_patience: 10.0 oscillation_distance: 5.0 oscillation_angle: 1.0 oscillation_patience: 15.0 controller_patience: 10.0 no_plan_retry_num: 10 waiting_patience: 10.0 global_plan_manager: ros__parameters: global_planner_action_name: "get_dwa_plan" global_plan_query_frequency: 5.0

local_planner: ros__parameters: odom_topic: /dog_odom forward_prune: 3.0 backward_prune: 1.0 heading_tracking_distance: 0.5 heading_align_angle: 0.5 prune_plane_timeout: 3.0 xy_goal_tolerance: 0.3 yaw_goal_tolerance: 0.3 controller_frequency: 10.0

cuboid:
  flb: [0.6, 0.25, 0.0] #front left buttom
  frb: [0.6, -0.25, 0.0]
  flt: [0.6, 0.25, 0.7] #front left top
  frt: [0.6, -0.25, 0.7]
  blb: [-0.6, 0.25, 0.0] #back left buttom
  brb: [-0.6, -0.25, 0.0]
  blt: [-0.6, 0.25, 0.7]
  brt: [-0.6, -0.25, 0.7]

recovery_behaviors: ros__parameters: plugins: ["rotate_inplace"] rotate_inplace: plugin: "recovery_behaviors::RotateInPlaceBehavior" frequency: 10.0 tolerance: 0.3 trajectory_generator_name: "differential_drive_rotate_inplace"

trajectory_generators: ros__parameters: plugins: ["differential_drive_simple", "differential_drive_rotate_inplace", "differential_drive_rotate_shortest_angle"] differential_drive_rotate_shortest_angle: plugin: "trajectory_generators::DDRotateInplaceTheory" controller_frequency: 10.0 rotation_speed: 0.5 cuboid: flb: [0.6, 0.25, 0.0] #front left buttom frb: [0.6, -0.25, 0.0] flt: [0.6, 0.25, 0.7] #front left top frt: [0.6, -0.25, 0.7] blb: [-0.6, 0.25, 0.0] #back left buttom brb: [-0.6, -0.25, 0.0] blt: [-0.6, 0.25, 0.7] brt: [-0.6, -0.25, 0.7]

differential_drive_rotate_inplace:
  plugin: "trajectory_generators::DDRotateInplaceTheory"
  controller_frequency: 10.0
  rotation_speed: 0.5
  cuboid:
    flb: [0.6, 0.25, 0.0] #front left buttom
    frb: [0.6, -0.25, 0.0]
    flt: [0.6, 0.25, 0.7] #front left top
    frt: [0.6, -0.25, 0.7]
    blb: [-0.6, 0.25, 0.0] #back left buttom
    brb: [-0.6, -0.25, 0.0]
    blt: [-0.6, 0.25, 0.7]
    brt: [-0.6, -0.25, 0.7]

differential_drive_simple:
  plugin: "trajectory_generators::DDSimpleTrajectoryGeneratorTheory"
  max_vel_x: 1.0
  min_vel_x: 0.1 #0.1 This value should not be negative, because we want to customize backward behavior
  max_vel_theta: 0.6
  min_vel_theta: 0.15
  acc_lim_x: 1.0
  acc_lim_theta: 3.0
  deceleration_ratio: 2.0
  max_motor_shaft_rpm: 3000.0
  wheel_diameter: 0.16
  gear_ratio: 1.0
  robot_radius: 0.25
  
  controller_frequency: 10.0
  sim_time: 2.0
  linear_x_sample: 5.0
  angular_z_sample: 10.0
  sim_granularity: 0.05
  angular_sim_granularity: 0.025

  cuboid:
    flb: [0.6, 0.25, 0.0] #front left buttom
    frb: [0.6, -0.25, 0.0]
    flt: [0.6, 0.25, 0.7] #front left top
    frt: [0.6, -0.25, 0.7]
    blb: [-0.6, 0.25, 0.0] #back left buttom
    brb: [-0.6, -0.25, 0.0]
    blt: [-0.6, 0.25, 0.7]
    brt: [-0.6, -0.25, 0.7]

mpc_critics: ros__parameters: plugins: ["collision", "stick_path", "pure_pursuit", "toward_global_plan", "collision_rotate", "collision_rotate_shortest", "prefer_rotate_shortest"] collision: plugin: "mpc_critics::CollisionModel" trajectory_generator: differential_drive_simple weight: 1.0 stick_path: plugin: "mpc_critics::StickPathModel" trajectory_generator: differential_drive_simple weight: 0.1 pure_pursuit: plugin: "mpc_critics::PurePursuitModel" trajectory_generator: differential_drive_simple translation_weight: 1.0 orientation_weight: 0.01 toward_global_plan: plugin: "mpc_critics::TowardGlobalPlanModel" trajectory_generator: differential_drive_simple weight: 1.0 collision_rotate: plugin: "mpc_critics::CollisionModel" trajectory_generator: differential_drive_rotate_inplace weight: 1.0 collision_rotate_shortest: plugin: "mpc_critics::CollisionModel" trajectory_generator: differential_drive_rotate_shortest_angle weight: 1.0
prefer_rotate_shortest: plugin: "mpc_critics::ShortestAngleModel" trajectory_generator: differential_drive_rotate_shortest_angle weight: 1.0

perception_3d_local: ros__parameters: global_frame: "map" robot_base_frame: "robot_footprint" max_obstacle_distance: 20.0 inscribed_radius: 0.7 inflation_descending_rate: 1.0 inflation_radius: 0.5 sensors_collected_frequency: 10.0 plugins: ["map", "path_blocked_strategy", "lidar"] map: plugin: "perception_3d::StaticLayer" is_local_planner: true mapping_mode: false map_topic: "mapcloud" ground_topic: "mapground" path_blocked_strategy: plugin: "perception_3d::PathBlockedStrategy" check_radius: 0.3 lidar: plugin: "perception_3d::MultiLayerSpinningLidar" is_local_planner: true topic: "/rslidar_points" pub_gbl_marking_for_visualization: false vertical_FOV_top: 15.0 vertical_FOV_bottom: -55.0 scan_effective_positive_start: 30.0 scan_effective_positive_end: 180.0 scan_effective_negative_start: -30.0 scan_effective_negative_end: -180.0 resolution: 0.05 height_resolution: 0.05 marking_height: 2.0 perception_window_size: 3.0 #the window for marking/clearing in meter, we dont casting to maximum range of the sensor even it is gbl map segmentation_ignore_ratio: 0.5 #f a segmentation match static pc with this ratio, we ignore it. ex: a wall expected_sensor_time: 0.1

perception_3d_global: ros__parameters: global_frame: "map" robot_base_frame: "robot_footprint" max_obstacle_distance: 20.0
inscribed_radius: 0.7 inflation_descending_rate: 1.0 inflation_radius: 0.5 sensors_collected_frequency: 5.0 plugins: ["map", "lidar"] dgraph_publish_frequency: 1.0 map: plugin: "perception_3d::StaticLayer" is_local_planner: false use_adaptive_connection: false #If true: the neighbors of node is sparse, it will automatically increase search radius. adaptive_connection_number: 20 radius_of_ground_connection: 1.5 #If use_adaptive_connection=false, the radius search of the node is applied. intensity_search_radius: 1.0 #this value is suggested to be 1.0 meters, because if it is too large, the narrow passage will be miscalculated intensity_search_punish_weight: 0.1 #the weight for a node. Set this value higher means we dont like path planning cross a orphan node. static_imposing_radius: 1.5 #use map to add weight on each ground node mapping_mode: false map_topic: "mapcloud" ground_topic: "mapground" lidar: plugin: "perception_3d::MultiLayerSpinningLidar" is_local_planner: false topic: "/rslidar_points" pub_gbl_marking_for_visualization: true vertical_FOV_top: 15.0 vertical_FOV_bottom: -55.0 scan_effective_positive_start: 30.0 scan_effective_positive_end: 180.0 scan_effective_negative_start: -30.0 scan_effective_negative_end: -180.0 euclidean_cluster_extraction_tolerance: 0.1 euclidean_cluster_extraction_min_cluster_size: 1 xy_resolution: 0.05 height_resolution: 0.05 marking_height: 2.0 perception_window_size: 5.0 #the window for marking/clearing in meter, we dont casting to maximum range of the sensor even it is gbl map segmentation_ignore_ratio: 1.1 #f a segmentation match static pc with this ratio, we ignore it. ex: a wall expected_sensor_time: 0.1

global_planner: ros__parameters: turning_weight: 0.5 #Punish the zig-zag path

My lidar topic uses the original point cloud data topic given by the radar: /rslidar_points [global_planner_node-4] [INFO] [1753784979.040792315] [perception_3d_global.lidar]: Receiving Lidar topic: /rslidar_points [p2p_move_base_node-5] [INFO] [1753784979.240183115] [perception_3d_local.lidar]: Receiving Lidar topic: /rslidar_points unitree@unitree-b2-pc4:~/dddmr_navigation$ ros2 topic echo --once /rslidar_points header: stamp: sec: 1753785020 nanosec: 180599451 frame_id: rslidar height: 1800 width: 32 fields:

name: x offset: 0 datatype: 7 count: 1
name: y offset: 4 datatype: 7 count: 1
name: z offset: 8 datatype: 7 count: 1
name: intensity offset: 12 datatype: 7 count: 1
name: ring offset: 16 datatype: 4 count: 1
name: timestamp offset: 18 datatype: 8 count: 1 is_bigendian: false point_step: 26 row_step: 832 data:
223

Jul 29 '25 10:07 adsaosdi

I tried to modify the range in an attempt to block the point cloud input to the back of the radar by the perception module, but failed. The point cloud would still be contained inside the box. So I wrote a node, filtered out all the point cloud data within the box, and successfully achieved the navigation control of the robot. Thank you very much for your help.

Jul 30 '25 09:07 adsaosdi