Navigation with stereo camera only

[minato_99]

Hello matlabbe,

I am very thankful for your help so far. I am working on navigation using rover. I have attached the launch code, database, and the navigation stack parameters. It is weird I am not getting any error when I launch the file. However, the global map, local map, and the trajectory planner are not showing up.

I used the tutorial available in rtabmap_ros/Tutorials/StereoOutdoorNavigation. ( https://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation


Any help would be appreciated.

Launch file:
https://drive.google.com/file/d/1hLtLhvBLlAjngxvpCYUxWDI_lIg5M1fG/view?usp=drive_link

Database:



rover_costmap_common_params:

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.03
#robot_radius: ir_of_robot
inflation_radius: 0.55
transform_tolerance: 1

controller_patience: 2.0

NavfnROS:
    allow_unknown: true

recovery_behaviors: [
    {name: conservative_clear, type: clear_costmap_recovery/ClearCostmapRecovery},
    {name: aggressive_clear, type: clear_costmap_recovery/ClearCostmapRecovery}
]

conservative_clear:
    reset_distance: 3.00
aggressive_clear:
    reset_distance: 1.84

rover_global_costmap_params.yaml:

global_costmap:
  global_frame: robot/map
  robot_base_frame: robot/chassis
  update_frequency: 0.5
  publish_frequency: 0.5
  static_map: true

rover_local_costmap_params.yaml:

local_costmap:
  global_frame: robot/odom
  robot_base_frame: robot/chassis
  update_frequency: 2.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.025
  origin_x: -2.0
  origin_y: -2.0

  observation_sources: point_cloud_sensor

  # assuming receiving a cloud from rtabmap_util/obstacles_detection node
  point_cloud_sensor: {
    sensor_frame: robot/chassis,
    data_type: PointCloud2,
    topic: openni_points,
    expected_update_rate: 0.5,
    marking: true,
    clearing: true,
    min_obstacle_height: -99999.0,
    max_obstacle_height: 99999.0}

rover_base_local_planner_params.yaml:


TrajectoryPlannerROS:

  # Current limits based on AZ3 standalone configuration.
  acc_lim_x:  0.75
  acc_lim_y:  0.75
  acc_lim_theta: 4.00
  max_vel_x:  0.500
  min_vel_x:  0.212
  max_rotational_vel: 0.550
  min_in_place_rotational_vel: 0.15
  escape_vel: -0.10
  holonomic_robot: false

  xy_goal_tolerance:  0.20
  yaw_goal_tolerance: 0.20

  sim_time: 1.7
  sim_granularity: 0.025
  vx_samples: 3
  vtheta_samples: 3
  vtheta_samples: 20

  goal_distance_bias: 0.8
  path_distance_bias: 0.6
  occdist_scale: 0.01
  heading_lookahead: 0.325
  dwa: true

  oscillation_reset_dist: 0.05
  meter_scoring: true

Sincerely,
minato_99

[minato_99]

Here is the database:
https://drive.google.com/file/d/1baqVPJ4VTdovohtuztJKYWG9uAOcznRL/view?usp=sharing

[matlabbe]

If the global costmap is not published, check if move_base is correctly subscribed to right map topic from rtabmap node.

If local costmap is not published, check for errors/warnings of move_base node.

If sending a goal doesn't work, check if the goal topic is correctly connected to move_base. If so, check if there are errors/warnings when sensing a goal.

[minato_99]

Hello matlabbe,

I am getting global costmap and  lost costmap. I am getting the following error.

The /planner_cloud observation buffer has not been updated for 293.43 seconds, and it should be updated every 0.50 seconds

When I use 2D nav goal in the rviz. I get the following error.
[/move_base]:Sensor data is out of date, we're not going to allow commanding of the base for safety

rover_rtab_stereo.launch

r_local_costmap_params.yaml
r_global_costmap_params.yaml
r_dwa_local_planner_params.yaml
r_costmap_common_params.yaml
r_base_local_planner_params.yaml

These are the required files.
Looking forward to your reply.
Sincerely,
minato_99

[matlabbe]

Is /planner_cloud published?

rostopic hz /planner_cloud

Based on your config files, you may have to change

<remap from="openni_points" to="/robot/stereo/points2"/>

for

<remap from="/planner_cloud" to="/robot/stereo/points2"/>

[minato_99]

Hello,

Yeah I got rid of that error. I had to replace the the topic name in the yaml file of common costmap with /robot/stereo/points2.
The next problem I am facing is I am getting the global trajectory but not local trajectory. When I used to give the goal point using " rostopic pub /move_base_simple_goal geometry_msgs/PoseStamped ", There is no motion. It seems local plan and global plan does not have any output.

Launch File:
rover_rtab_stereo.launch

Params:
r_local_costmap_params.yamlr_global_costmap_params.yaml
r_costmap_common_params.yaml
r_base_local_planner_params.yaml

database:
https://drive.google.com/file/d/1baqVPJ4VTdovohtuztJKYWG9uAOcznRL/view?usp=drive_link

I appreciate your help.

Sincerely,
minato_99

[matlabbe]

If you get a global trajectory, then move_base is using it.

You seem having similar issue than https://answers.ros.org/question/397088/configuring-dwa-navigation-correctly/

Make sure the DWA planner is correctly loaded in move_base. If you do rqt_reconfigure, under move_base, you should see all parameters for DWA.

As move_base seems to get the goal and plan a global path, there could be some warnings/errors for the local planner if there is nothing published on /cmd_vel published by move_base. Echo that topic to see if the local planner is doing something:

rostopic echo /cmd_vel

[minato_99]

Hello matlabbe,

I fixed that issue by replacing the cmd_vel topic with /robot/cmd_vel by remapping. It worked. However, I am still could not move the robot. I am getting the following error:

1) The origin for the sensor at (-0.17, -0.41) is out of map bounds. So, the costmap cannot raytrace for it.
2) Global map is quarter of the original map. I have attached the picture of both local and global.
3) The map starts with the robot/chassis frame so that the rover looks submerged in the point cloud.

