Indoor navigation with stereo

[pravin382]

Dear Mathieu,

I am trying to perform navigation using stereo+2D laser configuration. I have achieved this https://youtu.be/k8G9rrGOpi8 so far. It seems quite strange to me that the local planner is not able to track its path and this sometimes leads to hitting the obstacle. I am suspecting the problem does not lie with local planner but with localization. Can you provide me a hint to solve this problem? If it is localization problem, how should I fix it?

 costmap_common_param:
https://drive.google.com/file/d/15TnhTqOWfj5lMGOx9WveJDTKs7XrwI5V/view?usp=sharing

global_costmap_param:
https://drive.google.com/file/d/1FUcLfFMZpG9ozlBRo0C-eR4bvm-K3MUc/view?usp=sharing

local_costmap_param:
https://drive.google.com/file/d/1TJLCvCbc-bTQdFrd6_0B3h-kWaKS3sNi/view?usp=sharing

dwa_local_planner_param:
https://drive.google.com/file/d/1qymdhJoVa-j3IezbLT4aM4Sy0vEKyJfE/view?usp=sharing

launch_file:
https://drive.google.com/file/d/1tk0gxV-oxu9U4YxjeD5hmZuueJobhp_k/view?usp=sharing

If you need any other information, I can provide it.

[matlabbe]

Hi,

I don't see something wrong with a fast survey of your files. The local planner should be independent of localization. Localization would make move_base update the global path, so the current goal of the local planner could change will moving. This is something expected. You could make your robot following less accurately the global path, but more the current goal. I don't remember the DWA parameter names to adjust the weight of the local planner (goal vs path).

For hitting obstacles, do you see the obstacles in the local costmap? If the robot can see the obstacles and hit them, look at the acceleration values, the control update rate, the footprint of the robot...

cheers,
Mathieu

[pravin382]

Hi, thanks for the quick reply.

I have tried to tune the trajectory scoring parameters but it does not much help me. I recently checked the rate of cmd_vel from move_base, it is very low around 1-2 Hz but sometimes there is no message at all. I also see that the  cpu usage is around 80% when I am using rtabmap for localization, that is >50% increase in cpu usage. Can we reduce it without much deteriorating the performance on localization. My launch files was attached before if you need to have a look on them.

[matlabbe]

If cmd_vel rate is low, it means the control loop of move_base may be too slow to react in time. Can you show the cpu usage for all processes (with "$ top")?

[pravin382]

Sure. Here is the cpu usage from all processes using top. I increased the decimation from 2 to 4. It didnot reduced much of cpu usage.


 

matlabbe wrote

If cmd_vel rate is low, it means the control loop of move_base may be too slow to react in time.

How should we fix this? I get control loop missed its desired rate warning now. I also tried with amcl  for localization and map_server for publishing map, there the results are better. No missed control loop warning and the robot reaches the goal properly. I am only using laser as observation source in this configuration though.

In case you need them:
Image resolution is 1024 X 768, published at 10 hz using stereo throttle nodelet (from your azimut3_stereo_file.yaml).  Also, the size of my database is 583Mb.

[pravin382]

stereo_image_proc part:

 <!-- Run the ROS package stereo_image_proc for image rectification-->
    <group ns="/stereo_camera" >
	 <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
	    <!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
			 Throttle camera images -->
	 <node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle stereo_nodelet">
	    <remap from="left/image"       to="left/image_raw"/>
	    <remap from="right/image"      to="right/image_raw"/>
	    <remap from="left/camera_info" to="left/camera_info"/>
	    <remap from="right/camera_info" to="right/camera_info"/>
		    
            <param name="queue_size" type="int" value="15"/>
	    <param name="rate" type="double" value="10"/>
	 </node>
	
	 <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
	    <remap from="left/image_raw"    to="left/image_raw_throttle"/>
	    <remap from="left/camera_info"  to="left/camera_info_throttle"/>
            <remap from="right/image_raw"   to="right/image_raw_throttle"/>
	    <remap from="right/camera_info" to="right/camera_info_throttle"/>
	    <param name="disparity_range" value="128"/>
	 </node>		
  </group>

