Having some troubles when run the gbplanner2 with real Lidar databag

[Rye450]

Hello,I'm trying to run gbplanner2 with a real UAV and Lidar sensor(ouster) So I changed the odometry topic name and lidar topic name in rmf_sim.launch file as follows:

<arg name="odometry_topic" default="/aft_mapped_to_init"/>
<remap from="/pointcloud" to="/os_cloud_node/points"/>

Then,rviz shows valid samples are limited in a small cuboid as follows: and PCI shows no errors as follows:

I also tried to modify some parameters in three config files,for example,BoundedSpaceParams-Local,RobotParams-size,tsdf_voxel_size,tsdf_voxel_per_side,edge_length_min/max.However,it does no help.

Could you please tell me the reason why? I'm so desperate wondering why. Thanks!

[MihirDharmadhikari]

Hello @Rye450

Sorry for the delay. This issue arises when voxblox does not receive the point cloud or can not find the transformation between the sensor frame and the fixed frame. Can you please check if the transform exists and the fixed frame is set correctly here?

Best, Mihir

[Rye450]

Thank so much for your reply!

The frame_id of point cloud and odometry is /os_lidar and /camera_init respectively as follows: And then,I transform /os_lidar to /rmf_obelix/rmf_obelix/velodyne which is the frame_id of point cloud in simulation (topic name is /rmf_obelix/velodyne_points) and /camera_init to /world which is the frame_id of odometry (topic name is /rmf_obelix/ground_truth/odometry) in simulation by adding following code in the launch file: <node pkg="tf" type="static_transform_publisher" name="os_lidar_to_velodyne" args="0.0 0.0 0.0 0.0 0.0 0.0 /os_lidar /rmf_obelix/rmf_obelix/velodyne 1" /> <node pkg="tf" type="static_transform_publisher" name="camera_init_to_world" args="0.0 0.0 0.0 0.0 0.0 0.0 /camera_init /world 1" /> As to the transformation between the sensor frame and the fixed frame,I modify the odometry topic name in launch file as follows: <arg name="odometry_topic" default="/aft_mapped_to_init"/> I check the fixed frame,it is set to "world" correctly cause I didn't change anything in voxblox_sim_config.yaml file. I also change the point cloud topic name in launch file as follows: <remap from="odometry" to="/integrated_to_init" /> Thus,I think voxblox does receive the point cloud.

Could you please tell me where is the problem? Looking forward to your reply! Thanks again!

[MihirDharmadhikari]

Hi @Rye450

Among the static transforms that you have set, you will only need the second one, i.e. <node pkg="tf" type="static_transform_publisher" name="camera_init_to_world" args="0.0 0.0 0.0 0.0 0.0 0.0 /camera_init /world 1" />. Can you send me your exact launch file that you are using as well as the screenshot of the tf_tree visualization in rqt?

Best, Mihir

[Rye450]

Thanks a lot for your reply! Here is the launch file I'm using: https://github.com/Rye450/gbplanner_test/blob/dc03317fb38d043d1d83ef724dbb0c66b617cf34/byl_Lidar_test.launch And the tf_tree visualization in rqt is as follows:

I still cannot figure out what's wrong. Looking forward to your reply!

[Rye450]

Hi @MihirDharmadhikari

Considering the transformation between the sensor frame and the fixed frame which you mentioned before,I make some changes in the launch file,the link of launch file is as follows: https://github.com/Rye450/gbplanner_test/blob/8f7092edeece4e417b722e12234787c0ffb6170e/byl_tf.launch I add one transformation between /world and /rmf_obelix/base_link and another transformation between /world and /rmf_obelix/odometry_sensor1 by running node transformFusion： https://github.com/Rye450/gbplanner_test/blob/8f7092edeece4e417b722e12234787c0ffb6170e/byl_tf.launch#L29 I also delete the transformation between /os_lidar and /rmf_obelix/rmf_obelix/velodyne according to your advice. And the tf_tree visualization in rqt is as follows:

Now,it looks exactly same as the tf_tree when I'm running rmf_sim.launch except for /camera_init frame. However,it still doesn't work.The interface of rviz is as follows:

The reason why I delete https://github.com/Rye450/gbplanner_test/blob/8f7092edeece4e417b722e12234787c0ffb6170e/byl_tf.launch#L35 is that it causes jump back in time as terminal output shows:

I think time jump back is caused by the time collision between data bag and gazebo.

Can you please tell me the reason why it doesn't work?

Thanks!

[MihirDharmadhikari]

Hi @Rye450 ,

