How to modify the launch file to integrate into my robot system

[lyf23333]

Hi,

Thank you very much for sharing this awesome planner! I want to integrate this to my own robot system and I have already red the issue https://github.com/ntnu-arl/gbplanner_ros/issues/36 here, its very helpful. How can I modify the launch file? You mean I only need the pci and gbplanner package in the launch file, so this is my launch file, is this correct?

Thank you very much!

[MihirDharmadhikari]

Hi @lyf23333 ,

Your launch file looks good. I see that the odometry topics provided to gbplanner and PCI are different. Is this on purpose or not? I believe you are launching your simulator in a separate launch file, is that correct? In addition to this, you will need to make sure that you have the appropriate controller for your robot that can follow the paths given by the planner. You can either use the topic method where the planner will simply publish the path on the topic $(arg robot_name)/command/trajectory or the action method where the PCI has a ROS action client and your controller needs to have the action server for the same. You can find more details in the wiki. Let me know if this helps.

Best, Mihir

[lyf23333]

Hi @MihirDharmadhikari ,

thank you very much for this quick reply! I worked out the connection between my simulator and gbplanner, i.e. my simulator received the path from gbplanner, but I still have two further questions:

1. After I launched the both simulation, the rviz view for the gbplanner looks like this. : There is no exploration views, can I somehow visualize it here?

2. After I successfully executed the initialization step i.e. robot moves forward for 2m and the path tracker sends a success signal back, I clicked the start planner button, then the following error appears, do you have any idea why is that?

Appreciate your help!

Best, Yifan

[MihirDharmadhikari]

Hi @lyf23333

It seems that the voxblox map is not populating. Due to this, the adaptive bounding box calculator is not able to get the point cloud to do the PCA and hence complaining. You should check that the correct point cloud is being sent to the planner as well as there is a tf from the point cloud's frame (in the frame_id field of the message) and your fixed frame (the default is world). You can check this with the tf_tree viewer in RQT.

Let me know if this helps.

Best, Mihir

[lyf23333]

Hi @MihirDharmadhikari ,

sorry for late response. Thanks a lot for the help!

1. I found that when I change my launch file to same as the demo launch file, only modifying the topics, the error disappears, my current launch file: The view: Do you know why would this work? Just curious.

2. But there is also issue here, the planner keeps planning and sometimes my robot receives paths and my robot just keeps exploring a very small areas. just in the red square you see above. Sometimes, the robot receives no path at all and the gbplanner output looks like below. Do you possibly know what could be the cause?

3. My environment is indoor environment, do you know how can I remove the roof in voxblox?

Thank you so much for your help!

Yifan

[MihirDharmadhikari]

Hi @lyf23333

1. In the launch file you sent me earlier, you were not launching any simulator. Were you launching it separately? If not then probably that was the issue.
2. The red polygon you see is called a geofence. It is an area that the planner treats as an obstacle. Since it is blocking all the paths around the robot, the planner is not able to span the graph in any other areas. You can set any set of polygons as geofences. The one you see here was set for the particular demo world. You can disable the geofence here or change the geofence parameters here.
3. Unfortunately, you can't remove the roof directly. You have two options: a. Use the surface_pointcloud instead of the occupied_nodes in the rviz. This is what I do. b. Write a separate node that subscribes to the occupied_nodes topic (which is a visualization_msgs/MarkerArray), removes the markers above a certain z, and publishes it on another topic for visualization.

Let me know if this helps.

Best, Mihir

[lyf23333]

Hi @MihirDharmadhikari , I would like to thank you very much for all help you have offered, they are all in time and useful! I adopted 2 and 3 and the planner works now:)

But still there is one issue: my env is corridor env, should be similar to subterranean spaces, so I assume the performance should be pretty good. But it's not, like shown here, my robot explores to the end of corridor and just stucks there. Sometimes it runs back but then turn back again to that place. From the rviz, only there exists exploration gain, but you see there are other openings as well.

So do you know what could be the reason? Is there any important parameters I should adapt when using in new env?

Appreciate your help!

Best, Yifan

[MihirDharmadhikari]

Hi @lyf23333

From the second rviz screenshot I see that there is a no gain zone (the big red box) on the left side. It is like an artificial "explored" area. Hence the robot is not going there. You can disable that by commenting out this parameter (i.e. setting that list to empty). Furthermore, make sure that the global bounding box (set here) is correct and all of the area that you want to explore is within in. You can find detailed description of all these parameters in our wiki in the parameters section.

