Add Custom Ground Model and d435i Realsense Camera

[furkancetiny]

Hello, I want to integrate my own robot into the system. And I also have realsense2_camera. I also want to get my point cloud data accordingly. For this I updated the /input_pointcloud locations for my camera. I also gave my model in xacro format. But I couldn't get the output in RVIZ environment. Can you give information about these? Can I run d435i realsense_camera on the system? And do I need to do anything external for my model other than giving the path to my model file in the add model part of your project?

[MihirDharmadhikari]

Hi @furkancetiny ,

I will answer your second question first: you can use a D435i in the planner. You will have to specify the correct range and field of view for the volumetric gain calculation here. You will need to disable the yaw smoothing by setting this parameter to false.

Regarding your question about the custom robot model:
In our demo, we use our own ground robot simulator, the smb_simulator.
The changes you need to make will depend on whether you want to replace the model of our robot with yours in our simulator (i.e. use our controllers etc.) or if you have a robot model with its own controller (either position or velocity).

1. If it is the latter, you simply need to add the gbplanner and pci_general node launching snippets (these) into your launch file and remap the input point cloud and odometry topics as well as the output trajector/action (see the wiki for more details on the topic and action names).
2. If you want to use your robot model in our simulator, essentially replacing our robot mesh and sensors with yours, you will need to do that in the spawn_smb.launch in the smb_simulator. However, this will require some modifications in the lower-level controller configurations etc.

Let me know which option you want to go with so that we can discuss that in detail.

Best,
Mihir

[furkancetiny]

I have my own robot in real environment. There is also the xacro file of this robot. I want to get the data in RVIZ environment. I tried to do what you said in point 1. However, parameters related to my robot did not appear in the Global Options>fixed Frame section. When I start it, the "world" parameter appears. But it's not in the options. Due to this error, my robot does not appear in the RVIZ environment. Strangely, only camera-related parameters are available.

I updated the parameters as follows:

   <arg name="odometry_topic" default="base_controller/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="pointcloud" to="camera2/depth/color/points" />
     <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>

[MihirDharmadhikari]

I previously thought that you wanted to use your robot model in simulation, but I guess you want to run the planner on a real robot correct? If you want to visualize the robot model in rviz, you will need to publish it on the robot_description topic similar to what is done here.

[furkancetiny]

Sorry for the late reply. I am constantly trying to find a solution myself. Yes, I want to implement it on a robot. The outputs of this I want to get it from RVIZ environment to get it too. I placed my own model in the part you said. But it didn't. When I looked through the rqt graph, I could only see the camera data. Unfortunately, I can't see the right-left wheel base link and other values. And why did the tf's come out scattered over the graph?. But my model is like this

[MihirDharmadhikari]

Sorry for the late response, I was out of the office.

It seems that the robot model is disconnected from the rest of the tf tree. I see that the timestamps of the two images are different. Where is the first one from?

[furkancetiny]

The first image is the resulting tf tree after uploading my model to the gbplanner_ros package. To upload my own model, I gave the path to my own model here. The second is the output my robot model should have

[Sukhvansh2004]

@MihirDharmadhikari Hi I also want to use gb planner on husky simulation so i have followed your step2. While visualizing through rqt i think its ok but how do i go about visualising it in rviz also when i tried to launch the gb ui launch file and initialize or start the planner I faced the following errors

Error for GBPlanner START

[ERROR] [1704135442.271976378, 133.139000000]: Exception thrown while processing service call: bool pcl::PCA<PointT>::initCompute() [with PointT = pcl::PointXYZI] in /usr/include/pcl-1.10/pcl/common/impl/pca.hpp @ 58 : [pcl::PCA::initCompute] number of points < 3
           [ERROR] [1704135442.272263791, 133.140000000]: Service call failed: service [/gbplanner] responded with an error: bool pcl::PCA<PointT>::initCompute() [with PointT = pcl::PointXYZI] in /usr/include/pcl-1.10/pcl/common/impl/pca.hpp @ 58 : [pcl::PCA::initCompute] number of points < 3
     [ERROR] [1704135442.272389785, 133.140000000]: Planner service failed

Error for GBPlanner Initialise

 [ERROR] [1704135545.687458591, 234.635000000]: It was not possible to send path to action server.

[MihirDharmadhikari]

Hello @Sukhvansh2004 ,

It seems that the error is due to the adaptive bounding box calculator not being able to find enough points to do PCA to build the bounding box.
First, check if the voxblox map is built properly. If it is built then you should be able to see it in rviz. If the map is not built check if you are passing the correct point cloud topic to the planner and there is a tf from the world frame to the sensor frame.
Second, if you are using a depth camera like the realsense d435i, I would recommend disabling the adaptive bounding box calculation by setting this parameter to kBasicExploration.

