如何在已有pcd地图和定位的情况下接入dddmr

[sheng7564]

作者您好，关于您的导航框架，我实在不太清楚如何使用，我的需求是这样的。
目前，我已经有了基于fastlio2建立的图，Globalmap.pcd。我也有基于fastlio2的定位框架，其可以根据已有地图和3D点云输入，输出定位。我的任务目标是实现3D导航，所以我目前迫切需要一个3D导航框架。因此，我经过查找资料，找到了dddmr_navigation，但我不知道该如何把我的全局地图pcd接入到这个导航框架中，这个pcd地图是否需要被二次处理。 pose_graph_dir: "/root/map"这个参数要求给一个目录，我不知道该如何让我的数据与框架对齐。如果有必要的话，我也可以放弃我自己的定位框架，转而使用mcl_3dl。

[lfm-a]

同问

[tsengapola]

@sheng7564 @lfm-a

If you want to use your localization system (i.e. you can slam and localize), then:
You just need to pipe in TF, usually, map->base_link. Remove mcl_3dl and mcl_features. The launch file will be something like:

<launch>
  
  <!--Your localiztion node-->

  <!--- TF -->
  <node pkg="tf2_ros" exec="static_transform_publisher" name="sensor2baselink"  args="0.3 0 0.38 -3.1415926535 0.0 0 base_link laser_link" />

  <!--name argument is not required, because it will overwrite all node name in the exec-->
  <node pkg="global_planner" exec="global_planner_node" output="screen" respawn="false">
    <param from="$(find-pkg-share p2p_move_base)/config/p2p_move_base_localization.yaml" />
  </node>

  <!--name argument is not required, because it will overwrite all node name in the exec-->
  <node pkg="p2p_move_base" exec="p2p_move_base_node" output="screen" respawn="false">
    <param from="$(find-pkg-share p2p_move_base)/config/p2p_move_base_localization.yaml" />
  </node>
  <node pkg="p2p_move_base" exec="clicked2goal.py" output="screen" respawn="false"/>
  

  <let name="rviz_config" value="$(find-pkg-share p2p_move_base)/rviz/p2p_move_base_localization.rviz"/>
  <node name="rviz2" pkg="rviz2" exec="rviz2" args="-d $(var rviz_config)"/>

</launch>

And then publish your pcd to map_topic and ground_topic:

dddmr_navigation/src/dddmr_p2p_move_base/config/p2p_move_base_localization.yaml

Line 263 in d8e9b02

map_topic: "mapcloud"

dddmr_navigation/src/dddmr_p2p_move_base/config/p2p_move_base_localization.yaml

Line 264 in d8e9b02

ground_topic: "mapground"

dddmr_navigation/src/dddmr_p2p_move_base/config/p2p_move_base_localization.yaml

Line 306 in d8e9b02

map_topic: "mapcloud"

dddmr_navigation/src/dddmr_p2p_move_base/config/p2p_move_base_localization.yaml

Line 307 in d8e9b02

ground_topic: "mapground"

Because you only have a whole map (includes feature and ground), so you can only publish two topics using the same pcd.
However, since the ground and feature are mixed, the path planning results might not be good and will be slow.
I would suggest to to some post processing on your pcd, basically, keep the ground point cloud and remove anything else.

[tsengapola]

@sheng7564 @lfm-a

I have uploaded an example for one pcd file, if you want to use your pcd, replace your pcd to following names:
0_feature.pcd
0_ground.pcd
in here:
https://github.com/dfl-rlab/dddmr_navigation/tree/main/src/dddmr_mcl_3dl/map/pcd

And change your dir respectively:

dddmr_navigation/src/dddmr_mcl_3dl/config/mcl_3dl.yaml

Line 3 in 89874e0

pose_graph_dir: "/root/dddmr/src/dddmr_mcl_3dl/map"

Finally setup a large numbers for both following parameters, say 300:

dddmr_navigation/src/dddmr_mcl_3dl/config/mcl_3dl.yaml

Line 4 in 89874e0

sub_map_search_radius: 50.0

dddmr_navigation/src/dddmr_mcl_3dl/config/mcl_3dl.yaml

Line 5 in 89874e0

sub_map_warmup_trigger_distance: 20.0

[sheng7564]

@tsengapola Thank you so much!!!!!!!!!!!!!!! I'll try it in my environment right away

[sheng7564]

@tsengapola
I tried the two method.
1.use my own localize.
1.1I write a node to publish the global point cloud. The data is from your bag /root/dddmr_bags/gz_go2_demo_world/map.pcd and ground.pcd.
1.2 I change the go2_localization.yaml . I change the map_topic and ground_topic name.
1.3 I change go2_localizatioin.launch. My launch file has global_planner, p2p_move_base and rviz2.
1.4 I mocked the localize using "ros2 run tf2_ros static_transform_publisher -5 0 0 0 0 0 map base_link".
Finally,I failed. I can't see the global_path.

2.use mcl_3dl
2.1 I add mcl_3dl to the launch file.
2.2 I add the same pcd data to the dir "/root/ros2_docker_humble_ws/src/dddmr_navigation/src/dddmr_mcl_3dl/map/pcd" and change their name.
2.3 I change the go2_localization.yaml.Recover the map_topic and ground_topic and change the pose_graph_dir,sub_map_search_radius and sub_map_warmup_trigger_distance.
2.4 same to 1.4
Finally,I failed.It looks like different,but i can also not see the global path.

[tsengapola]

Do you setup lidar topic for plugin correctly?

dddmr_navigation/src/dddmr_p2p_move_base/config/go2_localization.yaml