I think there is a little confusion about the frames. All the rmf_obelix related frames are simulation specific, so you won't need those. So you can remove all the static transforms for those as well. For gbplanner you need the following things:

• A tf between the world frame and the fixed frame of your odometry system, if I understand correctly it will be camera_init for you. This will be a static tf.
• Your odometry solution should provide the transform between the camera_init and the ouster frame. You can find your ouster frame id by echoing the header of the pointcloud that your ouster driver provides. When you visualize the tf tree you should see a connection between the world and the sensor frame.
• Odometry to gbplanner and pci_general: You have already provided this
• Remap the correct point cloud topic: You have already done this.
• Set the use_sim_time to false here unless you are replaying a rosbag and running the planner.

The remaining of the launch file looks correct to me. Note that due to this remapping the output trajectory will be published on the topic /rmf_obelix/command/trajectory so you will have to either change it to the topic that you position controller subscribes to or remap the position controller's topic to this where it is launched.

I think running the planner with custom setup was a little hard to follow from the wiki, so I will add more explicit instructions about this.

Let me know if this works.

Best, Mihir

[Rye450]

Hi @MihirDharmadhikari Thank you so much for your reply! I have made some modifications according to your reply,and I couldn't tell whether the planner works. The output of terminal is as follows: Unlike the simulation result,the terminal shows in planning iteration 1,the planner formed a graph with 7 vertices and 8 edges with 4001 loops.The number of vertices and edges is extremely small compared to simulation result. Do I need to change other parameters like Local Space and robot size to find more vertices and edges? Beside,the planner stopped at planning iteration 1 or planning iteration 0.Is it caused by the fact that I'm replaying a rosbag and running the planner? The interface of rviz is as follows:

Looking forward to your reply!Thanks a lot for the time and effort you dedicate to your open-source repository!

[MihirDharmadhikari]

Hi @Rye450

Does the graph that is built make sense in the visualization? Also, do you see the voxblox map in rviz? If possible, can you record a rosbag and send it to me? Please include the following topics:

• /tf
• /tf_static
• your point cloud topic
• your odometry topic
• the voxblox occupied voxels (the name should be something like gbplanner_node/occupied_nodes)
• all the topics starting with vis prefix

Best, Mihir

[MihirDharmadhikari]

Hi @Rye450

I just saw the rviz screenshot that you added. The map and the graph looks correct to me. One thing that you will have to do before you start the planner is the initialization motion. Lift the robot up and move forward a little so that it maps the space around it properly. Then you should be able to see the graph built and the path selected. Let me know if this works.

Best, Mihir

[Rye450]

Hello @MihirDharmadhikari

• Do you mean I have to initialize the real MAV loaded with real Lidar according to init_motion parameters in planner_control_interface_sim_config.yaml?
• Is it correct for the planner to form a graph with only 7 vertices and 8 edges with 4001 loops in a planning iteration? If it isn't,could you tell me the reason why?

Thanks as always!

[MihirDharmadhikari]

Hi @Rye450

You don't need to initialize the robot exactly according to the parameters in the planner_control_interface_sim_config.yaml. But the real MAV with the lidar will have to move a little before you start the planner because it needs to build a map around it to plan.

Regarding your second question, those numbers are very small, but if you are in a very small space or if majority of the space around the robot is not free this can happen. I believe the rviz screenshot that you have added is not from the same planning iteration as it has a lot more vertices than 7.

Best, Mihir

[Rye450]

Hello @MihirDharmadhikari

Could you tell the reason why the planner stopped at planning iteration 1 or planning iteration 0? I'm currently replaying a rosbag and testing the planner.Is lt because of the trajectory of robot in the rosbag cannot reach the goal point which the planner assigns the robot to? In other words,the planner will not start another iteration until the robot arrives at the goal point result of current planning iteration?

[MihirDharmadhikari]

Hi @Rye450

As you correctly identified, the planner will not trigger the next iteration until the robot reaches the end of the current path. However, you can call the planner_control_interface trigger service to trigger one planning iteration whenever you call the service. The same can be done by directly calling the service gbplanner. I would recommend the second one if you are replaying a rosbag.

Best, Mihir

[Rye450]

Hi @MihirDharmadhikari

• Recently, I'm trying to add another MAV in simulation and then two MAVs can explore the same environment separately(from different starting point).

The link of launch file which I'm using is as follows: https://github.com/Rye450/gbplanner_test/blob/1583caafbd70fc93400b224723fe6b64b8da025d/byl_two.launch However,the belowing terminal output shows that the second MAV seems to plan much slower than the first one: The planner of the first MAV has already planned for 100 times,while the planner of the second MAV has only planned for 5 times. The second MAV stays still all the time while the first MAV has already moved for a long distance in gazebo as follows: I have already checked the tf tree,there are transformations between the sensor frame of two MAVs and the fixed frame. Could you tell me what's wrong with my launch file?