Regarding visualization of the robot, make sure that you have published your URDF/SDF as a robot description. You can check the spawn_smb.launch file for an example of how we do it.

Let me know if this works.

Best,
Mihir

[Sukhvansh2004]

Thanks for the early reply!
I did the changes that you have told. Do you also have the config file for d435i that you are talking about here?

IN REFERENCE TO YOUR COMMENT FROM ABOVE
"I will answer your second question first: you can use a D435i in the planner. You will have to specify the correct range and field of view for the volumetric gain calculation here. You will need to disable the yaw smoothing by setting this parameter to false."

[MihirDharmadhikari]

We have not created an example config file for D435i or any depth camera. But the only parameters that you will need to change to get it to work are the ones listed in that comment. So it should be pretty straightforward to create one.

[Sukhvansh2004]

Thank you for the clarifications. Can you please help with some more issues which we are facing. To solve our issues first of all we had set all our verbosities to DEBUG. Also we have changed our world frame to our odom frame in voxblox_sim_config file and world_frame_id to odom frame in planner_control_interface_sim_config. Now initially form your comment

"If it is the latter, you simply need to add the gbplanner and pci_general node launching snippets (these) into your launch file and remap the input point cloud and odometry topics as well as the output trajector/action (see the wiki for more details on the topic and action names)."

we have remapped the input point cloud and odometry topic to our robot one's but where do we have to map the output/trajector/action and from where, we could'nt understand that.

also we are getting the following error from params.cpp file
[ERROR][File: /home/sukhvansh/gbplanner2_ws/src/exploration/gbplanner_ros/planner_common/src/params.cpp] [Line: 444]
Param is not set: /gbplanner_node/FreeFrustumParams/sensor_list

we have also changed output type from kAction to kAuto cause we were getting this error
[ERROR] [1704406992.117032688, 1344.600000000]: It was not possible to send path to action server.

Finally we wanted to use our robot for autonomous navigation in unexplored terrain to a specific waypoint but many time we got messages that could not execute path or empty paths were going in. Also in Rviz some topics had the following warning/error
Uninitialized quaternion, assuming identity.
Points should not be empty for specified marker type.

This was showed as a warning for /vis/negative_edges topic.

also we had putting PlanningParams/geofence_checking_enable as false, as we though that geofencing was maybe causing issues in planning

I am also pasting our rqt graph for your reference.

I am attatching some snippets of images while we were running the planner and tried to gave it waypoints. This includes images of both cases where planner was on and when it was off and waypoints were given as 2D Nav Goal after which Plan to Waypoint was initiated from gbplanner_ui

I am also attatching a picture of all the packages we have installed to as if any supplementary package is missing you could guide us

We would be grateful if you could help us
Best Regards

[Sukhvansh2004]

@MihirDharmadhikari It would greatful if you help us

[MihirDharmadhikari]

Hello @Sukhvansh2004 ,

Sorry for the delay.

1. The functionality to plan to a waypoint is only to plan to a waypoint in explored space where the global graph (refer to the paper) is spanned. This graph is built during the exploration process. Hence, you first need to start the exploration and let the graph span, before you can use this function. The global graph can be visualized in rviz under the PlannerViz group.
2. There is no mode kAuto. It is either kAction or kTopic. When you use kTopic, the planner will publish the path to the topic command/trajectory. You path follower needs to subscribe to it. Similarly, if you use 'kAction the planner will act as an action client and your path follower will be the action server. The planner will send the path on pci_output_path.
3. Have you confirmed that the voxblox map is being built? The voxblox map should be visualized in rviz.

Let me know if this works.

Best,
Mihir

[Sukhvansh2004]

Hello, We are able to visualise the voxblox mesh and occupied nodes, so considering the voxblox mesh is same as of the map, it was was being built and sorry for the miscommunication regarding the kAction and kTopic thing. Actually we had set it to kTopic only (however a typo of kAuto happened). The voxblox config file is as follows

I am also attaching all the topic which we were able to get in rviz incase if it helps.

Also when we initialised the planner after starting the planner it was still not giving us velocities and acceleration in the command/trajectory topic needed to drive the robot.

We had tried to explore the map manually also and after it had generated global graph it wasnt still giving us the velocities and acceleration needed to drive the robot in the command/trajectory topic. However, it was giving us some linear and angular transforms.

[MihirDharmadhikari]

Hi @Sukhvansh2004 ,

Your files seem to be correct. You have attached the config file for the planner_control_interface instead of the voblox config file by mistake.
GBPlanner is a path planner and hence the output path only has timestamped positions. We use the MultiDofTrajectory message to keep a generic datastructure. The time stamps are calculated by assuming constant speed throughout the path. If you need velocities then you will have to calculate them from the position waypoints and timestamps. Sorry for the misunderstanding about this.

Best,
Mihir