search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

Localization error when using kinetic camera for mapping and localization #302

Open Manoj-sanagapalli opened 5 years ago

Manoj-sanagapalli commented 5 years ago

Hello People, I am trying autonomous navigation for a real robot using Kinect camera. I am using rtabmap for mapping and localization. move_base is used for navigation to specified goal. map_server is used to convert my map in to "/mapData" format acceptable by move_base package. steps followed: 1.launch robot state publisher,camera node, joystick node (for robot). - initialized my robot transforms,camera point clouds. 2.launch rtabmap.launch in localization mode with rviz:=true (for visualizatoin). (i have previously mapped data as .db file) 3.launch map_server which publishes my map in 2d format (grid map) 4.launch move_base for path planning.

  1. I set goal in rviz.

move_base package is not getting proper localization position. I can see error in the estimated localization pose in rviz. Due to the error my move_base package is unable to navigate the bot. can anyone help me to solve my localization error.

Thank you.

  1. Error in move_base package 1_1

2.starting recovery behaviour and my bot rotates 360 degrees to localiza itself. but finally stuck with error. 2

  1. Error in localization. error_localization
matlabbe commented 5 years ago

From the last screenshot, it is like there is no occupancy grid map, which is maybe why move_base fails. Note that you don't have to use map_server if you use rtabmap, rtabmap already publishes the map under /rtabmap/grid_map topic. Just remap it to move_base map input topic.

Manoj-sanagapalli commented 5 years ago

Thanks for your comment admin, I have remaped and it and move_base is taking the map from rtabmap. I got one basic problem now while launching rtabmap.launch in localizaition mode. Steps followed:

  1. I launched mapping using command roslaunch rtabmap_ros rtabmap.launch database_path:="/home/bomma/catkin_ws/Maps/rtabmap.db" rtabmap_args:="--delete_db_on_start" rgb_topic:=/kinect2/qhd/image_color_rect depth_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info approx_sync:=false rviz:=true and mapped and got a map successfully.
  2. For localization I have launched using command roslaunch rtabmap_ros rtabmap.launch localization:=true database_path:="/home/bomma/catkin_ws/Maps/rtabmap.db" rgb_topic:=/kinect2/qhd/image_color_rect depth_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info approx_sync:=false rviz:=true and even I have changed value of arg "localization" to true in launch file also.

1

Result: rtabmapviz is still getting launched in mapping mode (refer screenshot) and I had to click localization in GUI. problem: everytime i try to launch in localization mode, some part of map is getting added till I switch to localization mode in GUI.

Could you please suggest me where I am going wrong!!

matlabbe commented 5 years ago

I cannot reproduce the problem. Here is what I tested (with an openni2 camera):

$ roslaunch openni2_launch openni2.launch depth_registration:=true

  1. Mapping

    $ roslaunch rtabmap_ros rtabmap.launch args:="-d"

  2. Localization

    $ roslaunch rtabmap_ros rtabmap.launch  localization:=true

The only thing I see is that on your computer rtabmapviz is launched before rtabmap node, so rtabmapviz doesn't know that rtabmap is in localization mode, then the UI state is confused and show default value. Note that rtabmap should still be working in localization mode even if the UI says mapping. The terminal launching rtabmap should show the following on initialization when localization is true:

[ INFO] [1555536725.384109643]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="false"
[ INFO] [1555536725.384854484]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="true"

Doing Window->Preferences in rtabmapviz would make rtabmapviz refresh the parameters on its side and update the UI. Look at the RTAB-Map Settings panel in Preferences, SLAM mode should be unchecked when in localization mode. Screenshot_2019-04-17_17-48-05

cheers, Mathieu

Manoj-sanagapalli commented 5 years ago

Thanks for your comment Mathieu, As you said just UI didn't get updated, rtabmap is launched in localization mode only.

Manoj-sanagapalli commented 5 years ago

I am able to localize it properly and navigate it autonomously. I am having one problem now. I want to check if the packages are working for dynamic obstacles and see how the path changes. Steps followed are:

  1. I have mapped and localized the robot using the above packages. Now the robot is localized. 2.I have launched move_base package and given a goal in rviz. 3.I am able to find global path and local path in rviz GUI. 4.Now I place a obstacle in the path.
  2. The robot is not sensing the obstacle and it is getting crashed in to the obstacle.

Can you please help me what I am missing in testing the dynamic obsacle.

matlabbe commented 5 years ago

Look at your local costmap, is the obstacle detected in it? If not, check the configuration of its obstacle layer to make sure it is subscribed to scan/point cloud.

Manoj-sanagapalli commented 5 years ago

Dear Admin, I have modified the costmap.yaml. But I am unable to get the costmap with kinect camera in rviz. can you please check if my yaml file is correct. I have attached tf of kinect v2 camera. Thanks

obstacle_range: 2.5 raytrace_range: 3.0 footprint: [[x0, y0], [x1, y1], ... [xn, yn]]

robot_radius: ir_of_robot

inflation_radius: 0.55

observation_sources: kinect1 kinect2

kinect1: {sensor_frame: kinect2_rgb_optical_frame, data_type: PointCloud, topic: /kinect2/qhd/image_color_rect, marking: true, clearing: true}

kinect2: {sensor_frame: kinect2_ir_optical_frame, data_type: PointCloud, topic: /kinect2/qhd/image_depth_rect, marking: true, clearing: true} frames.pdf

matlabbe commented 5 years ago

Your observation sources are PointCloud type, but you are passing images. You should send kinect2 cloud topic.

Manoj-sanagapalli commented 5 years ago

There are two types of poincloud data in kinect. one is rgb point cloud and other is depth point cloud. can you please tell which pointcloud topic to include?

matlabbe commented 5 years ago

One or the other should work, though depth point cloud would be a little more efficient in term of memory (no RGB channel).

Manoj-sanagapalli commented 5 years ago

Dear matlabbe, I have modified as you suggested. Topics can be found in this link https://github.com/code-iai/iai_kinect2/tree/master/kinect2_bridge

observation_sources: kinect1

kinect1: {sensor_frame: kinect2_rgb_optical_frame, data_type: PointCloud, topic: /kinect2/sd/points, marking: true, clearing: true}

Unfortunately even this modification is not working and I am unable to see the local cost map of obstacles (unable to detect dynamic obstacles). Is there any other places that need modification other than yaml file? Thanks.

matlabbe commented 5 years ago

Is this defined in the obstacle layer? Is the obstacle layer added to local costmap? http://wiki.ros.org/navigation/Tutorials/RobotSetup#Creating_a_Launch_File_for_the_Navigation_Stack

You may try a working example (like turtlebot navigation tutorials) to see how the obstacle layer is configured. http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot#Simulation_.28Gazebo.29