• According to your last reply,I can trigger the next iteration by directly calling the service gbplanner. But I have no idea how to determine the parameters which the service requires. The required parameters of rosservice gbplanner are:header,bound_mode and root_pose. Could you give a example of calling the service gbplanner?

Looking forward to your reply! Thanks as always!

[MihirDharmadhikari]

Hi @Rye450,

I have answered the first question about the multi-robot simulation in the separate issue that you opened. Regarding the service arguments, you can leave the header and bound_mode empty. The root_pose is the pose from which you want the planner to consider as the root of the graph it builds. Putting it all zeros (including the orientation) will trigger the planner from the current robot location. For more details about the service arguments please check the comments in the planner_srv.srv file.

Best, Mihir

[Henryhongw]

Hi @Rye450 To enable a single robot to perform frontier detection based on the point cloud data provided by the rosbag data, what modifications did you finally make to the launch file? I use lego_loam to obtain the odometry information of the robot in real time, and the 3D Lidar uses Hesai Pandar40P (topic: hesai/pandar, frame_id: Pandar40P). System:Ubutu18.04, ROS Melodic

[ajay1606]

@Rye450 and @Henryhongw , Would you please help me, what modification you made to work with own lidar rosbag ? Currently, I am using LiDAR SLAM and able to publish odometry and unable to get planner work as there is tf need to be fixed. Curently, my Lidar data in "velodyne" frame id.

[MihirDharmadhikari]

Hi @ajay1606 ,

Your SLAM solution will need to publish a tf from the sensor frame (velodyne in your case) to the world frame. Voxblox queries this tf to integrate the point cloud in the map. Can you check this and let me know if this tf is published?

Best, Mihir

[ajay1606]

@MihirDharmadhikari Thank you so much for your response ! And after publishing odom topic tf from sensor frame to world frame, Its looks like this.

And in the tf, robot base_link is not configured correctly i guess, would please help where i am missing something?

[MihirDharmadhikari]

Hi @ajay1606 ,

It is hard to understand what is going on from this image. Could you record a video of rviz and send that to better understand what the issue might be?

Best, Mihir

[ajay1606]

@MihirDharmadhikari

Here is the sample test video, I have followed as you suggested. Now odom is in velodyne frame and tf configured to velodyne to world.

I can visualize the occupied voxels, But planning is not working as there were no trajectories i can visualize. Also i tried with initialize and start planner from the GUI. But nothing works. Sorry if my understanding is not clear. Would you please help me to generate trajectory from this planner ?

https://github.com/ntnu-arl/gbplanner_ros/assets/29722843/455996ed-f544-4ee2-bb73-3d0f4bb1768f

Best regards, Ajay

[ajay1606]

@MihirDharmadhikari With fixed_frame id to world i am able to see like below.

But after selecting initialization and start_planner from GUI, there is nothing changed means, still unable to see the trajectory path. Would you please check once and share your thoughts !. Would be very much helpful.

Looking forward to hearing back from you.

[ajay1606]

@MihirDharmadhikari

Here is my sample test bag file with following topics: odom: robot/odom Point cloud:/points _raw

Sample test bag: test.bag

Able visualized occupied voxel, but unable to see the planned trajectory ! Seems planner is not working. Would you please check once.

And below is the launch file currently using.

<launch>
  <!-- All settings -->
  <arg name="robot_name" default="smb"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->
  <param name="use_sim_time" value="$(arg use_sim_time)"/>

  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/> 
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>

  <!-- Static TF -->
  <node pkg="tf" type="static_transform_publisher" name="robot_world_link" args="0 0 0 0 0 0 world navigation 100" />

  <include file="$(find smb_path_tracker)/launch/pure_pursuit_tracker.launch"/>
  <node pkg="topic_tools" type="relay" name="vlp_relay" args="/points_raw /input_pointcloud" />
  <node pkg="topic_tools" type="relay" name="front_lidar_relay" args="/points_raw /input_pointcloud" />
  <arg name="odometry_topic" default="/robot/odom"/>
  <node name="pose_throttler" type="throttle" pkg="topic_tools" args="messages $(arg robot_name)/ground_truth/pose_with_covariance 10 /msf_core/pose" />

  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <!-- <remap from="odometry" to="$(arg odometry_topic)" /> -->
    <remap from="odometry" to="/robot/odom" />
    <remap from="/pointcloud" to="/input_pointcloud" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>

  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">
    <remap from="command/trajectory" to="$(arg robot_name)/command/trajectory" />
    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/smb.rviz"/>