Let me know if this helps.

Best, Mihir

[lyf23333]

Hi @MihirDharmadhikari,

Thank you. I removed the ´no gain zone´ and made sure that the global bounding is ok, but the performance doesn't change. Have you set any time threshold that if the robot doesn't reach the planned local goal, it replans? I wonder about this, because the robot still gets stuck there and it never goes to the next planning iteration. And I found that at first the planner gets exploration gain at all openings, but once it enters that corner, the gains are gone, so it just stuck there.

Thank you again for all help!

Best, Yifan

[MihirDharmadhikari]

Can you send me the terminal log of the mission? Also, can you record a video of the mission (just the rviz window) and send that as well?

[lyf23333]

I have sent the video and log to your mail:)

[MihirDharmadhikari]

It seems that the corridors are too narrow for the current robot size. I would recommend try the following changes:

• Reduce the RobotParams/size to [0.4, 0.4, 0.4], RobotParams/size_extension to [0.0, 0.0, 0.0].
• Change the PlanningParams/type to kBasicExploration.
• Increasing the maximum number of edges to around 9000. This will most likely improve the performance. I also noticed that it did not plan for a long time in the beginning. It seemed like the robot was very close to the walls and hence could not plan. Can you make the robot move somewhere else during the initialization motion? You don't need to use the initialization button provided in the UI, you can move it using any other way if your simulator allows.

Let me know if this works.

Best, Mihir

[lyf23333]

Hi Mihir, Thank you again! I tried all of stuff above and it works. Changing the initialization to wider space indeed solves the problem of not planning for a long time at beginning. And I guess it is working properly, I have sent another video to your mail. Besides, I have a few further questions that I may bother you:

1. Do you think what I show in the video reaches the planner's capability? Or lets say it works as you expected.
2. I would like to know: why is the robot wandering around in the open space? What could be the reason for that from your opinion?
3. I haven't found anything like send a signal through action server and replan when robot doesn't reach the planned local path for a long time. If it exists, where is it? If not, didn't you meet these problems before in sim2real? Like the corner case I mentioned above, if robot indeed cannot reach there, it should replan, correct?
4. I am doing some study related to exploration, this planner is absolutely awesome, the first one working so properly! But to help me better understand the situation, what are the drawbacks for gbplanner2? What could possibly be improved from your point of view?

Thank you again for all the help, I really appreciate that!!

Best, Yifan

[MihirDharmadhikari]

Hey Yifan,

Glad to hear that it worked.

1. Most of the behaviors that it is doing make sense. It does not have enough motivation to go into the narrow corridors (around 2:02 in the video) because there is very little exploration gain for the vertices near the entrance of that corridor. I see that you have both narrow openings and wide-open areas. If it was just the former, I would have suggested to reduce the voxel size but with the open areas it will become computationally very expensive.
2. If you see the rviz, you will notice that the planner is not able to span a graph on the central part of the big open area which is a little elevated and has a lot of bushes. However, it should plan along the pathway around it (the pathway where the robot is at 11:48 in the video). Can you visualize the vis/sampler and the vis/planning_workspace to see if there are any samples in that area and if the global bounds are set correctly? You can also relax the maximum allowed inclination by increasing this parameter.
3. During the DARPA SubT challenge when we deployed the planner on the ANYmal robots, there was another behavior tree monitoring the entire stack of ANYmal that did this. The planner (PCI to be precise) provides an option to the path tracker to send an artificial signal saying the path execution is complete. In the pci_general this functionality is available through the feedback. In the feedbackCallback, you will see this check where the action server (the path tracker) can tell how much time is remaining. If your path tracker (or any system monitoring software you have) can check if the robot is stuck or not it can set the estimated_time_remaining in the action feedback message to 0 and the planner will trigger the next iteration. Furthermore, you can always call the stop service planner_control_interface/std_srvs/stop and then the start service planner_control_interface/std_srvs/automatic_planning to begin the planning again.
4. Some drawbacks:
   • GBPlanner2 uses a very minimalistic traversability calculation for ground robots. It was designed to be a higher-level planner that can calculate exploration paths and the robot-specific lower-level traversability can be handled separately.
   • The volumetric gain formulation is not well suited for very open environments. Another drawback of the way GBPlanner2 calculates the gain is that it does not motivate the robot to go from wider areas to smaller openings. Hence even though the planner goes for maximum exploration in each planning iteration, it does not allow for a complete exploration of small areas that were missed. This can be handled with tuning but then you reduce the overall efficiency.
   • The planner is designed to work out of the box for both aerial and ground robots (actually any robot that has a position controller and can provide point cloud and odometry :) ). So the paths are not smooth for aerial robots to track at high speed. This is more of a trade-off than a drawback that can be overcome in a relatively straightforward way. Regarding the software itself:
   • We only provide a minimalistic ground robot controller.
   • The planner does not support the integration of other traversability maps at the moment.
   • No ROS2 yet (coming soon).