Line 271 in 89874e0

topic: "segmented_cloud_pure"

dddmr_navigation/src/dddmr_p2p_move_base/config/go2_localization.yaml

Line 298 in 89874e0

plugin: "perception_3d::StaticLayer"

Quick one to test is to remove lidar plugin and test global path planning (remove "lidar"):

dddmr_navigation/src/dddmr_p2p_move_base/config/go2_localization.yaml

Line 258 in 89874e0

plugins: ["map", "path_blocked_strategy", "lidar"]

dddmr_navigation/src/dddmr_p2p_move_base/config/go2_localization.yaml

Line 258 in 89874e0

plugins: ["map", "path_blocked_strategy", "lidar"]

If you specify lidar plugin, but none topic being received, the path planning will reject all path due to safety concern.

[sheng7564]

@tsengapola After many attempts, I'm sorry that I still can't get the path I want. I have map and localize. I have no lidar(now but not future) and poses.pcd. Coud you tell me how to code a total yaml and config

[sheng7564]

@tsengapola I tried use the mcl_3dl and I remove the lidar topic plugin. I can finally see the path !!! But I still have no method to find the path without mcl_3dl.

[tsengapola]

You can use pcl publisher node in mcl_3dl package to publish your point cloud to global planner without using mcl_3dl node, something like below:

<launch>
  <let name="use_sim" value="false"/>
  
  <node pkg="tf2_ros" exec="static_transform_publisher" name="baselink2footprint"  args="-5 0.0 0.0 0 0 0 map base_link" />

  <node pkg="mcl_3dl" exec="pcl_publisher" output="screen" respawn="false">
    <param from="$(find-pkg-share mcl_3dl)/config/pcl_publisher.yaml" />
    <param name="map_dir" value="$(find-pkg-share mcl_3dl)/map/map.pcd" />
    <param name="ground_dir" value="$(find-pkg-share mcl_3dl)/map/ground.pcd" />
  </node>
  
  <!--name argument is not required, because it will overwrite all node name in the exec-->
  <node pkg="global_planner" exec="global_planner_node" output="screen" respawn="false">
    <param from="$(find-pkg-share p2p_move_base)/config/go2_localization.yaml" />
    <param name="use_sim_time" value="$(var use_sim)"/>
  </node>

  <!--name argument is not required, because it will overwrite all node name in the exec-->
  <node pkg="p2p_move_base" exec="p2p_move_base_node" output="screen" respawn="false">
    <param from="$(find-pkg-share p2p_move_base)/config/go2_localization.yaml" />
    <param name="use_sim_time" value="$(var use_sim)"/>
  </node>

  <node pkg="p2p_move_base" exec="clicked2goal.py" output="screen" respawn="false">
    <param name="use_sim_time" value="$(var use_sim)"/>
  </node>
  

  <node pkg="rviz2" exec="rviz2" output="screen" respawn="false" args="-d $(find-pkg-share p2p_move_base)/rviz/p2p_move_base_localization.rviz">
    <param name="use_sim_time" value="$(var use_sim)"/>
  </node>

</launch>

pcl_publisher.yaml (in mcl_3dl package) allows you to rotate your pcd file if needed

[sheng7564]

@tsengapola Oh，thanks！！！！！I make it .Before this, I was always using my own pcl_publisher, which had some issues. Now, after using your version, they are resolved.

[sheng7564]

During my test , I use https://github.com/byangw/PCT_planner/blob/main/README.md
spiral0.3_2.pcd.
It is low and it can't navigate to the top floor. What should I do to modify it.

[tsengapola]

You need to modify the algorithm to add z exploration limit, because this map contains pillar which is not ground.

spiral.mp4

We suggest to explore our navigation stack from mapping to navigation, because we try to
accommodate most of real world issues.

The modification point is here:

dddmr_navigation/src/dddmr_global_planner/src/a_star_on_pc.cpp

Line 283 in 02a7f52

//if(getPitchFromParent2Expanding(pcl_current_parent, pcl_current, pcl_expanding)>0.2)

enable line 283, 284 and setup threshold to 0.6. You should be able to get what the video shows.

Again this map includes wall, pillar etc..., it is not a perfect map for path planning, unless there is an algorithm to tell A* that this point is not ground for the path planning😃

[sheng7564]

I still have two problem.
1.why you can operate so fluently?When I publish another point , it takes a lot of time to show the path. And after i enable line 283,284 above and setup threshold to 0.6 , i can not be able to get the video show😭.
2.It takes a lot of time to "Waiting for static layer".May be 3mins for the spiral0.3_2.pcd. Is that normal for real world map? If I use larger map,it may take more time.

[tsengapola]

Let's check one by one:

1. Do you use our docker image?
2. Do you build by Release mode?
3. Which computing platform you are using?
4. For me it took 30 seconds for static layer.
   It is computing the edge of the map which takes time, so yes, if it is a large map with dense point cloud, it will take a lot longer. But if you the map is static, then a good approach is to save generated result, so next time the boot up will be fast. However, we have not implement this features yet.

[sheng7564]

1.No，I use humble docker (osrf/ros:humble-desktop-full-jammy), git clone and colcon build by myself.
2.No. I build by default(colcon build).
3.Ubuntu 22.04 humble.I use x86_64 for test. I will apply it in the Unitree go2 edu which is jetson with cuda114 in the future using docker-ros2.
4.It takes me 3 mins for spiral0.3_2.pcd and about 30s for /root/dddmr_bags/gz_go2_demo_world/ground.pcd
5. I enable line 283,284 above and setup threshold to 0.6 , i can not be able to get the video show😭.