</launch>

[celikemir]

Hİ @MihirDharmadhikari I am facing same issue, voxblock working but when i move the robot voxblocks not moving as odometry moves. I think that gbplanner cannot subscribe properly to odom topic due to frame_id even though I have changed frame_id to world

[MihirDharmadhikari]

Hello,

Voxblox integrates point clouds by querying the tf for the transformation between the frame in which the point cloud is published in and the fixed frame (world). I would check the following:

• If the point cloud is published in the sensor frame (that moves with the robot) and NOT in the world frame.
• Check that a tf exits between the world frame and the sensor frame
• Make sure that you compiled the code with the flag -DCMAKE_BUILD_TYPE=Release as mentioned in the Readme and wiki.

Let me know if this works.

Best, Mihir

[celikemir]

Hi,

Thank you for response. I believe that we are near the solution.

I have another type of robot sim and topics below. I have changed in voxblock world frame to odom in yaml.

If I run gbplanner without slam algorithm it is still working but as you can see voxblocks overlap.

Slam algorithm launch file looks like this https://github.com/vectr-ucla/direct_lidar_odometry/blob/master/launch/dlo.launch

I red other issues and I applied below tf <node pkg="tf2_ros" type="static_transform_publisher" name="sensor_to_world_tf" args="0 0 0 0 0 0 odom veloyne" />

So can you please show me what should i exactly write for tf. I have altered odometry topic's frame_id and cant effect the resutl.

I think that I am only responsible to change odomtery topic frame_id, velodyne_points frame_id and voxblock frame so I dont want to handle already exist robots odom tf and I have moved to odom in voxblock setting but planning still search world ?

[celikemir]

I have changed voxblock to world and publish new tf and bind them all to world as below. Still overlapping issue and planner try to start different place rather than robots existing place in voxblock also there is error base_link and target_frame issue

[celikemir]

Now it takes goal without error but robot position and world dont coincide

[MihirDharmadhikari]

Hello,

The static transform you have put between the world frame and the velodyne frame is not correct. Because of this, voxblox thinks that all the point clouds are at the origin of the world frame and hence they are all overlapping. You need the transform between these two frames as given by your odometry. If the odometry node or the simulator does not publish this tf, you will have to write a node that does this.

Regarding your comment "... but robot position and world dont coincide", I am not sure I understand what you mean. Can you please elaborate?

Best, Mihir

[celikemir]

Hi @MihirDharmadhikari ,

Thank you again. default robot tf frame looks below,

then as I have read on other issue I have put between world to odom then tf looks like this,

and the end of the day still overlapping. I have removed velodyne world frame tf. When robot moves the two coordinate wide spread themselves.

[celikemir]

Hi,

I finally managed tf's and get correct voxblock. Now the map and odometry sync and moves together but gbplanner is not plan.

I have also bind planner output to my 4 whell differantial robot with below <node pkg="topic_tools" type="relay" name="cmd_vel_relay" args="/smb_velocity_controller/cmd_vel /r2d2_diff_drive_controller/cmd_vel" />

I think that I should use pure pursied based thing but I just want to see result. After clicking start planner robot starts to swing around. with 0.5 z angle and small linear x input.

After restarting gbplanner and starting again with clicking start planer sometimes geofence appears and transparent square on robot and itreation counts again but there is no visual path on screen and robot just waiting.

I am using only smb_sim.lauch

I have tried to modify some voxblocks by setting their size or height for the robot to hang back.

[celikemir]

Hi again @MihirDharmadhikari ,

I am nearly finished running gbplanner.

The robot draws the trajectory but does not follow the path. The carrot pose appears on the robot pose, but sometimes on the planned trajectory, and the robot moves slowly around the carrot pose.

I have set the output of gbplanner /smb/command/trajectory to /r2d2_diff_drive_controller/cmd_vel, but the robot is just turning around and not moving forward. I also increased the height of the robot because sometimes it can't see any path that gbplanner creates.

I am also facing holes and moving objects. Do you have any recommendations on which configurations I should modify to improve the path? And last but not least, how can I increase the speed of the UGV? It is really slow.

PS. I think this repo does not successfully work for ground robots without adjusting the parameters exactly.

[MihirDharmadhikari]

Hi @celikemir ,

It seems that the planner is not able to build the graph for exploration. The path that you see is actually the global graph. The purple spheres that you see are the vertices of the graph for exploration. Can you send me the world and the config file so I can have a better look?

Best, Mihir