global_and_local_maps.pngrobot_in_space.png

I have tried to fix them with the solutions available. I am not having much success. Thanks.



Sincerely,
Rakesh Yadav

[minato_99]

Also a general question, I brute force the global map by feeding the height and width along with the position. However, the rover does navigate smoothly on the flat surface avoiding the obstacles. However, it does not avoid the crater present in the environment even though it shows as an obstacle in the map. Any suggestions if the move_base is appropriate for avoiding the craters. Thanks.

[matlabbe]

I think in local costmap parameters the origin should have an offset:

  origin_x:  -2
  origin_y:  -2

In global costmap parameters, "rtabmap_ros::StaticLayer" should be changed to "rtabmap_costmap_plugins::StaticLayer" after packages have been renamed.

As long as the global costmap is empty or the local costmap is empty, the robot will plan direct path through obstacles. Try fixes above to see if the costmap can be correctly generated.

[minato_99]

Hello matlabbe,
The global trajectory is being created avoiding the obstacles. However, the map being created has noise representing obstacles and some empty space. I have attached the stereo camera used for gazebo simulation and the map with noise.

Can you tell me which parameters should I choose to get a better point clouds with less noise?

Stereo Camera for gazebo simulations:

<gazebo reference="camera">
<sensor type="multicamera" name="left_stereo_camera">   
<update_rate>10</update_rate> 
<camera name="left">

<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>                       
<height>800</height>                       
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>20</far>                       
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<camera name="right">                       
<pose>0 -0.07 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>20</far> 
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>10.0</updateRate>   
<cameraName>stereo</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera</frameName>

<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>


Map:




LAUNCH FILE:
rtab_stereo.launch

Thanks,
minato_99

[matlabbe]

Hi,

Spurious pixels like that could be filtered with GridGlobal/Eroded=true. Other option would be to look at the local maps created around there causing the obstacle in rtabmap-databaseViewer (3D view). It could be bad disparity estimation. You can also play with Grid/MaxGroundHeight to filter them if they are very close to other points labelled as ground. If you want to tune Grid/ parameters in rtabmap-databaseViewer (Core Parameters -> Grid panel), the most convenient way to avoid reprocessing the map everything time is to use the regenerate local grid menu action:


cheers,
Mathieu