Using Kinect2 and LiDAR to Map and Navigate
Posted by jingyu on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Using-Kinect2-and-LiDAR-to-Map-and-Navigate-tp6599.html
Hi Matlabbe, I am new to ROS and I am trying to run mapping and navigating with RTAB-Map. My sensors are Kinect V2 and RPLIDAR A1, and I have a robot with internal odometry. I am now having some troubles integrate them with RTAB-map to realize better mapping and navigation 1. What I have done: 1.1 Using the RTAB-Map to run mapping with Kinect+Odom / Kinect+Laser+Odom. 1.2 Using the gmapping to map with my LiDAR. 1.3 Navigate using navigation stack within the map constructed by gampping (LiDAR only).
My Problem: The LiDAR is placed at a high position, so it cannot detect low obstacles (like this figure).
2. I tried many approaches to solve this problem:
2.1 Using the projected 2D map and re-run the navigation with LiDAR only (map server+AMCL). Although the projected map can detect the obstacles, during navigation stage, the robot will hit on it (I guess it is because LiDAR cannot find the obstacles and remove it during navigation).
2.2 I carefully look through the Turtlebot tutorial, but found the costmap only uses the laser, so I guess the same problem may still exists.
2.3 I followed the demo of az3 and modify it to try to run it on my own robot but failed (cannot send goal, seems no obstacle cloud and ground cloud). It seems there is something wrong with the obstacle detection node, and communication between the RTAB-Map and move_base. I will post some codes. I am really new so I guess there may be many mistakes.
3. My question:
3.1 What do you think is the best implementation to solve my question?
3.2 If it is possible to use the RTAB-Map obstacle detection node or just the pointcloud from Kinect to integrate into navigation stack to achieve avoiding low obstacles? If so, may I have your help on giving some detailed instructions?
3.3 If there is some thing I misunderstood with your tutorial on turtlebot mapping and navigation. Can it solve my problem? And how does RTAB-Map localize the robot during navigation (by using the VSLAM sensor of LiDAR?)?
Sorry for so many questions. I am new to ROS and want to develop a robot that can do SLAM and collect a database of Wi-Fi signal. This is my final year project and if you are interested in it we could together do some development (time is tight so I am desperate for your help).
Thanks again!
2.3
My launch file:
My costmap_common.yaml
footprint: [[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]
inflation_layer:
inflation_radius: 0.55 # 2xfootprint, it helps to keep the global planned path farther from obstacles
transform_tolerance: 0.13
cost_scaling_factor: 3.0
obstacle_layer:
obstacle_range: 5.0
raytrace_range: 5.5
max_obstacle_height: 0.4
track_unknown_space: true
observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
laser_scan_sensor: {
data_type: LaserScan,
topic: scan,
expected_update_rate: 0.1,
marking: true,
clearing: true
}
point_cloud_sensorA: {
sensor_frame: base_footprint,
data_type: PointCloud2,
topic: obstacles_cloud,
expected_update_rate: 0.5,
marking: true,
clearing: true,
min_obstacle_height: 0.04
}
point_cloud_sensorB: {
sensor_frame: base_footprint,
data_type: PointCloud2,
topic: ground_cloud,
expected_update_rate: 0.5,
marking: false,
clearing: true,
min_obstacle_height: -1.0 # make sure the ground is not filtered
}
My global_costmap.yaml
global_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 3.0
publish_frequency: 3.0
transform_tolerance: 0.5
static_map: true
plugins:
- {name: static_layer, type: "rtabmap_ros::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
My local_costmap.yaml
local_costmap:
global_frame: odom
robot_base_frame: base_footprint
update_frequency: 3.0
publish_frequency: 3.0
transform_tolerance: 0.5
static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.05
URL: http://official-rtab-map-forum.206.s1.nabble.com/Using-Kinect2-and-LiDAR-to-Map-and-Navigate-tp6599.html
Hi Matlabbe, I am new to ROS and I am trying to run mapping and navigating with RTAB-Map. My sensors are Kinect V2 and RPLIDAR A1, and I have a robot with internal odometry. I am now having some troubles integrate them with RTAB-map to realize better mapping and navigation 1. What I have done: 1.1 Using the RTAB-Map to run mapping with Kinect+Odom / Kinect+Laser+Odom. 1.2 Using the gmapping to map with my LiDAR. 1.3 Navigate using navigation stack within the map constructed by gampping (LiDAR only).
My Problem: The LiDAR is placed at a high position, so it cannot detect low obstacles (like this figure).
2. I tried many approaches to solve this problem:
2.1 Using the projected 2D map and re-run the navigation with LiDAR only (map server+AMCL). Although the projected map can detect the obstacles, during navigation stage, the robot will hit on it (I guess it is because LiDAR cannot find the obstacles and remove it during navigation).
2.2 I carefully look through the Turtlebot tutorial, but found the costmap only uses the laser, so I guess the same problem may still exists.
2.3 I followed the demo of az3 and modify it to try to run it on my own robot but failed (cannot send goal, seems no obstacle cloud and ground cloud). It seems there is something wrong with the obstacle detection node, and communication between the RTAB-Map and move_base. I will post some codes. I am really new so I guess there may be many mistakes.
3. My question:
3.1 What do you think is the best implementation to solve my question?
3.2 If it is possible to use the RTAB-Map obstacle detection node or just the pointcloud from Kinect to integrate into navigation stack to achieve avoiding low obstacles? If so, may I have your help on giving some detailed instructions?
3.3 If there is some thing I misunderstood with your tutorial on turtlebot mapping and navigation. Can it solve my problem? And how does RTAB-Map localize the robot during navigation (by using the VSLAM sensor of LiDAR?)?
Sorry for so many questions. I am new to ROS and want to develop a robot that can do SLAM and collect a database of Wi-Fi signal. This is my final year project and if you are interested in it we could together do some development (time is tight so I am desperate for your help).
Thanks again!
2.3
My launch file:
<launch> <!-- Kinect 2 Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2 Make sure it is calibrated! Run: $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true $ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch --> <!-- config param --> <arg name="pub_imu" default="False" /> <arg name="sub_ackermann" default="False" /> <arg name="lidar_frame" default="base_laser_link"/> <arg name="rgbd_odometry" default="false"/> <arg name="localization" default="false" /> <!-- Which image resolution to process in rtabmap: sd, qhd, hd --> <arg name="resolution" default="qhd" /> <!-- Choose visualization --> <arg name="rviz" default="true" /> <arg name="rtabmapviz" default="false" /> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <arg name="frame_id" default="base_footprint"/> <include file="$(find base_control)/launch/base_control.launch"> <arg name="pub_imu" value="$(arg pub_imu)"/> <arg name="sub_ackermann" value="$(arg sub_ackermann)"/> </include> <include file="$(find robot_navigation)/launch/lidar.launch"> <arg name="lidar_frame" value="$(arg lidar_frame)"/> </include> <!-- Rotate the camera --> <arg name="pi/2" value="1.5707963267948966"/> <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" /> <node pkg="tf" type="static_transform_publisher" name="kinect2_link" args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" /> <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link" args="0.1 0.0 0.4 0.0 0.2618 0.0 base_footprint kinect2_base_link 20" /> <!-- Corresponding config files --> <arg name="rtabmapviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" /> <!--<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" /> --> <!-- slightly increase default parameters for larger images (qhd=720p) --> <arg name="gftt_block_size" default="5" /> <arg name="gftt_min_distance" default="5" /> <group ns="rtabmap"> <!-- use rgb-d sync --> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen"> <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/> <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> <!-- output --> <!-- Should be true for not synchronized camera topics (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)--> <param name="approx_sync" value="false"/> </node> <!-- Odometry --> <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen"> <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/> <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="approx_sync" type="bool" value="false"/> <param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/> <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/> </node> <param unless="$(arg localization)" name="Rtabmap/TimeThr" type="string" value="500"/> <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/> <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/> <!-- Visual SLAM (robot side) --> <!-- args: "delete_db_on_start" and "udebug" --> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args= "--delete_db_on_start"> <param name="subscribe_depth" type="bool" value="false"/> <param name="subscribe_rgbd" type="bool" value="true"/> <param name="subscribe_scan" type="bool" value="true"/> <param name="Grid/FromDepth" type="bool" value="false"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="rgbd_odometry" type="bool" value="false"/> <param name="visual_odometry" type="bool" value="false"/> <param name="odom_frame_id" type="string" value="odom"/> <remap from="odom" to="/odom"/> <remap from="rgbd_image" to="rgbd_image"/> <remap from="scan" to="/scan"/> <param name="approx_sync" type="bool" value="true"/> <param name="use_action_for_goal" type="bool" value="true"/> <remap from="move_base" to="/move_base"/> <!-- RTAB-Map's parameters --> <param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <param name="RGBD/ProximityBySpace" type="string" value="true"/> <param name="RGBD/AngularUpdate" type="string" value="0.01"/> <param name="RGBD/LinearUpdate" type="string" value="0.01"/> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <param name="Reg/Force3DoF" type="string" value="true"/> <param name="Reg/Strategy" type="string" value="1"/> <!-- 1=ICP --> <param name="GridGlobal/MinSize" type="string" value="20"/> <!-- ICP parameters --> <param name="Icp/VoxelSize" type="string" value="0.05"/> <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/> <param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/> <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/> </node> <node pkg="nodelet" type="nodelet" name="obstacle_nodelet_manager" args="manager" output="screen"/> <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz obstacle_nodelet_manager"> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/> <remap from="depth/camera_info" to="/kinect2/$(arg resolution)/camera_info"/> <remap from="cloud" to="cloudXYZ" /> <param name="decimation" type="int" value="4"/> <param name="max_depth" type="double" value="4.0"/> <param name="voxel_size" type="double" value="0.02"/> </node> <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacle_nodelet_manager"> <remap from="cloud" to="cloudXYZ"/> <remap from="obstacles" to="/obstacles_cloud"/> <remap from="ground" to="/ground_cloud"/> <param name="frame_id" type="string" value="base_footprint"/> <param name="map_frame_id" type="string" value="map"/> <param name="wait_for_transform" type="bool" value="true"/> <param name="Grid/MinClusterSize" type="int" value="20"/> <param name="Grid/MaxObstacleHeight" type="double" value="0.5"/> <param name="Grid/MaxGroundAngle" type="double" value="0.1"/> </node> <!-- Visualisation RTAB-Map --> <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen"> <param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_scan" type="bool" value="true"/> <param name="approx_sync" type="bool" value="true"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/> <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/> <remap from="scan" to="/scan"/> </node> </group> <group ns="planner"> <arg name="cmd_vel_topic" default="cmd_vel" /> <arg name="odom_topic" default="odom" /> <arg name="planner" default="dwa" doc="opt: dwa, teb"/> <arg name="simulation" default= "false"/> <arg name="base_type" default= "$(env BASE_TYPE)"/> <remap from="scan" to="/scan"/> <remap from="obstacles_cloud" to="/obstacles_cloud"/> <remap from="ground_cloud" to="/ground_cloud"/> <remap from="map" to="/rtabmap/grid_map"/> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params(new).yaml" command="load" ns="global_costmap" /> <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params(new).yaml" command="load" ns="local_costmap" /> <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params(new).yaml" command="load" /> <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params(new).yaml" command="load" /> <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" /> <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/dwa_local_planner_params(new).yaml" command="load" /> <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/> <remap from="odom" to="$(arg odom_topic)"/> </node> </group> <!-- Visualization RVIZ --> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_navigation)/rviz/new_navigation.rviz" output="screen"/> <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) --> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet"> <remap from="rgb/image_in" to="/kinect2/$(arg resolution)/image_color_rect"/> <remap from="depth/image_in" to="/kinect2/$(arg resolution)/image_depth_rect"/> <remap from="rgb/camera_info_in" to="/kinect2/$(arg resolution)/camera_info"/> <remap from="odom_in" to="rtabmap/odom"/> <param name="approx_sync" type="bool" value="true"/> <remap from="rgb/image_out" to="data_odom_sync/image"/> <remap from="depth/image_out" to="data_odom_sync/depth"/> <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/> <remap from="odom_out" to="odom_sync"/> </node> </launch>
| Free forum by Nabble | Edit this page |