Hope this helps.

Best, Mihir

[lyf23333]

Hi @MihirDharmadhikari ,

that's so helpful! Thank you:)

Can I ask maybe the last question? I saw the video of the demo, how did you make the awesome video, I mean how did you plot the tracks, occupancy, traversability during the whole process? and for the local graph you show here, how did you make the background white? You record the rosbag and made replay or? Just like the ones shown here

If I want to also have such views, which buttons should I click in rviz?

Hope this won't be too annoying and thank you!

Best, Yifan

[MihirDharmadhikari]

Hey @lyf23333

We record all the data, including the visualization topics of gbplanner, in rosbag. The videos are then created by screen-recording rviz and visualizing these topics. Some topics are created by running the planner offline, for example, the voxblox map which we don't record in the bag. The traversability map is also recreated offline. The top-down view is created by accumulating the point cloud and visualizing the robot's path. We use the jsk_rviz_plugins/TFTrajectory for visualizing the robot's path. In the follow view we use the orbit view of rviz and set the target frame to the robot's body frame in the Views panel:

Always happy to help!

Best, Mihir

[lyf23333]

Thanks!

[lyf23333]

Hi @MihirDharmadhikari ,

I have several questions bothering me recently after studying into this:

1. Why exactly is gbplanner2 not suited for open area like the public space I showed you? The design of exploration gain computation seems pretty good, so I don't understand what is the real reason for it not suitable for open areas. Maybe too many openings at open sapce?
2. You mentioned there is only a minimalistic traversibility estimation. Did you use this traversibility estimation in DARPA? Or you mean you just provide a simple version here and I can replace it anytime with more complete version?
3. Remember that in the previous video I showed you, at first when the robot starts near the wall, it took a long time until it plan a path, but you suggest me to move it to a bit more open space, then it starts planning right away. Why is that? why would the robot not planning for many iterations at beginning phase if starting near the wall?
4. You mean the top-down view is created by accumulating the point cloud, how do you exactly accumulate the point clouds may I ask?(or if you have it somewhere in code, i can just go to see that) I am not so experienced in ros:(
5. In the paper, you have a plot showing the speed of exploration, how do you calculate the volume of explored space at each time? Is this included in the code as well?

Sorry for inconvenience if these are too many questions:), but I really feel interested and curious about the work and these details. Thank you very much for always answering my questions in patience:)

Best regards, Yifan

[MihirDharmadhikari]

Hey Yifan,

Sorry for the late reply. I was out of the office.

1. When I say not well suited I don't mean that it won't work, I mean that it won't be as good as in say in a tunnel. The reason for this is that the robot might go back and forth in kind of a zigzag manner to map the whole area.
2. This is the same traversability estimation used by gbplanner in the DARPA subt challenge. However, there was an additional navigation planner used for tracking the path given by gbplanner. You can check the details in the SubT Final Event Paper.
3. This was simply because the robot was too close to the wall and its initial location was in collision. It was not taking a long time to plan but it was trying to plan for multiple iterations and failing. Due to the noise in the lidar and due to the fact that the planner tries to free the voxels around the current robot state, an instance came where the starting position of the robot was no longer occupied and hence it planned.
4. If the rosbag records the tf topic and the tf includes the transformation from the sensor frame (the header/frame_id of the point cloud message) to the world frame then simply setting the decay_time in rviz to a high value works. That is what I did in the video.
5. The explored volume is calculated by counting the number of voxels in the map and multiplying it by the voxel size. The simplest way to do this with voxblox is to subscribe to the gbplanner_node/tsdf_pointcloud topic (which is of type sensor_msgs/PointCloud2) and count the number of points.

Hope this helps.

Best, Mihir

[lyf23333]

@MihirDharmadhikari ,

Thank you very much! I got it!

Best, Yifan

[MihirDharmadhikari]

Great! Happy to help!