[matlabbe]

Hi,

Well stereo_image_proc is taking 220% CPU usage, so everything else is slower. When using amcl and map_server, stereo_image_proc would not run, thus everything else running on the robot would work as expected.

Image rectification done on every image is computationally expensive, as rtabmap doesn't need 10 Hz frame rate, only 1 Hz. You could reduce the frame rate of stereo_throttle to 1 Hz, rectifying only one image per second. With images received at 1 Hz, you can set Rtabmap/DetectionRate to 0 or over 1 to not miss one, the map will still be update at 1 Hz like the default.

Jsut saw that the costmap is expecting 2 Hz disparity images. You could set stereo_throttle at 2 Hz and keep Rtabmap/DetectionRate at 1 hz.

cheers,
Mathieu

[pravin382]

Hi,

I could not completely check the result today. But I noticed that reducing the frame rate significantly reduced the CPU usage. However, the problem of lower control loop rate of move_base still exists. Maybe something else is causing the problem but I need to check one more time properly if  I changed some parameters. Do you have any idea, what can go wrong in my case?

Just out of curiosity, I mapped the environment using fused odometry from VO, wheel and Imu but I am now trying to perform navigation only with wheel_odom. It would not matter, isn't it?

Why do we use rtabmap::StaticLayer and not costmap_2d::StaticLayer?

[matlabbe]

Hi,

When adding more nodes using processing power, it is likely that control loop may suffer, if the system doesn't have many CPU cores.

You could use wheel odometry directly, you may save a 100% CPU usage of one core without using visual odometry.

The reason why I use rtabmap::StaticLayer is described in this issue: https://github.com/ros-planning/navigation/issues/320. Note that if you don't use planning from rtabmap, this may not be a problem to use the standard one.

cheers,
Mathieu

[pravin382]

Hi,
 
here is the current usage from top, also showing the cores of my CPU.

matlabbe wrote

When adding more nodes using processing power, it is likely that control loop may suffer, if the system doesn't have many CPU cores.

Do you mean ros nodes or nodes in graph of rtabmap. If you meant ros nodes, then here is my current rosnode list.


If you mean graph nodes in rtabmap, I have also tried to map a small environment and perform navigation. The results did not change much.

I am using wheel odometry to further save the CPU usage. But the problem still persists i.e. the motion command is not generated on time, so the robot hits the obstacle until the next motion command comes from move_base. The local path is also generated too late. What I do not understand is, why it is slow with rtabmap?

Here is the rqt_graph for navigation with rtabmap, only using laser as observation source for navigation.
https://drive.google.com/file/d/19EN9e319J3xElrrA7lNiStzsXfA1V7hD/view?usp=sharing

Here is the rqt_graph for navigation with amcl, only laser as observation source for navigation.
https://drive.google.com/file/d/16PxoHRVQymbvRYjidGFcXHe3MTVNuiB5/view?usp=sharing

I have also checked transform. The main difference that i see is in map-to-odom :
for rtabmap:
Average rate: 20.425; most recent transform: xxxxx(-0.056 sec old), Buffer length: 2.350sec

for amcl:
Average rate: 4.396; most recent transform: xxxxx(-0.301 sec old), Buffer length: 4.777sec

Other data seems to fairly same.

In case of navigation with rtabmap, there is timed out waiting for transform warning two times at the beginning. I understand that this is because the map is not loaded yet and localization is not started.
Other than that there is no warning at all, just info from rtabmap and got new plan continuously.

[pravin382]

Hi,

can you send me the rtabmapviz config file and the launch file that you are using for stereo configuration?

Or is it the same as az3_mapping_robot_stereo_nav.launch  and azimut3_stereo_nav.rviz in github?

 

[matlabbe]

Hi,
In that example, rtabmapviz was not used, only rviz to send goals for convenience.

With the rtabmap launch file, if you disable the camera and use only sick, do you still have the same problem with move_base control speed? To use only lidar with rtabmap node, set:

<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan" type="bool" value="true"/>

<param name="Grid/FromDepth"            type="string" value="false"/>

cheers,
Mathieu