i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/introlab/rtabmap_ros into jade-devel

Browse Source
This commit is contained in:
matlabbe
2016-05-14 12:35:40 -04:00
parent 1648e6dfa3 804c6627d3
commit 0540fc71ce
72 changed files with 3650 additions and 2072 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+97 -33
View File
@@ -7,23 +7,41 @@ project(rtabmap_ros)
find_package(catkin REQUIRED COMPONENTS
cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions
pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
pcl_ros nodelet dynamic_reconfigure message_filters class_loader
genmsg stereo_msgs move_base_msgs
)
# Optional components
find_package(costmap_2d)
find_package(octomap_ros)
find_package(rviz)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.10.10 REQUIRED)
find_package(RTABMap 0.11.5 REQUIRED)
find_package(OpenCV REQUIRED)
#Qt stuff
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
INCLUDE(${QT_USE_FILE})
# If librtabmap_gui.so is found, rtabmapviz will be built
# If rviz is found, plugins will be built
IF(RTABMAP_GUI OR rviz_FOUND)
IF(RTABMAP_QT_VERSION EQUAL 4)
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
INCLUDE(${QT_USE_FILE})
ELSE()
IF(RTABMAP_GUI)
FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui REQUIRED)
ELSE()
# For rviz plugins, look for Qt5 before Qt4
FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
IF(NOT Qt5_FOUND)
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
INCLUDE(${QT_USE_FILE})
ENDIF(NOT Qt5_FOUND)
ENDIF()
ENDIF()
ENDIF(RTABMAP_GUI OR rviz_FOUND)
## We also use Ogre
include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
@@ -52,6 +70,7 @@ add_message_files(
Link.msg
OdomInfo.msg
Point2f.msg
Point3f.msg
Goal.msg
)
@@ -91,8 +110,9 @@ catkin_package(
LIBRARIES rtabmap_ros
CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions
pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
stereo_msgs move_base_msgs
pcl_ros nodelet dynamic_reconfigure message_filters class_loader
stereo_msgs move_base_msgs
DEPENDS RTABMap OpenCV
)
###########
@@ -116,21 +136,7 @@ SET(Libraries
${RTABMap_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
)
## RVIZ plugin
qt4_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
# tf:message_filters, mixing boost and Qt signals
set_property(
SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
)
SET(rtabmap_ros_lib_src
src/nodelets/data_throttle.cpp
src/nodelets/stereo_throttle.cpp
@@ -142,11 +148,7 @@ SET(rtabmap_ros_lib_src
src/nodelets/point_cloud_aggregator.cpp
src/MsgConversion.cpp
src/OdometryROS.cpp
src/rviz/MapCloudDisplay.cpp
src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp
src/rviz/OrbitOrientedViewController.cpp
${MOC_FILES}
src/MapsManager.cpp
)
# If costmap_2d is found, add the plugin
@@ -163,15 +165,72 @@ SET(rtabmap_ros_lib_src
)
ENDIF(costmap_2d_FOUND)
IF(QT4_FOUND OR Qt5_FOUND)
SET(Libraries
${Libraries}
${QT_LIBRARIES}
)
ENDIF(QT4_FOUND OR Qt5_FOUND)
# If rviz is found, add plugins
IF(rviz_FOUND)
MESSAGE(STATUS "WITH rviz")
include_directories(
${rviz_INCLUDE_DIRS}
)
SET(Libraries
${Libraries}
${rviz_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
)
## RVIZ plugin
IF(QT4_FOUND)
qt4_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ELSE()
qt5_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ENDIF()
# tf:message_filters, mixing boost and Qt signals
set_property(
SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
)
SET(rtabmap_ros_lib_src
${rtabmap_ros_lib_src}
src/rviz/MapCloudDisplay.cpp
src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp
src/rviz/OrbitOrientedViewController.cpp
${MOC_FILES}
)
ENDIF(rviz_FOUND)
############################
## Declare a cpp library
############################
add_library(rtabmap_ros
${rtabmap_ros_lib_src}
)
target_link_libraries(rtabmap_ros
${Libraries}
${QT_LIBRARIES}
${OGRE_LIBRARIES}
)
IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmap_ros Widgets Core Gui)
ENDIF(Qt5_FOUND)
add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
# If octomap is found, add definition
@@ -187,7 +246,7 @@ SET(Libraries
add_definitions(-DWITH_OCTOMAP)
ENDIF(octomap_ros_FOUND)
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp src/MapsManager.cpp)
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
add_executable(rgbd_odometry src/RGBDOdometryNode.cpp)
@@ -202,9 +261,6 @@ target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
add_executable(map_assembler src/MapAssemblerNode.cpp)
target_link_libraries(map_assembler rtabmap_ros ${Libraries})
add_executable(grid_map_assembler src/GridMapAssemblerNode.cpp)
target_link_libraries(grid_map_assembler rtabmap_ros ${Libraries})
add_executable(camera src/CameraNode.cpp)
add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(camera ${Libraries})
@@ -212,6 +268,9 @@ target_link_libraries(camera ${Libraries})
IF(RTABMAP_GUI)
add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries})
IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmapviz Widgets Core Gui)
ENDIF()
ELSE()
MESSAGE(WARNING "Found RTAB-Map built without its GUI library. Node rtabmapviz will not be built!")
ENDIF()
@@ -245,7 +304,6 @@ install(TARGETS
rgbd_odometry
stereo_odometry
map_assembler
grid_map_assembler
map_optimizer
data_player
camera
@@ -260,7 +318,6 @@ install(TARGETS
rgbd_odometry
stereo_odometry
map_assembler
grid_map_assembler
map_optimizer
data_player
camera
@@ -279,6 +336,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
launch/rtabmap.launch
launch/rgbd_mapping.launch
launch/stereo_mapping.launch
launch/data_recorder.launch
@@ -299,6 +357,12 @@ install(FILES
costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
IF(costmap_2d_FOUND)
install(FILES
costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
ENDIF(costmap_2d_FOUND)
#############
## Testing ##
README.md
+8 -3
View File
@@ -31,14 +31,19 @@ This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade**
* The next instructions assume that you have set up your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). I will use indigo prefix for convenience, but it should work with hydro and jade. The workspace path is `~/catkin_ws` and your `~/.bashrc` contains:
```bash
source /opt/ros/indigo/setup.bash
source ~/catkin_ws/devel/setup.bash
$ source /opt/ros/indigo/setup.bash
$ source ~/catkin_ws/devel/setup.bash
```
* Make sure you don't have the binaries installed too (if you tried them before):
```bash
$ sudo apt-get remove ros-indigo-rtabmap
```
0. Optional dependencies
* If you want SURF/SIFT on Indigo/Jade (Hydro has already SIFT/SURF), you have to build [OpenCV]([OpenCV](http://opencv.org/)) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). RTAB-Map can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but rtabmap_ros package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3.
* ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster.
* ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev` before building `g2o`) and would be [required to avoid some crashes](http://official-rtab-map-forum.67519.x6.nabble.com/ROS-2D-occupancy-grid-tp1204p1215.html).
```bash
$ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-g2o ros-indigo-rviz ros-indigo-cv-bridge
```
include/rtabmap_ros/MsgConversion.h
+24
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <tf/tf.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/CameraInfo.h>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>
@@ -40,10 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/OdometryInfo.h>
#include <rtabmap/core/Statistics.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap_ros/Link.h>
#include <rtabmap_ros/KeyPoint.h>
#include <rtabmap_ros/Point2f.h>
#include <rtabmap_ros/Point3f.h>
#include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/MapGraph.h>
#include <rtabmap_ros/NodeData.h>
@@ -83,6 +86,24 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg);
void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);
cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg);
void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg);
std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg);
void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg);
rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo,
const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
void cameraModelToROS(
const rtabmap::CameraModel & model,
sensor_msgs::CameraInfo & camInfo);
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses,
@@ -110,6 +131,9 @@ void mapGraphToROS(
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg);
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
launch/azimut3/az3_mapping_robot_kinect_scan.launch
+117 -55
View File
@@ -1,69 +1,131 @@
<launch>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<arg name="rtabmap_args" default="" />
<arg name="localization" default="false" />
<!-- AZIMUT 3 bringup: launch motors/odometry -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="rgb/image_out" to="data_throttled_image"/>
<remap from="depth/image_out" to="data_throttled_image_depth"/>
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet camera_nodelet_manager">
<remap from="image" to="depth_registered/image_raw"/>
<remap from="camera_info" to="depth_registered/camera_info"/>
<remap from="scan" to="/kinect_scan"/>
<param name="range_max" type="double" value="4"/>
</node>
</group>
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/kinect_scan"/>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Kp/MaxDepth" type="string" value="4.0"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccIcp2/Iterations" type="string" value="100"/>
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.5"/>
<param name="LccBow/MinInliers" type="string" value="3"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.05"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<remap from="scan" to="/kinect_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="goal_out" to="current_goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<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"/>
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/LocalRadius" type="string" value="4"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/OptimizeSlam2d" type="string" value="true"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="RGBD/OptimizeVarianceIgnored" type="string" value="false"/>
<param name="RGBD/PlanAngularVelocity" type="string" value="1.0"/> <!-- preference for path traversed forward -->
<param name="LccBow/Force2D" type="string" value="true"/>
<param name="LccIcp/Type" type="string" value="2"/>
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.2"/>
</node>
</group>
<!-- teleop -->
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
<remap from="joy" to="/joy"/>
<node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
<param name="cmd_eta/abtr_priority" value="50"/>
</group>
<!-- ROS navigation stack move_base -->
<group ns="planner">
<remap from="scan" to="/kinect_scan"/>
<remap from="obstacles_cloud" to="/obstacles_cloud"/>
<remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/map"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
</node>
<!-- Arbitration between teleop and planner -->
<node name="register_cmd_eta" pkg="abtr_priority" type="register"
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<param name="rate" type="double" value="3"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="rgb/image_out" to="throttled_image"/>
<remap from="depth/image_out" to="throttled_image_depth"/>
<remap from="rgb/camera_info_out" to="throttled_camera_info"/>
</node>
<!-- for the planner -->
<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="throttled_image_depth"/>
<remap from="depth/camera_info" to="throttled_camera_info"/>
<remap from="cloud" to="cloudXYZ" />
<param name="decimation" type="int" value="2"/>
<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="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
<param name="ground_normal_angle" type="double" value="0.1"/>
</node>
<!-- scan from the camera -->
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet camera_nodelet_manager">
<remap from="image" to="depth_registered/image_raw"/>
<remap from="camera_info" to="depth_registered/camera_info"/>
<remap from="scan" to="/kinect_scan"/>
<param name="range_max" type="double" value="4"/>
</node>
</group>
</launch>
launch/azimut3/az3_mapping_robot_nav.launch → launch/azimut3/az3_nav.launch
+49 -46
View File
@@ -1,8 +1,10 @@
<launch>
<!-- args: "delete_db_on_start" and "udebug" -->
<arg name="rtabmap_args" default="" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
@@ -13,59 +15,61 @@
<!-- SLAM (robot side) -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="use_action_for_goal" type="bool" value="true"/>
<param name="cloud_decimation" type="int" value="1"/> <!-- we already decimate in memory below -->
<param name="grid_eroded" type="bool" value="true"/>
<param name="grid_cell_size" type="double" value="0.05"/>
<param name="cloud_decimation" type="int" value="1"/> <!-- we already decimate in memory below -->
<param name="grid_eroded" type="bool" value="true"/>
<param name="grid_cell_size" type="double" value="0.05"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="mapData" to="mapData"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="goal_out" to="current_goal"/>
<remap from="goal_out" to="current_goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="grid_map" to="/map"/>
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="LccIcp/Type" type="string" value="2"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Mem/ImageDecimation" type="string" value="4"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/ImageDecimation" type="string" value="4"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
<param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
<param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Bayes/PredictionLC" type="string" value="0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.00035"/>
<param name="RGBD/OptimizeSlam2d" type="string" value="true"/>
<param name="RGBD/OptimizeIterations" type="string" value="100"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="Kp/NNStrategy" type="string" value="1"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="SURF/HessianThreshold" type="string" value="500"/>
<param name="LccBow/Force2D" type="string" value="true"/>
<param name="LccBow/MaxDepth" type="string" value="5"/>
<param name="LccBow/MinInliers" type="string" value="5"/>
<param name="LccBow/InlierDistance" type="string" value="0.1"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="5"/>
<param name="Vis/MinInliers" type="string" value="5"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
</group>
@@ -79,17 +83,16 @@
<!-- ROS navigation stack move_base -->
<group ns="planner">
<remap from="base_scan" to="/base_scan"/>
<remap from="scan" to="/base_scan"/>
<remap from="obstacles_cloud" to="/obstacles_cloud"/>
<remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_2d.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
@@ -109,7 +112,7 @@
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<param name="rate" type="double" value="5"/>
<param name="decimation" type="int" value="2"/>
@@ -128,7 +131,7 @@
<remap from="depth/camera_info" to="data_resized_camera_info"/>
<remap from="cloud" to="cloudXYZ" />
<param name="decimation" type="int" value="1"/> <!-- already decimated above -->
<param name="max_depth" type="double" value="3.0"/>
<param name="max_depth" type="double" value="3.0"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
@@ -137,10 +140,10 @@
<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="min_cluster_size" type="int" value="20"/>
<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="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
</node>
</group>
launch/azimut3/az3_mapping_client_nav.launch → launch/azimut3/az3_nav_client.launch
View File
launch/azimut3/az3_nav_kinect-only.launch
+40
View File
@@ -0,0 +1,40 @@
<launch>
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<!-- "Disable" odometry from azimut3 -->
<group ns="base_controller">
<param name="odom_frame_id" type="string" value="az3_odom"/>
<param name="base_frame_id" type="string" value="az3_base_link"/>
</group>
<remap from="/base_controller/odom" to="/base_controller/az3_odom"/>
<!-- use same launch file as "Kinect + Odometry" configuration -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_nav_kinect_odom.launch">
<arg name="localization" value="$(arg localization)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
</include>
<!-- Odometry -->
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="odom" to="/base_controller/odom"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/InlierDistance" type="string" value="0.1"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
</node>
</group>
</launch>
launch/azimut3/az3_nav_kinect_odom.launch
+161
View File
@@ -0,0 +1,161 @@
<launch>
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
<!-- SLAM (robot side) -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_scan" type="bool" value="false"/>
<param name="use_action_for_goal" type="bool" value="true"/>
<param name="cloud_decimation" type="int" value="4"/> <!-- we already decimate in memory below -->
<param name="grid_eroded" type="bool" value="true"/>
<param name="grid_cell_size" type="double" value="0.05"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="mapData" to="mapData"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="goal_out" to="current_goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="proj_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/ImageDecimation" type="string" value="1"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
<param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Mem/RawDescriptorsKept" type="string" value="true"/>
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="false"/>
<param name="Mem/UseDepthAsMask" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/EstimationType" type="string" value="1"/>
<param name="Bayes/PredictionLC" type="string" value="0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.00035"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Optimizer/Iterations" type="string" value="100"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Optimizer/Robust" type="string" value="false"/>
<param name="Optimizer/VarianceIgnored" type="string" value="true"/>
<param name="RGBD/PlanStuckIterations" type="string" value="10"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/MaxFeatures" type="string" value="300"/>
<param name="SURF/HessianThreshold" type="string" value="500"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
</group>
<!-- teleop -->
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
<remap from="joy" to="/joy"/>
<node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
<param name="cmd_eta/abtr_priority" value="50"/>
</group>
<!-- ROS navigation stack move_base -->
<group ns="planner">
<remap from="scan" to="/base_scan"/>
<remap from="obstacles_cloud" to="/obstacles_cloud"/>
<remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<arg name="observation_sources" value="point_cloud_sensorA point_cloud_sensorB"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
<param name="global_costmap/obstacle_layer/observation_sources" value="$(arg observation_sources)"/>
<param name="local_costmap/obstacle_layer/observation_sources" value="$(arg observation_sources)"/>
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
</node>
<!-- Arbitration between teleop and planner -->
<node name="register_cmd_eta" pkg="abtr_priority" type="register"
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<param name="rate" type="double" value="5"/>
<param name="decimation" type="int" value="2"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="rgb/image_out" to="data_resized_image"/>
<remap from="depth/image_out" to="data_resized_image_depth"/>
<remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
</node>
<!-- for the planner -->
<node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="data_resized_image_depth"/>
<remap from="depth/camera_info" to="data_resized_camera_info"/>
<remap from="cloud" to="cloudXYZ" />
<param name="decimation" type="int" value="1"/> <!-- already decimated above -->
<param name="max_depth" type="double" value="3.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 camera_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="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
</node>
</group>
</launch>
launch/azimut3/az3_openni.launch
+2 -1
View File
@@ -2,6 +2,7 @@
<launch>
<!-- Xtion -->
<param name="/camera/driver/data_skip" value="1" />
<include file="$(find openni2_launch)/launch/openni2.launch">
<arg name="depth_registration" value="True" />
<arg name="rgb_camera_info_url"
@@ -14,4 +15,4 @@
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.057 0.087 0.185 0.0 0.0 0.0 /base_link /camera_link 100" />
</launch>
</launch>
launch/azimut3/config/azimut3_nav.rviz
+160 -16
View File
@@ -7,7 +7,7 @@ Panels:
- /Global Options1
- /TF1/Frames1
Splitter Ratio: 0.601881
Tree Height: 187
Tree Height: 353
- Class: rviz/Selection
Name: Selection
- Class: rviz/Views
@@ -19,7 +19,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: Info
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
@@ -52,13 +52,73 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
base_footprint:
Value: true
base_laser_link:
Value: true
base_link:
Value: true
camera_depth_frame:
Value: true
camera_depth_optical_frame:
Value: true
camera_link:
Value: true
camera_rgb_frame:
Value: true
camera_rgb_optical_frame:
Value: true
map:
Value: true
odom:
Value: true
wheelLB_linkWheel_link:
Value: true
wheelLB_wheel_link:
Value: true
wheelLF_linkWheel_link:
Value: true
wheelLF_wheel_link:
Value: true
wheelRB_linkWheel_link:
Value: true
wheelRB_wheel_link:
Value: true
wheelRF_linkWheel_link:
Value: true
wheelRF_wheel_link:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
map:
odom:
base_footprint:
base_link:
base_laser_link:
{}
camera_link:
camera_depth_frame:
camera_depth_optical_frame:
{}
camera_rgb_frame:
camera_rgb_optical_frame:
{}
wheelLB_linkWheel_link:
wheelLB_wheel_link:
{}
wheelLF_linkWheel_link:
wheelLF_wheel_link:
{}
wheelRB_linkWheel_link:
wheelRB_wheel_link:
{}
wheelRF_linkWheel_link:
wheelRF_wheel_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
@@ -102,18 +162,18 @@ Visualization Manager:
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: false
Enabled: true
Name: Global costmap
Topic: /planner/move_base/global_costmap/costmap
Value: false
Value: true
- Alpha: 0.7
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Enabled: false
Name: Local costmap
Topic: /planner/move_base/local_costmap/costmap
Value: true
Value: false
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
@@ -124,6 +184,61 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLB_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLB_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLF_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLF_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRB_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRB_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRF_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRF_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
@@ -132,7 +247,7 @@ Visualization Manager:
Visual Enabled: true
- Class: rviz/Image
Enabled: true
Image Topic: /camera/data_resized_image_relay
Image Topic: /camera/throttled_image
Max Value: 1
Median window: 5
Min Value: 0
@@ -171,7 +286,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.01
Style: Points
Topic: /rtabmap/mapData_relay
Topic: /rtabmap/mapData
Use Fixed Frame: true
Use rainbow: true
Value: true
@@ -185,7 +300,13 @@ Visualization Manager:
Class: rviz/Path
Color: 255; 149; 57
Enabled: true
Line Style: Lines
Line Width: 0.03
Name: move_base global plan
Offset:
X: 0
Y: 0
Z: 0
Topic: /planner/move_base/NavfnROS/plan
Value: true
- Alpha: 1
@@ -193,7 +314,13 @@ Visualization Manager:
Class: rviz/Path
Color: 2; 14; 255
Enabled: true
Line Style: Lines
Line Width: 0.03
Name: move_base local plan
Offset:
X: 0
Y: 0
Z: 0
Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
Value: true
- Alpha: 1
@@ -227,17 +354,28 @@ Visualization Manager:
Value: true
- Alpha: 1
Class: rtabmap_ros/MapGraph
Color: 0; 0; 255
Enabled: true
Global loop closure: 255; 0; 0
Local loop closure: 255; 255; 0
Merged neighbor: 255; 170; 0
Name: MapGraph
Topic: /rtabmap/mapData_relay
Neighbor: 0; 0; 255
Topic: /rtabmap/mapGraph
User: 255; 0; 0
Value: true
Virtual: 255; 0; 255
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 0; 255
Enabled: true
Line Style: Lines
Line Width: 0.03
Name: Rtabmap global path
Offset:
X: 0
Y: 0
Z: 0
Topic: /rtabmap/global_path
Value: true
- Alpha: 1
@@ -245,7 +383,13 @@ Visualization Manager:
Class: rviz/Path
Color: 85; 255; 255
Enabled: true
Line Style: Lines
Line Width: 0.03
Name: Rtabmap local path
Offset:
X: 0
Y: 0
Z: 0
Topic: /rtabmap/local_path
Value: true
- Alpha: 1
@@ -308,10 +452,10 @@ Visualization Manager:
Z: -0.0856586
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.884797
Pitch: 1.2448
Target Frame: base_footprint
Value: Orbit (rviz)
Yaw: 3.81544
Yaw: 3.76044
Saved: ~
Window Geometry:
Displays:
@@ -321,7 +465,7 @@ Window Geometry:
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a2000000dd00fffffffb0000000a0049006d00610067006501000001d0000000870000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -331,5 +475,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1228
X: 406
Y: 154
X: 396
Y: 144
launch/azimut3/config/costmap_common_params_2d.yaml
+4 -6
View File
@@ -1,7 +1,5 @@
footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.02
#robot_radius: 0.38
#robot_radius: ir_of_robot
footprint_padding: 0.04
inflation_layer:
inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
transform_tolerance: 2
@@ -16,8 +14,8 @@ obstacle_layer:
laser_scan_sensor: {
data_type: LaserScan,
topic: base_scan,
expected_update_rate: 0.2,
topic: scan,
expected_update_rate: 0.1,
marking: true,
clearing: true
}
@@ -39,7 +37,7 @@ obstacle_layer:
expected_update_rate: 0.5,
marking: false,
clearing: true,
min_obstacle_height: -1.0 # make usre the ground is not filtered
min_obstacle_height: -1.0 # make sure the ground is not filtered
}
launch/azimut3/config/global_costmap_params.yaml
+1 -1
View File
@@ -2,7 +2,7 @@
global_frame: map
robot_base_frame: base_footprint
update_frequency: 1
publish_frequency: 2
publish_frequency: 1
always_send_full_costmap: false
plugins:
- {name: static_layer, type: "rtabmap_ros::StaticLayer"}
launch/config/appearance_gui.ini
+136 -14
View File
@@ -6,17 +6,12 @@ General\loggerPauseLevel=4
General\loggerType=1
General\loggerPrintTime=true
General\verticalLayoutUsed=false
General\imageFlipped=false
General\imageRejectedShown=true
General\imageHighestHypShown=true
General\beep=false
General\keypointsOpacity=16
General\voxelSize=0
General\decimation=16
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x2\xaf\0\0\x1\xf4\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x1\xf4\0\0\x1\xcc\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x1\xf0\xfc\x2\0\0\0\x3\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0(\0\0\x1\xf4\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf7\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0(\0\0\x1\xf0\0\0\0y\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0O\0\xff\xff\xff\0\0\0\0\0\0\x1\xf0\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xbd\0\0\0x\0\0\x5\xcc\0\0\x3k\0\0\0\xc5\0\0\0\x94\0\0\x5\xc4\0\0\x3\x63\0\0\0\0\0\0)
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1\a\0\0\x2\x30\xfc\x2\0\0\0\x2\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x30\0\0\x2\x30\0\xff\xff\xff\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1\xf4\0\0\0\x19\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x30\xfc\x2\0\0\0\x3\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0(\0\0\x1\xf4\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf7\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0(\0\0\x2\x30\0\0\0+\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0O\0\xff\xff\xff\0\0\0\0\0\0\x2\x30\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xc5\0\0\0K\0\0\x5\xd8\0\0\x3t\0\0\0\xcf\0\0\0q\0\0\x5\xce\0\0\x3j\0\0\0\0\0\0)
General\showClouds0=true
General\voxelSize0=0
General\decimation0=4
General\maxDepth0=4
General\showScans0=true
@@ -25,7 +20,6 @@ General\ptSize0=1
General\opacityScan0=1
General\ptSizeScan0=1
General\showClouds1=true
General\voxelSize1=0
General\decimation1=2
General\maxDepth1=0
General\showScans1=true
@@ -33,12 +27,140 @@ General\opacity1=1
General\ptSize1=1
General\opacityScan1=1
General\ptSizeScan1=1
General\showClouds2=true
General\voxelSize2=0.01
General\decimation2=1
General\maxDepth2=4
General\showScans2=true
General\meshing0=false
General\cloudFiltering=false
General\cloudFilteringRadius=0.5
General\cloudFilteringAngle=30
MainWindow\maximized=false
MainWindow\status_bar=false
PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0)
AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0)
widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0)
widget_cloudViewer\camera_focal=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0)
widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0)
widget_cloudViewer\grid=false
widget_cloudViewer\grid_cell_count=50
widget_cloudViewer\grid_cell_size=1
widget_cloudViewer\trajectory_shown=true
widget_cloudViewer\trajectory_size=100
widget_cloudViewer\camera_target_locked=false
widget_cloudViewer\camera_target_follow=true
widget_cloudViewer\camera_free=false
widget_cloudViewer\camera_lockZ=true
widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
imageView_source\image_shown=true
imageView_source\depth_shown=false
imageView_source\features_shown=true
imageView_source\lines_shown=true
imageView_source\alpha=50
imageView_source\graphics_view=false
imageView_source\graphics_view_scale=true
imageView_loopClosure\image_shown=true
imageView_loopClosure\depth_shown=false
imageView_loopClosure\features_shown=true
imageView_loopClosure\lines_shown=true
imageView_loopClosure\alpha=50
imageView_loopClosure\graphics_view=false
imageView_loopClosure\graphics_view_scale=true
imageView_odometry\image_shown=true
imageView_odometry\depth_shown=false
imageView_odometry\features_shown=true
imageView_odometry\lines_shown=true
imageView_odometry\alpha=200
imageView_odometry\graphics_view=false
imageView_odometry\graphics_view_scale=true
ExportCloudsDialog\binary=true
ExportCloudsDialog\normals_k=6
ExportCloudsDialog\regenerate=false
ExportCloudsDialog\regenerate_decimation=1
ExportCloudsDialog\regenerate_max_depth=4
ExportCloudsDialog\filtering=false
ExportCloudsDialog\filtering_radius=0.02
ExportCloudsDialog\filtering_min_neighbors=2
ExportCloudsDialog\assemble=true
ExportCloudsDialog\assemble_voxel=0.01
ExportCloudsDialog\subtract=false
ExportCloudsDialog\subtract_point_radius=0.02
ExportCloudsDialog\subtract_point_angle=45
ExportCloudsDialog\subtract_min_neighbors=5
ExportCloudsDialog\mls=false
ExportCloudsDialog\mls_radius=0.04
ExportCloudsDialog\mls_polygonial_order=2
ExportCloudsDialog\mls_upsampling_method=0
ExportCloudsDialog\mls_upsampling_radius=0.01
ExportCloudsDialog\mls_upsampling_step=0
ExportCloudsDialog\mls_point_density=0
ExportCloudsDialog\mls_dilation_voxel_size=0.01
ExportCloudsDialog\mls_dilation_iterations=0
ExportCloudsDialog\mesh=false
ExportCloudsDialog\mesh_radius=0.04
ExportCloudsDialog\mesh_mu=2.5
ExportCloudsDialog\mesh_decimation_factor=0
ExportCloudsDialog\mesh_texture=false
ExportCloudsDialog\mesh_angle_tolerance=15
ExportCloudsDialog\mesh_quad=false
ExportCloudsDialog\mesh_triangle_size=2
PostProcessingDialog\detect_more_lc=true
PostProcessingDialog\cluster_radius=0.5
PostProcessingDialog\cluster_angle=30
PostProcessingDialog\iterations=1
PostProcessingDialog\reextract_features=false
PostProcessingDialog\refine_neigbors=false
PostProcessingDialog\refine_lc=false
PostProcessingDialog\sba=false
PostProcessingDialog\sba_iterations=20
PostProcessingDialog\sba_epsilon=0.0001
PostProcessingDialog\sba_inlier_distance=0.05
PostProcessingDialog\sba_min_inliers=10
graphicsView_graphView\node_radius=0.00999999977648258
graphicsView_graphView\link_width=0
graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0)
graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0)
graphicsView_graphView\intra_inter_session_colors_enabled=false
graphicsView_graphView\grid_visible=true
graphicsView_graphView\origin_visible=true
graphicsView_graphView\referential_visible=true
graphicsView_graphView\local_radius_visible=false
graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0)
graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n)
General\loggerPrintThreadId=false
General\notifyNewGlobalPath=false
General\odomQualityThr=50
General\posteriorGraphView=true
General\showFeatures0=false
General\downsamplingScan0=1
General\voxelSizeScan0=0
General\ptSizeFeatures0=3
General\showFeatures1=true
General\downsamplingScan1=1
General\voxelSizeScan1=0
General\ptSizeFeatures1=3
General\showGraphs=true
General\showLabels=false
General\noFiltering=true
General\subtractFiltering=false
General\subtractFilteringMinPts=5
General\subtractFilteringRadius=0.02
General\subtractFilteringAngle=45
General\gridMapShown=false
General\gridMapResolution=0.05
General\gridMapOccupancyFrom3DCloud=false
General\gridMapEroded=false
General\gridMapOpacity=0.75
General\meshing=false
General\meshing_angle=15
General\meshing_quad=false
General\meshing_triangle_size=2
Figures\counts=1
Figures\curves=Loop/Highest_hypothesis_value/
launch/config/demo_stereo_outdoor.rviz
+75 -12
View File
@@ -6,7 +6,6 @@ Panels:
Expanded:
- /Global Options1
- /Status1
- /Info1
Splitter Ratio: 0.5
Tree Height: 438
- Class: rviz/Selection
@@ -134,6 +133,7 @@ Visualization Manager:
Download graph: false
Download map: false
Enabled: true
Filter ceiling (m): 0
Filter floor (m): 0
Invert Rainbow: false
Max Color: 255; 255; 255
@@ -147,17 +147,22 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.01
Style: Points
Topic: /rtabmap/mapData_optimized
Topic: /rtabmap/mapData
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rtabmap_ros/MapGraph
Color: 25; 255; 0
Enabled: true
Global loop closure: 255; 0; 0
Local loop closure: 255; 255; 0
Merged neighbor: 255; 170; 0
Name: MapGraph
Topic: /rtabmap/mapDataGraph_optimized
Neighbor: 0; 0; 255
Topic: /rtabmap/mapGraph
User: 255; 0; 0
Value: true
Virtual: 255; 0; 255
- Class: rviz/Image
Enabled: true
Image Topic: /stereo_camera/left/image_rect_color
@@ -173,15 +178,73 @@ Visualization Manager:
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: false
Enabled: true
Name: Map
Topic: /map
Value: false
Topic: /rtabmap/proj_map
Value: true
- Class: rtabmap_ros/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.17745
Min Value: -1.72299
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: OdomMap
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.01
Style: Points
Topic: /rtabmap/odom_local_map
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 85; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: OdomFrame
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.01
Style: Points
Topic: /rtabmap/odom_last_frame
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -206,7 +269,7 @@ Visualization Manager:
Views:
Current:
Class: rtabmap_ros/OrbitOriented
Distance: 6.60197
Distance: 8.28384
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
@@ -218,10 +281,10 @@ Visualization Manager:
Z: 0.113349
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.455398
Pitch: 0.635398
Target Frame: base_footprint
Value: OrbitOriented (rtabmap)
Yaw: 3.1304
Yaw: 3.0704
Saved: ~
Window Geometry:
Displays:
@@ -241,5 +304,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1341
X: 147
Y: 48
X: 97
Y: 14
launch/data_recorder.launch
+13 -7
View File
@@ -4,7 +4,8 @@
<arg name="subscribe_odometry" default="false"/>
<arg name="subscribe_depth" default="true"/>
<arg name="subscribe_stereo" default="false"/>
<arg name="subscribe_laserScan" default="false"/>
<arg name="subscribe_scan" default="false"/>
<arg name="stereo_approx_sync" default="false"/>
<arg name="frame_id" default="camera_link"/>
<arg name="odom_frame_id" default=""/> <!-- use topic when not set, otherwise use TF if set -->
@@ -30,13 +31,17 @@
<node name="data_recorder" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<!-- Disable any processing -->
<param name="Mem/RehearsalSimilarity" type="string" value="1.0"/> <!-- desactivate rehearsal -->
<param name="Kp/WordsPerImage" type="string" value="-1"/> <!-- desactivate keypoints extraction -->
<param name="Rtabmap/MaxRetrieved" type="string" value="0"/> <!-- desactivate global retrieval -->
<param name="RGBD/MaxLocalRetrieved" type="string" value="0"/> <!-- desactivate local retrieval -->
<param name="Rtabmap/MemoryThr" type="string" value="1"/> <!-- keep the WM empty -->
<param name="Mem/RehearsalSimilarity" type="string" value="1.0"/> <!-- deactivate rehearsal -->
<param name="Kp/MaxFeatures" type="string" value="-1"/> <!-- deactivate keypoints extraction -->
<param name="Rtabmap/MaxRetrieved" type="string" value="0"/> <!-- deactivate global retrieval -->
<param name="RGBD/MaxLocalRetrieved" type="string" value="0"/> <!-- deactivate local retrieval -->
<param name="Mem/MapLabelsAdded" type="string" value="false"/> <!-- don't create map labels -->
<param name="Rtabmap/MemoryThr" type="string" value="2"/> <!-- keep the WM empty -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- STM=1 -->
<param name="publish_tf" type="bool" value="false"/> <!-- don't publish TF -->
<param name="RGBD/ProximityBySpace" type="string" value="false"/>
<param name="RGBD/LinearUpdate" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0"/>
<param unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
<param name="Rtabmap/DetectionRate" type="string" value="$(arg max_rate)"/>
@@ -44,9 +49,10 @@
<param name="database_path" type="string" value="$(arg output_path)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="$(arg subscribe_depth)"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_laserScan)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_stereo" type="bool" value="$(arg subscribe_stereo)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg stereo_approx_sync)"/>
<!-- Hack to use a fake odom_frame_id=frame_id if subscribe_odometry = false -->
<param if="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
launch/demo/demo_appearance_localization.launch
-47
View File
@@ -1,47 +0,0 @@
<launch>
<!-- APPEARANCE-BASED LOCALIZATION VERSION -->
<group ns="rtabmap">
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
<param name="subscribe_depth" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="subscribe_laserScan" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="queue_size" type="int" value="10"/>
<remap from="rgb/image" to="/image"/> <!-- connect to "image" topic of the camera below -->
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/Enabled" type="string" value="false"/> <!-- False: appearance-based -->
<param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/> <!-- Database used for localization -->
<param name="Rtabmap/ImageBufferSize" type="string" value="0"/> <!-- process all images -->
<param name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- Go as fast as the camera rate (here 2 Hz, see below) -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- 1 location in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- false = Localization mode-->
<param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
</node>
<!-- visualization of the "infoEx" topic sent by rtabmap node -->
<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
<!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
<!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
<param name="camera_node_name" type="string" value="/camera"/>
</node>
</group>
<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
<node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
<remap from="image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
<param name="frame_rate" value="2.0" type="double"/>
<param name="width" value="0" type="int"/>
<param name="height" value="0" type="int"/>
<param name="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
</node>
</launch>
launch/demo/demo_appearance_mapping.launch
+27 -18
View File
@@ -4,27 +4,35 @@
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<group ns="rtabmap">
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="subscribe_depth" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="subscribe_depth" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="subscribe_laserScan" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="queue_size" type="int" value="10"/>
<param name="queue_size" type="int" value="10"/>
<remap from="rgb/image" to="/image"/> <!-- connect to "image" topic of the camera below -->
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/Enabled" type="string" value="false"/> <!-- False: appearance-based -->
<param name="Rtabmap/ImageBufferSize" type="string" value="0"/> <!-- process all images -->
<param name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- Go as fast as the camera rate (here 2 Hz, see below) -->
<param name="Mem/RehearsalSimilarity" type="string" value="0.4"/> <!-- 40% -->
<param name="Mem/STMSize" type="string" value="15"/> <!-- 15 locations in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="true"/> <!-- true = SLAM mode -->
<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/> <!-- On merging, update to new ID-->
<param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="RGBD/Enabled" type="string" value="false"/> <!-- False: appearance-based -->
<param name="Rtabmap/ImageBufferSize" type="string" value="0"/> <!-- process all images -->
<param name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- Go as fast as the camera rate (here 2 Hz, see below) -->
<param name="Mem/RehearsalSimilarity" type="string" value="0.4"/> <!-- 40% -->
<param name="Mem/STMSize" type="string" value="15"/> <!-- 15 locations in short-term memory -->
<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/> <!-- On merging, update to new ID-->
<param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- visualization of the "infoEx" topic sent by rtabmap node -->
@@ -40,11 +48,12 @@
<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
<node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
<remap from="image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="device_id" value="0" type="int"/>
<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
<param name="frame_rate" value="2.0" type="double"/>
<param name="width" value="0" type="int"/>
<param name="height" value="0" type="int"/>
<param name="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
<param name="frame_rate" value="2.0" type="double"/>
<param name="width" value="0" type="int"/>
<param name="height" value="0" type="int"/>
<param name="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
</node>
</launch>
launch/demo/demo_data_recorder.launch
+1 -2
View File
@@ -10,7 +10,7 @@
<arg name="subscribe_odometry" value="true"/>
<arg name="subscribe_depth" value="true"/>
<arg name="subscribe_stereo" value="false"/>
<arg name="subscribe_laserScan" value="true"/>
<arg name="subscribe_scan" value="true"/>
<arg name="frame_id" value="base_footprint"/>
<arg name="odom_frame_id" value=""/> <!-- use topic when not set, otherwise use TF if set -->
@@ -20,7 +20,6 @@
<arg name="queue_size" value="10"/>
<arg name="max_rate" value="0"/>
<arg name="camera_prefix" value="/camera"/>
<arg name="odom_topic" value="/az3/base_controller/odom"/>
<arg name="scan_topic" value="/jn0/base_scan"/>
</include>
launch/demo/demo_find_object.launch
+52 -32
View File
@@ -1,8 +1,12 @@
<launch>
<param name="use_sim_time" type="bool" value="True"/>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<param name="use_sim_time" type="bool" value="True"/>
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<group ns="rtabmap">
@@ -10,39 +14,56 @@
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.9"/>
<param name="LccIcp2/MaxFitness" type="string" value="0.1"/>
<param name="LccIcp2/Iterations" type="string" value="100"/>
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Proximity detection (using estimated position) with locations in WM -->
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- Registration strategy: 0=visual, 1=ICP, 2=visual+ICP -->
<param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<param name="Icp/Iterations" type="string" value="30"/>
<param name="Icp/VoxelSize" type="string" value="0.025"/>
<param name="Vis/MinInliers" type="string" value="10"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="Vis/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group>
<!-- send AZIMUT 3 urdf to param server -->
@@ -51,16 +72,15 @@
-->
<!-- Visualisation -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
@@ -69,16 +89,16 @@
<!-- Find-Object -->
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
<param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
<remap from="rgb/image_rect_color" to="/camera/data_throttled_image"/>
<remap from="depth_registered/image_raw" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/image_rect_color" to="/camera/data_throttled_image"/>
<remap from="depth_registered/image_raw" to="/camera/data_throttled_image_depth"/>
<remap from="depth_registered/camera_info" to="/camera/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth_registered/image_transport" type="string" value="compressedDepth"/>
</node>
launch/demo/demo_hector_mapping.launch
+16 -14
View File
@@ -49,37 +49,39 @@
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/scanmatch_odom"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<!-- RTAB-Map's parameters -->
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccBow/MaxDepth" type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Vis/MaxDepth" type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/scanmatch_odom"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/scanmatch_odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
@@ -93,7 +95,7 @@
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="voxel_size" type="double" value="0.01"/>
launch/demo/demo_multi-session_mapping.launch
+31 -32
View File
@@ -17,57 +17,56 @@
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/PoseScanMatching" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/>
<param name="LccIcp/Type" type="string" value="2"/>
<param name="LccIcp2/Iterations" type="string" value="100"/>
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
<param name="LccBow/MinInliers" type="string" value="5"/>
<param name="LccBow/MaxDepth" type="string" value="4.0"/>
<param name="LccBow/InlierDistance" type="string" value="0.1"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Kp/TfIdfLikelihoodUsed" type="string" value="false"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="false"/>
<param name="RGBD/ProximityByTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Icp/Iterations" type="string" value="30"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Vis/MinInliers" type="string" value="5"/>
<param name="Vis/MaxDepth" type="string" value="4.0"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Kp/TfIdfLikelihoodUsed" type="string" value="false"/>
<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="Kp/WordsPerImage" type="string" value="400"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/MaxFeatures" type="string" value="400"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
@@ -81,7 +80,7 @@
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
launch/demo/demo_robot_localization.launch
-83
View File
@@ -1,83 +0,0 @@
<launch>
<!-- ROBOT LOCALIZATION VERSION: use this with ROS bag demo_mapping.bag -->
<!-- A database "~/.ros/rtabmap.db" must be already created from -->
<!-- the "demo_robot_mapping.launch" demo -->
<!-- Once RTAB-Map GUI started, you can do "Edit->Download Map" to get all the map in the GUI -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<param name="use_sim_time" type="bool" value="True"/>
<group ns="rtabmap">
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/> <!-- Database used for localization -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- 1 location in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- false = Localization mode-->
<param name="Mem/InitWMWithAllNodes" type="string" value="true"/> <!-- Load the full global map in RAM -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccIcp2/MaxFitness" type="string" value="10"/>
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group>
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
launch/demo/demo_robot_mapping.launch
+35 -23
View File
@@ -10,51 +10,63 @@
<arg name="rtabmapviz" default="true" />
<param name="use_sim_time" type="bool" value="True"/>
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<group ns="rtabmap">
<!-- 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="frame_id" type="string" value="base_footprint"/>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
</node>
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/ProximityByTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group>
@@ -67,7 +79,7 @@
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
launch/demo/demo_stereo_outdoor.launch
+47 -66
View File
@@ -21,7 +21,7 @@
<param name="use_sim_time" type="bool" value="True"/>
<!-- Just to uncompress images for stereo_image_rect -->
<node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
<node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
<node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
<!-- Run the ROS package stereo_image_proc for image rectification -->
@@ -36,88 +36,69 @@
<param name="disparity_range" value="128"/>
</node>
</group>
<!-- Stereo Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom" to="/odometry"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="Odom/InlierDistance" type="string" value="0.1"/>
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="OdomBow/NNDR" type="string" value="0.8"/>
<param name="GFTT/MaxCorners" type="string" value="500"/>
<param name="GFTT/MinDistance" type="string" value="5"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
</node>
<group ns="rtabmap">
<!-- Stereo Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom" to="/stereo_odometry"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D 1=3D->2D (PnP) -->
<param name="Vis/MaxDepth" type="string" value="10"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom" to="/odometry"/>
<remap from="odom" to="/stereo_odometry"/>
<param name="queue_size" type="int" value="30"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="LccBow/MaxDepth" type="string" value="5"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="LccReextract/Activated" type="string" value="true"/>
<param name="LccReextract/MaxWords" type="string" value="500"/>
<!-- Disable graph optimization because we use map_optimizer node below -->
<param name="RGBD/ToroIterations" type="string" value="0"/>
</node>
<!-- Optimizing outside rtabmap node makes it able to optimize always the global map -->
<node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
<node if="$(arg rviz)" pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<param name="occupancy_grid" type="bool" value="true"/>
<remap from="mapData" to="mapData_optimized"/>
<remap from="grid_projection_map" to="/map"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="Kp/MaxDepth" type="string" value="10"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="10"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom_info" to="/odom_info"/>
<remap from="odom" to="/odometry"/>
<remap from="mapData" to="mapData_optimized"/>
<remap from="odom_info" to="odom_info"/>
<remap from="odom" to="/stereo_odometry"/>
<remap from="mapData" to="mapData"/>
</node>
</group>
launch/demo/demo_turtlebot_mapping.launch
+24 -23
View File
@@ -25,9 +25,13 @@
<arg name="localization" default="false"/>
<arg name="rgbd_odometry" default="false"/>
<arg name="args" default=""/>
<arg name="version083" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg name="wait_for_transform" default="0.1"/>
<arg name="wait_for_transform" default="0.2"/>
<!--
robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
can be increase from 5 to 10 Hz to avoid some TF warnings.
-->
<!-- Navigation stuff (move_base) -->
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>
@@ -42,7 +46,7 @@
<param name="odom_frame_id" type="string" value="odom"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
@@ -51,21 +55,22 @@
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<!-- output -->
<remap unless="$(arg version083)" from="grid_map" to="/map"/>
<!-- <remap unless="$(arg version083)" from="proj_map" to="/map"/> -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Set to false to generate map correction between /map and /odom -->
<param name="Kp/MaxDepth" type="string" value="4.0"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.3"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- Loop closure transformation refining with ICP: 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Icp/CoprrespondenceRatio" type="string" value="0.3"/>
<param name="Vis/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
@@ -75,25 +80,21 @@
<!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
<node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/Force2D" type="string" value="true"/>
<param name="Odom/InlierDistance" type="string" value="0.05"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/InlierDistance" type="string" value="0.05"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node>
<!-- backward compatibility with hydro 0.8.3 only -->
<node if="$(arg version083)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
<remap from="grid_map" to="/map"/>
</node>
<!-- visualization with rtabmapviz -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
launch/demo/demo_two_kinects.launch
+9 -9
View File
@@ -26,7 +26,7 @@
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"strategy" : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
@@ -63,12 +63,12 @@
<param name="depth_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node>
@@ -89,8 +89,8 @@
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
launch/rgbd_mapping.launch
+40 -46
View File
@@ -9,6 +9,9 @@
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- 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" />
@@ -29,91 +32,82 @@
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set -->
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.1"/>
<arg name="wait_for_transform" default="0.2"/>
<!-- Odometry parameters: -->
<arg name="strategy" default="0" /> <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
<arg name="feature" default="6" /> <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
<arg name="estimation" default="0" /> <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
<arg name="nn" default="3" /> <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE (SIFT, SURF), 2=FLANN_LSH, 3=BRUTEFORCE (ORB/FREAK/BRIEF/BRISK) -->
<arg name="max_depth" default="0" /> <!-- Maximum features depth (m) -->
<arg name="min_inliers" default="20" /> <!-- Minimum visual correspondences to accept a transformation (m) -->
<arg name="inlier_distance" default="0.1" /> <!-- RANSAC maximum inliers distance (m) -->
<arg name="local_map" default="1000" /> <!-- Local map size: number of unique features to keep track -->
<arg name="variance_inliers" default="true"/> <!-- Variance from inverse of inliers count -->
<!-- Nodes -->
<group ns="$(arg namespace)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
<!-- Odometry -->
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" launch-prefix="$(arg launch_prefix)">
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/EstimationType" type="string" value="$(arg estimation)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
<param name="Odom/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
</node>
<!-- Visual SLAM (robot side) -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="LccBow/EstimationType" type="string" value="$(arg estimation)"/>
<param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="LccIcp/Type" type="string" value="2"/>
<param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/>
<param if="$(arg subscribe_scan)" name="Reg/Force3DoF" type="string" value="true"/>
<!-- when 3D scan is set -->
<param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy" type="string" value="1"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
launch/rgbd_mapping_kinect2.launch
+10 -10
View File
@@ -30,7 +30,7 @@
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"strategy" : Strategy: Frame-to-Map 1=Frame-To-Frame
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
@@ -63,14 +63,14 @@
<param name="approx_sync" type="bool" value="true"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="Odom/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
<param name="Vis/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
<param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
</node>
@@ -86,8 +86,8 @@
<param name="approx_sync" type="bool" value="true"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
launch/rtabmap.launch
+187
View File
@@ -0,0 +1,187 @@
<launch>
<!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
<!-- For rgbd:=true
Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch openni2_launch openni2.launch depth_registration:=true -->
<!-- For stereo:=true
Your camera should be calibrated and publishing rectified left and right
images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
Example:
$ roslaunch rtabmap_ros bumblebee.launch -->
<!-- Choose between RGB-D and stereo -->
<arg name="rgbd" default="true"/>
<arg name="stereo" default="false"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="false" />
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="10"/>
<arg name="wait_for_transform" default="0.2"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<arg name="approx_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<!-- For depth_topic, "compressedDepth" image_transport is used. -->
<!-- For rgb_topic, see "rgb_image_transport" argument. -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="false"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/>
<arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/>
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- RGB-D Odometry -->
<group if="$(arg rgbd)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
<!-- Stereo Odometry -->
<group if="$(arg stereo)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="$(arg rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="$(arg rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="left/image" to="$(arg left_image_topic_relay)"/>
<remap from="right/image" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
<param if="$(arg stereo)" name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
</launch>
launch/stereo_mapping.launch
+43 -47
View File
@@ -9,6 +9,9 @@
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="false" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- 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" />
@@ -18,6 +21,7 @@
<arg name="optimize_from_last_node" default="false"/> <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/>
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
@@ -26,28 +30,20 @@
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<arg name="approximate_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set -->
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.1"/>
<arg name="wait_for_transform" default="0.2"/>
<!-- Odometry parameters: -->
<arg name="strategy" default="0" /> <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
<arg name="feature" default="6" /> <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
<arg name="estimation" default="1" /> <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
<arg name="nn" default="3" /> <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE (SIFT, SURF), 2=FLANN_LSH, 3=BRUTEFORCE (ORB/FREAK/BRIEF/BRISK) -->
<arg name="max_depth" default="0" /> <!-- Maximum features depth (m) -->
<arg name="min_inliers" default="20" /> <!-- Minimum visual correspondences to accept a transformation (m) -->
<arg name="inlier_distance" default="0.1" /> <!-- RANSAC maximum inliers distance (m) -->
<arg name="local_map" default="1000" /> <!-- Local map size: number of unique features to keep track -->
<arg name="odom_info_data" default="true" /> <!-- Fill odometry info messages with inliers/outliers data. -->
<arg name="variance_inliers" default="true"/> <!-- Variance from inverse of inliers count -->
<!-- Nodes -->
<group ns="$(arg namespace)">
@@ -55,7 +51,7 @@
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic)" />
<!-- Odometry -->
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -65,57 +61,56 @@
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/EstimationType" type="string" value="$(arg estimation)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
<param name="Odom/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="LccBow/EstimationType" type="string" value="$(arg estimation)"/>
<param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="LccIcp/Type" type="string" value="2"/>
<param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/>
<param if="$(arg subscribe_scan)" name="Reg/Force3DoF" type="string" value="true"/>
<!-- when 3D scan is set -->
<param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy" type="string" value="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="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
@@ -123,6 +118,7 @@
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
launch/tests/bumblebee.launch
+8 -8
View File
@@ -10,15 +10,16 @@
<param name="camera_info_url_right" value="" />
</node>
<arg name="gen_depth" default="false"/>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Run the ROS package stereo_image_proc (throttle to 10 Hz to avoid rectifying all images) -->
<group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
<remap from="left/image" to="left/image_raw"/>
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
<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"/>
@@ -28,13 +29,12 @@
</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/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"/>
</node>
<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
</group>
</launch>
</launch>
launch/tests/rgbdslam_datasets.launch
+22 -25
View File
@@ -10,20 +10,7 @@
-->
<param name="use_sim_time" type="bool" value="True"/>
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="1000" />
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
@@ -40,13 +27,18 @@
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
<param name="Vis/CorType" type="string" value="0"/> <!-- 0=features matching 1=Optical Flow -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="Vis/CorNNDR" type="string" value="0.6"/>
<param name="Odom/ResetCountdown" type="string" value="15"/>
<param name="Odom/KeyFrameThr" type="string" value="0.5"/>
<param name="odom_frame_id" type="string" value="vis_odom"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="publish_tf" type="bool" value="false"/>
<param name="queue_size" type="int" value="30"/>
@@ -64,15 +56,20 @@
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
<param name="Vis/EstimationType" type="string" value="0"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="false"/>
<param name="Mem/RawDescriptorsKept" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="odom"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.05"/>
<remap from="odom" to="vis_odom"/>
<param name="queue_size" type="int" value="30"/>
</node>
@@ -89,7 +86,7 @@
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="odom"/>
<remap from="odom" to="vis_odom"/>
</node>
</group>
launch/tests/test_odometry.launch
-61
View File
@@ -1,61 +0,0 @@
<launch>
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="2000" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
<remap from="odom_in" to="odom"/>
<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"/>
<param name="queue_size" type="int" value="30"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
<!-- Odometry -->
<node pkg="rtabmap_ros" type="visual_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="camera/rgb/image_rect_color"/>
<remap from="depth/image" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="camera/depth_registered/camera_info"/>
<remap from="odom" to="odom"/>
<param name="frame_id" type="string" value="base_link"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap visual_odometry (double-dash)params" to see the list of available parameters. -->
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/test_odometry.rviz"/>
</launch>
launch/tests/test_stereo_data_recorder.launch
-56
View File
@@ -1,56 +0,0 @@
<launch>
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
<param name="format7_color_coding" value="raw16" />
<param name="bayer_pattern" value="bggr" />
<param name="bayer_method" value="" />
<param name="stereo_method" value="Interlaced" />
<param name="camera_info_url_left" value="" />
<param name="camera_info_url_right" value="" />
</node>
<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="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
<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="10"/>
<param name="rate" type="double" value="20"/>
</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"/>
</node>
</group>
<node name="data_recorder" pkg="rtabmap_ros" type="data_recorder" output="screen">
<param name="output_file_name" value="output.db" type="string"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="subscribe_odometry" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="queue_size" type="int" value="20"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect_color"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<param name="queue_size" type="int" value="10"/>
</node>
</launch>
launch/tests/test_stereo_odometry.launch
-48
View File
@@ -1,48 +0,0 @@
<launch>
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
<param name="format7_color_coding" value="raw16" />
<param name="bayer_pattern" value="bggr" />
<param name="bayer_method" value="" />
<param name="stereo_method" value="Interlaced" />
<param name="camera_info_url_left" value="" />
<param name="camera_info_url_right" value="" />
</node>
<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="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
<!-- Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="left/image_rect"/>
<remap from="right/image_rect" to="right/image_rect"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<remap from="odom" to="odom_stereo"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="queue_size" type="int" value="5"/>
<param name="Odom/InlierDistance" type="string" value="0.1"/>
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="GFTT/MaxCorners" type="string" value="500"/>
<param name="GFTT/MinDistance" type="string" value="5"/>
</node>
</group>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/test_odometry.rviz"/>
</launch>
msg/Info.msg
+1 -1
View File
@@ -7,7 +7,7 @@ Header header
int32 refId
int32 loopClosureId
int32 localLoopClosureId
int32 proximityDetectionId
geometry_msgs/Transform loopClosureTransform
msg/NodeData.msg
+3
View File
@@ -8,6 +8,9 @@ string label
# Pose from odometry not corrected
geometry_msgs/Pose pose
# Ground truth (optional)
geometry_msgs/Pose groundTruthPose
# compressed image in /camera_link frame
# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
uint8[] image
msg/OdomInfo.msg
+2
View File
@@ -42,6 +42,8 @@ int32[] wordsKeys
KeyPoint[] wordsValues
int32[] wordMatches
int32[] wordInliers
int32[] localMapKeys
Point3f[] localMapValues
Point2f[] refCorners
Point2f[] newCorners
msg/Point3f.msg
+10
View File
@@ -0,0 +1,10 @@
#class cv::Point3f
#{
# float x;
# float y;
# float z;
#}
float32 x
float32 y
float32 z
package.xml
+4 -3
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.10.10</version>
<version>0.11.5</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
@@ -37,7 +37,8 @@
<build_depend>class_loader</build_depend>
<build_depend>rtabmap</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>costmap_2d</build_depend>
<!-- costmap_2d is not on kinetic yet -->
<!-- <build_depend>costmap_2d</build_depend> -->
<build_depend>octomap_ros</build_depend>
<build_depend>octomap</build_depend>
@@ -67,7 +68,7 @@
<run_depend>class_loader</run_depend>
<run_depend>rtabmap</run_depend>
<run_depend>move_base_msgs</run_depend>
<run_depend>costmap_2d</run_depend>
<!-- <run_depend>costmap_2d</run_depend> -->
<run_depend>octomap_ros</run_depend>
<run_depend>octomap</run_depend>
src/CameraNode.cpp
+1 -1
View File
@@ -193,7 +193,7 @@ public:
if(!path.empty() && UDirectory::exists(path))
{
//images
camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate);
camera_ = new rtabmap::CameraImages(path, frameRate);
}
else if(!path.empty() && UFile::exists(path))
{
src/CoreNode.cpp
+3 -14
View File
@@ -48,14 +48,6 @@ int main(int argc, char** argv)
{
deleteDbOnStart = true;
}
else if(strcmp(argv[i], "--udebug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
}
else if(strcmp(argv[i], "--uinfo") == 0)
{
ULogger::setLevel(ULogger::kInfo);
}
else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -94,14 +86,11 @@ int main(int argc, char** argv)
"argument \"--params\" is detected!");
exit(0);
}
else
{
ROS_ERROR("Not recognized argument \"%s\"", argv[i]);
exit(-1);
}
}
CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart);
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart, parameters);
ROS_INFO("rtabmap %s started...", RTABMAP_VERSION);
ros::spin();
src/CoreWrapper.cpp
+541 -267
View File
File diff suppressed because it is too large Load Diff
src/CoreWrapper.h
+84 -10
View File
@@ -80,13 +80,14 @@ typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseCl
class CoreWrapper
{
public:
CoreWrapper(bool deleteDbOnStart);
CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters);
virtual ~CoreWrapper();
private:
void setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync,
@@ -102,20 +103,23 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
void commonDepthCallback(
const std::string & odomFrameId,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg);
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
void commonStereoCallback(
const std::string & odomFrameId,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
// with odom msg
void depthCallback(
@@ -129,6 +133,12 @@ private:
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void depthScan3dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg);
void stereoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -142,6 +152,13 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void stereoScan3dCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
@@ -161,6 +178,11 @@ private:
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void depthScan3dTFCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg);
void stereoTFCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -172,8 +194,14 @@ private:
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void stereoScan3dTFCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg);
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp);
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
void updateGoal(const ros::Time & stamp);
@@ -183,8 +211,8 @@ private:
const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
double odomRotationalVariance = 1.0,
double odomTransitionalVariance = 1.0);
float odomRotationalVariance = 1.0,
float odomTransitionalVariance = 1.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -194,6 +222,10 @@ private:
bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
@@ -225,8 +257,9 @@ private:
bool paused_;
rtabmap::Transform lastPose_;
ros::Time lastPoseStamp_;
double rotVariance_;
double transVariance_;
bool lastPoseIntermediate_;
float rotVariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_;
bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_;
@@ -234,6 +267,7 @@ private:
std::string frameId_;
std::string mapFrameId_;
std::string odomFrameId_;
std::string groundTruthFrameId_;
std::string configPath_;
std::string databasePath_;
bool waitForTransform_;
@@ -241,6 +275,7 @@ private:
bool useActionForGoal_;
bool genScan_;
double genScanMaxDepth_;
double genScanMinDepth_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
@@ -276,6 +311,7 @@ private:
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
@@ -285,6 +321,14 @@ private:
sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
@@ -301,6 +345,15 @@ private:
nav_msgs::Odometry> MyStereoScanSyncPolicy;
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
sensor_msgs::PointCloud2,
nav_msgs::Odometry> MyStereoScan3dSyncPolicy;
message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
@@ -335,6 +388,13 @@ private:
sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
@@ -349,6 +409,14 @@ private:
sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy;
message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
@@ -374,6 +442,10 @@ private:
ros::ServiceServer backupDatabase_;
ros::ServiceServer setModeLocalizationSrv_;
ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer setLogDebugSrv_;
ros::ServiceServer setLogInfoSrv_;
ros::ServiceServer setLogWarnSrv_;
ros::ServiceServer setLogErrorSrv_;
ros::ServiceServer getMapDataSrv_;
ros::ServiceServer getProjMapSrv_;
ros::ServiceServer getGridMapSrv_;
@@ -392,7 +464,9 @@ private:
boost::thread* transformThread_;
float rate_;
bool createIntermediateNodes_;
ros::Time time_;
ros::Time previousStamp_;
};
#endif /* COREWRAPPER_H_ */
src/DbPlayerNode.cpp
+6 -8
View File
@@ -90,7 +90,7 @@ int main(int argc, char** argv)
std::string odomFrameId = "odom";
std::string cameraFrameId = "camera_optical_link";
std::string scanFrameId = "base_laser_link";
double rate = 1.0f;
double rate = -1.0f;
std::string databasePath = "";
bool publishTf = true;
int startId = 0;
@@ -214,7 +214,7 @@ int main(int argc, char** argv)
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{
//stereo
if(odom.data().stereoCameraModel().isValid())
if(odom.data().stereoCameraModel().isValidForProjection())
{
camInfoA.D.resize(8,0);
@@ -257,14 +257,12 @@ int main(int argc, char** argv)
// publish transforms first
if(publishTf)
{
ros::Time tfExpiration = time + ros::Duration(rate>0?1.0/rate:acquisitionTime);
rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModel().isValid())
else if(odom.data().stereoCameraModel().isValidForProjection())
{
localTransform = odom.data().stereoCameraModel().left().localTransform();
}
@@ -273,7 +271,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = tfExpiration;
baseToCamera.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera);
}
@@ -283,7 +281,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = tfExpiration;
odomToBase.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase);
}
@@ -293,7 +291,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId;
baseToLaserScan.header.frame_id = frameId;
baseToLaserScan.header.stamp = tfExpiration;
baseToLaserScan.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform);
tfBroadcaster.sendTransform(baseToLaserScan);
}
src/GridMapAssemblerNode.cpp
-199
View File
@@ -1,199 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>
#include <std_srvs/Empty.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
using namespace rtabmap;
class GridMapAssembler
{
public:
GridMapAssembler() :
gridCellSize_(0.05), // meters
mapSize_(0), // meters
eroded_(false),
filterRadius_(0.5),
filterAngle_(30.0) // degrees
{
ros::NodeHandle pnh("~");
pnh.param("cell_size", gridCellSize_, gridCellSize_); // m
pnh.param("map_size", mapSize_, mapSize_); // m
pnh.param("filter_radius", filterRadius_, filterRadius_);
pnh.param("filter_angle", filterAngle_, filterAngle_);
pnh.param("eroded", eroded_, eroded_);
UASSERT(gridCellSize_ > 0.0);
UASSERT(mapSize_ >= 0.0);
ros::NodeHandle nh;
mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this);
gridMap_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
//private service
getMapService_ = pnh.advertiseService("get_map", &GridMapAssembler::getGridMapCallback, this);
resetService_ = pnh.advertiseService("reset", &GridMapAssembler::reset, this);
}
~GridMapAssembler()
{
}
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
UTimer timer;
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
if(!uContains(gridMaps_, msg->nodes[i].id) && msg->nodes[i].laserScan.size())
{
cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
if(!laserScan.empty())
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, gridCellSize_);
if(!ground.empty() || !obstacles.empty())
{
gridMaps_.insert(std::make_pair(msg->nodes[i].id, std::make_pair(ground, obstacles)));
}
}
}
}
std::map<int, Transform> poses;
UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
{
poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
}
if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
{
poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
}
if(gridMap_.getNumSubscribers())
{
// create the map
float xMin=0.0f, yMin=0.0f;
//cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin, mapSize_);
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
gridMaps_,
gridCellSize_,
xMin, yMin,
mapSize_,
eroded_);
if(!pixels.empty())
{
//init
map_.info.resolution = gridCellSize_;
map_.info.origin.position.x = 0.0;
map_.info.origin.position.y = 0.0;
map_.info.origin.position.z = 0.0;
map_.info.origin.orientation.x = 0.0;
map_.info.origin.orientation.y = 0.0;
map_.info.origin.orientation.z = 0.0;
map_.info.origin.orientation.w = 1.0;
map_.info.width = pixels.cols;
map_.info.height = pixels.rows;
map_.info.origin.position.x = xMin;
map_.info.origin.position.y = yMin;
map_.data.resize(map_.info.width * map_.info.height);
memcpy(map_.data.data(), pixels.data, map_.info.width * map_.info.height);
map_.header.frame_id = msg->header.frame_id;
map_.header.stamp = ros::Time::now();
gridMap_.publish(map_);
ROS_INFO("Grid Map published [%d,%d] (%fs)", pixels.cols, pixels.rows, timer.ticks());
}
}
}
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
if(map_.data.size())
{
res.map = map_;
return true;
}
return false;
}
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("grid_map_assembler: reset!");
gridMaps_.clear();
map_ = nav_msgs::OccupancyGrid();
return true;
}
private:
double gridCellSize_;
double mapSize_;
bool eroded_;
double filterRadius_;
double filterAngle_;
ros::Subscriber mapDataTopic_;
ros::Publisher gridMap_;
ros::ServiceServer getMapService_;
ros::ServiceServer resetService_;
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; //<ground,obstacles>
nav_msgs::OccupancyGrid map_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "grid_map_assembler");
GridMapAssembler assembler;
ros::spin();
return 0;
}
src/GuiNode.cpp
+17 -6
View File
@@ -33,8 +33,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <signal.h>
QApplication * app = 0;
ros::AsyncSpinner * spinner = 0;
void my_handler(int s){
QApplication::exit();
ROS_INFO("rtabmapviz: ctrl-c catched! Exiting Qt app...");
spinner->stop();
exit(-1);
}
int main(int argc, char** argv)
@@ -46,7 +51,10 @@ int main(int argc, char** argv)
ros::init(argc, argv, "rtabmapviz");
GuiWrapper gui(argc, argv);
app = new QApplication(argc, argv);
app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
GuiWrapper * gui = new GuiWrapper(argc, argv);
// Catch ctrl-c to close the gui
// (Place this after QApplication's constructor)
@@ -57,15 +65,18 @@ int main(int argc, char** argv)
sigaction(SIGINT, &sigIntHandler, NULL);
// Here start the ROS events loop
ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
spinner = new ros::AsyncSpinner(1); // Use 1 thread
spinner->start();
ROS_INFO("rtabmapviz started.");
// Now wait for application to finish
int r = gui.exec();// MUST be called by the Main Thread
int r = app->exec();// MUST be called by the Main Thread
spinner.stop();
spinner->stop();
delete spinner;
delete gui;
delete app;
ROS_INFO("rtabmapviz: All done! Closing...");
return r;
}
src/GuiWrapper.cpp
+455 -102
View File
@@ -26,8 +26,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "GuiWrapper.h"
#include <QtGui/QApplication>
#include <QtCore/QDir>
#include <QApplication>
#include <QDir>
#include <cv_bridge/cv_bridge.h>
#include <std_srvs/Empty.h>
@@ -40,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#include <rtabmap/gui/MainWindow.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.h>
@@ -71,11 +68,10 @@ float max3( const float& a, const float& b, const float& c)
}
GuiWrapper::GuiWrapper(int & argc, char** argv) :
app_(0),
mainWindow_(0),
frameId_("base_link"),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
waitForTransformDuration_(0.2), // 200 ms
cameraNodeName_(""),
lastOdomInfoUpdateTime_(0),
depthScanSync_(0),
@@ -88,7 +84,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
depthOdomInfo2Sync_(0)
{
ros::NodeHandle nh;
app_ = new QApplication(argc, argv);
QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
for(int i=1; i<argc; ++i)
@@ -114,12 +109,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
bool paused = false;
nh.param("is_rtabmap_paused", paused, paused);
mainWindow_->setMonitoringState(paused);
app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
ros::NodeHandle pnh("~");
// To receive odometry events
bool subscribeLaserScan = false;
bool subscribeLaserScan2d = false;
bool subscribeLaserScan3d = false;
bool subscribeDepth = false;
bool subscribeOdomInfo = false;
bool subscribeStereo = false;
@@ -130,7 +125,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d) && subscribeLaserScan2d)
{
ROS_WARN("rtabmapviz: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
}
pnh.param("subscribe_scan", subscribeLaserScan2d, subscribeLaserScan2d);
pnh.param("subscribe_scan_cloud", subscribeLaserScan3d, subscribeLaserScan3d);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
pnh.param("depth_cameras", depthCameras, depthCameras);
@@ -181,7 +181,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
this->setupCallbacks(
subscribeDepth,
subscribeLaserScan,
subscribeLaserScan2d,
subscribeLaserScan3d,
subscribeOdomInfo,
subscribeStereo,
queueSize,
@@ -210,6 +211,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
GuiWrapper::~GuiWrapper()
{
UDEBUG("");
if(depthSync_)
delete depthSync_;
if(depth2Sync_)
@@ -245,12 +247,6 @@ GuiWrapper::~GuiWrapper()
delete infoMapSync_;
delete mainWindow_;
delete app_;
}
int GuiWrapper::exec()
{
return app_->exec();
}
void GuiWrapper::infoMapCallback(
@@ -292,7 +288,7 @@ void GuiWrapper::goalPathCallback(
poses[i].first = -int(i)-1;
poses[i].second = rtabmap_ros::transformFromPoseMsg(pathMsg->poses[i].pose);
}
this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses));
this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses, 0.0));
}
void GuiWrapper::goalReachedCallback(
@@ -441,7 +437,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
poses[i].first = setGoalSrv.response.path_ids[i];
poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]);
}
this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses));
this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time));
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
@@ -489,7 +485,8 @@ Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::s
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_);
ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds (for stamp=%f)!",
fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec());
return transform;
}
}
@@ -510,7 +507,8 @@ void GuiWrapper::commonDepthCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
@@ -519,7 +517,7 @@ void GuiWrapper::commonDepthCallback(
imageMsgs.push_back(imageMsg);
depthMsgs.push_back(depthMsg);
cameraInfoMsgs.push_back(cameraInfoMsg);
commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg);
commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void GuiWrapper::commonDepthCallback(
@@ -527,7 +525,8 @@ void GuiWrapper::commonDepthCallback(
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
@@ -547,9 +546,13 @@ void GuiWrapper::commonDepthCallback(
}
else
{
if(scanMsg.get())
if(scan2dMsg.get())
{
odomHeader = scanMsg->header;
odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
}
else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
{
@@ -679,22 +682,15 @@ void GuiWrapper::commonDepthCallback(
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
}
}
cv::Mat scan;
if(scanMsg.get() != 0)
if(scan2dMsg.get() != 0)
{
// make sure the frame of the laser is updated too
if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
{
return;
}
@@ -702,16 +698,16 @@ void GuiWrapper::commonDepthCallback(
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
// sync with odometry stamp
if(odomHeader.stamp != scanMsg->header.stamp)
if(odomHeader.stamp != scan2dMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp);
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp);
if(sensorT.isNull())
{
return;
@@ -723,6 +719,12 @@ void GuiWrapper::commonDepthCallback(
}
scan = util3d::laserScanFromPointCloud(*pclScan);
}
else if(scan3dMsg.get() != 0)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
scan = util3d::laserScanFromPointCloud(*pclScan);
}
rtabmap::OdometryInfo info;
if(odomInfoMsg.get())
@@ -733,8 +735,8 @@ void GuiWrapper::commonDepthCallback(
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
scanMsg.get()?(int)scanMsg->ranges.size():0,
scanMsg.get()?(int)scanMsg->range_max:0,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
rgb,
depth,
cameraModels,
@@ -754,7 +756,8 @@ void GuiWrapper::commonStereoCallback(
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
// limit 10 Hz max
@@ -787,9 +790,13 @@ void GuiWrapper::commonStereoCallback(
}
else
{
if(scanMsg.get())
if(scan2dMsg.get())
{
odomHeader = scanMsg->header;
odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
}
else
{
@@ -838,17 +845,9 @@ void GuiWrapper::commonStereoCallback(
}
}
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
if(model.baseline() > 10.0)
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
@@ -856,7 +855,7 @@ void GuiWrapper::commonStereoCallback(
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
stereoModel.baseline());
shown = true;
}
}
@@ -878,10 +877,10 @@ void GuiWrapper::commonStereoCallback(
cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
cv::Mat scan;
if(scanMsg.get() != 0)
if(scan2dMsg.get() != 0)
{
// make sure the frame of the laser is updated too
if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
{
return;
}
@@ -889,16 +888,16 @@ void GuiWrapper::commonStereoCallback(
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
// sync with odometry stamp
if(odomHeader.stamp != scanMsg->header.stamp)
if(odomHeader.stamp != scan2dMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp);
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp);
if(sensorT.isNull())
{
return;
@@ -908,6 +907,12 @@ void GuiWrapper::commonStereoCallback(
}
}
scan = util3d::laserScan2dFromPointCloud(*pclScan);
}
else if(scan3dMsg.get() != 0)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
scan = util3d::laserScanFromPointCloud(*pclScan);
}
@@ -920,8 +925,8 @@ void GuiWrapper::commonStereoCallback(
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
scanMsg.get()?(int)scanMsg->ranges.size():0,
scanMsg.get()?(int)scanMsg->range_max:0,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
left,
right,
stereoModel,
@@ -944,6 +949,7 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
sensor_msgs::ImageConstPtr(),
sensor_msgs::CameraInfoConstPtr(),
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
@@ -959,6 +965,7 @@ void GuiWrapper::depthCallback(
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
@@ -987,6 +994,7 @@ void GuiWrapper::depth2Callback(
depthMsgs,
cameraInfoMsgs,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
@@ -1003,6 +1011,7 @@ void GuiWrapper::depthOdomInfoCallback(
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
@@ -1032,6 +1041,7 @@ void GuiWrapper::depthOdomInfo2Callback(
depthMsgs,
cameraInfoMsgs,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
@@ -1048,9 +1058,63 @@ void GuiWrapper::depthScanCallback(
depthMsg,
cameraInfoMsg,
scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::depthScanOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
commonDepthCallback(
odomMsg,
imageMsg,
depthMsg,
cameraInfoMsg,
scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
void GuiWrapper::depthScan3dCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
commonDepthCallback(
odomMsg,
imageMsg,
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
scanMsg,
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::depthScan3dOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
commonDepthCallback(
odomMsg,
imageMsg,
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
scanMsg,
odomInfoMsg);
}
void GuiWrapper::stereoScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1066,9 +1130,69 @@ void GuiWrapper::stereoScanCallback(
leftCameraInfoMsg,
rightCameraInfoMsg,
scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::stereoScanOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
{
commonStereoCallback(
odomMsg,
leftImageMsg,
rightImageMsg,
leftCameraInfoMsg,
rightCameraInfoMsg,
scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
void GuiWrapper::stereoScan3dCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
{
commonStereoCallback(
odomMsg,
leftImageMsg,
rightImageMsg,
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
scanMsg,
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::stereoScan3dOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
{
commonStereoCallback(
odomMsg,
leftImageMsg,
rightImageMsg,
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
scanMsg,
odomInfoMsg);
}
void GuiWrapper::stereoOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1084,6 +1208,7 @@ void GuiWrapper::stereoOdomInfoCallback(
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
@@ -1101,6 +1226,7 @@ void GuiWrapper::stereoCallback(
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
@@ -1116,6 +1242,7 @@ void GuiWrapper::depthTFCallback(
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
@@ -1131,6 +1258,7 @@ void GuiWrapper::depthOdomInfoTFCallback(
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
@@ -1146,6 +1274,23 @@ void GuiWrapper::depthScanTFCallback(
depthMsg,
cameraInfoMsg,
scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::depthScan3dTFCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
commonDepthCallback(
nav_msgs::OdometryConstPtr(),
imageMsg,
depthMsg,
cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
scanMsg,
rtabmap_ros::OdomInfoConstPtr());
}
@@ -1163,6 +1308,25 @@ void GuiWrapper::stereoScanTFCallback(
leftCameraInfoMsg,
rightCameraInfoMsg,
scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::stereoScan3dTFCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
{
commonStereoCallback(
nav_msgs::OdometryConstPtr(),
leftImageMsg,
rightImageMsg,
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
scanMsg,
rtabmap_ros::OdomInfoConstPtr());
}
@@ -1180,6 +1344,7 @@ void GuiWrapper::stereoOdomInfoTFCallback(
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg);
}
@@ -1196,12 +1361,14 @@ void GuiWrapper::stereoTFCallback(
leftCameraInfoMsg,
rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeLaserScan2d,
bool subscribeLaserScan3d,
bool subscribeOdomInfo,
bool subscribeStereo,
int queueSize,
@@ -1212,9 +1379,11 @@ void GuiWrapper::setupCallbacks(
if(subscribeDepth && subscribeStereo)
{
ROS_WARN("\"subscribe_depth\" already true, ignoring \"subscribe_stereo\".");
ROS_WARN("rtabmapviz: Parameters subscribe_depth and subscribe_stereo cannot be true at the "
"same time. Parameter subscribe_depth is set to false.");
subscribeDepth = false;
}
if(!subscribeDepth && !subscribeStereo && subscribeLaserScan)
if(!subscribeDepth && !subscribeStereo && (subscribeLaserScan2d || subscribeLaserScan3d))
{
ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
}
@@ -1231,7 +1400,7 @@ void GuiWrapper::setupCallbacks(
if(subscribeDepth)
{
UASSERT(depthCameras >= 1 && depthCameras <= 2);
UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan2d || subscribeLaserScan3d || !odomFrameId_.empty()), "Not yet supported!");
imageSubs_.resize(depthCameras);
imageDepthSubs_.resize(depthCameras);
@@ -1265,25 +1434,95 @@ void GuiWrapper::setupCallbacks(
if(odomFrameId_.empty())
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeLaserScan)
if(subscribeLaserScan2d)
{
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
MyDepthScanSyncPolicy(queueSize),
scanSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
depthScanOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy>(
MyDepthScanOdomInfoSyncPolicy(queueSize),
odomInfoSub_,
scanSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
MyDepthScanSyncPolicy(queueSize),
scanSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
}
else if(subscribeLaserScan3d)
{
scan3dSub_.subscribe(nh, "scan_cloud", 1);
if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
depthScan3dOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy>(
MyDepthScan3dOdomInfoSyncPolicy(queueSize),
odomInfoSub_,
scan3dSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scan3dSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
MyDepthScan3dSyncPolicy(queueSize),
scan3dSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scan3dSub_.getTopic().c_str());
}
}
else if(subscribeOdomInfo)
{
@@ -1380,7 +1619,7 @@ void GuiWrapper::setupCallbacks(
else
{
// use TF as odom
if(subscribeLaserScan)
if(subscribeLaserScan2d)
{
scanSub_.subscribe(nh, "scan", 1);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
@@ -1398,6 +1637,24 @@ void GuiWrapper::setupCallbacks(
cameraInfoSubs_[0]->getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else if(subscribeLaserScan3d)
{
scan3dSub_.subscribe(nh, "scan_cloud", 1);
depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
MyDepthScan3dTFSyncPolicy(queueSize),
scan3dSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
scan3dSub_.getTopic().c_str());
}
else if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
@@ -1452,27 +1709,103 @@ void GuiWrapper::setupCallbacks(
if(odomFrameId_.empty())
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeLaserScan)
if(subscribeLaserScan2d)
{
scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
MyStereoScanSyncPolicy(queueSize),
scanSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
stereoScanOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy>(
MyStereoScanOdomInfoSyncPolicy(queueSize),
odomInfoSub_,
scanSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
MyStereoScanSyncPolicy(queueSize),
scanSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
}
else if(subscribeLaserScan3d)
{
scan3dSub_.subscribe(nh, "scan_cloud", 1);
if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
stereoScan3dOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy>(
MyStereoScan3dOdomInfoSyncPolicy(queueSize),
odomInfoSub_,
scan3dSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scan3dSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
stereoScan3dSync_ = new message_filters::Synchronizer<MyStereoScan3dSyncPolicy>(
MyStereoScan3dSyncPolicy(queueSize),
scan3dSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scan3dSub_.getTopic().c_str());
}
}
else if(subscribeOdomInfo)
{
@@ -1519,7 +1852,7 @@ void GuiWrapper::setupCallbacks(
else
{
//use odom TF
if(subscribeLaserScan)
if(subscribeLaserScan2d)
{
scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
@@ -1539,6 +1872,26 @@ void GuiWrapper::setupCallbacks(
cameraInfoRight_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else if(subscribeLaserScan3d)
{
scan3dSub_.subscribe(nh, "scan_cloud", 1);
stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
MyStereoScan3dTFSyncPolicy(queueSize),
scan3dSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
scan3dSub_.getTopic().c_str());
}
else if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
src/GuiWrapper.h
+133 -7
View File
@@ -68,8 +68,6 @@ public:
GuiWrapper(int & argc, char** argv);
virtual ~GuiWrapper();
int exec();
protected:
virtual void handleEvent(UEvent * anEvent);
@@ -80,7 +78,8 @@ private:
void setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeLaserScan2d,
bool subscribeLaserScan3d,
bool subscribeOdomInfo,
bool subscribeStereo,
int queueSize,
@@ -91,14 +90,16 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonStereoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -106,7 +107,8 @@ private:
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -146,6 +148,26 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depthScanOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depthScan3dCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depthScan3dOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void stereoScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -154,6 +176,29 @@ private:
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoScanOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoScan3dCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoScan3dOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -182,6 +227,11 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
void depthScan3dTFCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
void stereoScanTFCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -189,6 +239,12 @@ private:
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoScan3dTFCallback(
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoOdomInfoTFCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -205,7 +261,6 @@ private:
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private:
QApplication * app_;
rtabmap::MainWindow * mainWindow_;
std::string cameraNodeName_;
double lastOdomInfoUpdateTime_;
@@ -231,6 +286,7 @@ private:
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
@@ -256,6 +312,32 @@ private:
sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::LaserScan,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy;
message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy> * depthScanOdomInfoSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::PointCloud2,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy;
message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::PointCloud2,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy;
message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy> * depthScan3dOdomInfoSync_;
typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
sensor_msgs::Image,
@@ -288,6 +370,35 @@ private:
sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::LaserScan,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy;
message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy> * stereoScanOdomInfoSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::PointCloud2,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy;
message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::PointCloud2,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy;
message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy> * stereoScan3dOdomInfoSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
nav_msgs::Odometry,
@@ -326,6 +437,13 @@ private:
sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::PointCloud2,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy;
message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
@@ -354,6 +472,14 @@ private:
sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::PointCloud2,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy;
message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::Image,
src/MapAssemblerNode.cpp
+32 -251
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include "MapsManager.h"
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
@@ -49,54 +50,13 @@ class MapAssembler
public:
MapAssembler() :
cloudDecimation_(4),
cloudMaxDepth_(4.0),
cloudVoxelSize_(0.02),
scanVoxelSize_(0.01),
nodeFilteringAngle_(30), // degrees
nodeFilteringRadius_(0.5),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
computeOccupancyGrid_(false),
gridCellSize_(0.05),
groundMaxAngle_(M_PI_4),
clusterMinSize_(20),
maxHeight_(0),
occupancyMapSize_(0.0)
mapsManager_(false)
{
ros::NodeHandle pnh("~");
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("filter_radius", nodeFilteringRadius_, nodeFilteringRadius_);
pnh.param("filter_angle", nodeFilteringAngle_, nodeFilteringAngle_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("occupancy_grid", computeOccupancyGrid_, computeOccupancyGrid_);
pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_);
pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_);
pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_);
pnh.param("occupancy_max_height", maxHeight_, maxHeight_);
pnh.param("occupancy_map_size", occupancyMapSize_, occupancyMapSize_);
UASSERT(gridCellSize_ > 0);
UASSERT(maxHeight_ >= 0);
UASSERT(occupancyMapSize_ >=0.0);
ros::NodeHandle nh;
mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
assembledMapClouds_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_clouds", 1);
assembledMapScans_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_scans", 1);
if(computeOccupancyGrid_)
{
occupancyMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_projection_map", 1);
}
// private service
resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
}
@@ -108,239 +68,60 @@ public:
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
UTimer timer;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
Transform mapOdom;
rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
int id = msg->nodes[i].id;
if(!uContains(rgbClouds_, id))
if(msg->nodes[i].image.size() ||
msg->nodes[i].depth.size() ||
msg->nodes[i].laserScan.size())
{
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
{
cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0);
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloudDecimation_,
cloudMaxDepth_);
if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indices, *tmp);
cloud = tmp;
}
if(cloud->size() && cloudVoxelSize_ > 0)
{
cloud = util3d::voxelize(cloud, cloudVoxelSize_);
}
if(cloud->size())
{
rgbClouds_.insert(std::make_pair(id, cloud));
if(computeOccupancyGrid_)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(cloudClipped->size() && maxHeight_ > 0)
{
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
}
if(cloudClipped->size())
{
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
cv::Mat ground, obstacles;
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_);
if(!ground.empty() || !obstacles.empty())
{
occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles)));
}
}
}
}
}
}
}
if(!uContains(scans_, id) && msg->nodes[i].laserScan.size())
{
cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
if(!laserScan.empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
if(cloud->size() && scanVoxelSize_ > 0)
{
cloud = util3d::voxelize(cloud, scanVoxelSize_);
}
if(cloud->size())
{
scans_.insert(std::make_pair(id, cloud));
}
}
uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
}
}
// filter poses
std::map<int, Transform> poses;
UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
// create a tmp signature with latest sensory data
if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
{
poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
}
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
{
poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
Signature tmpS = nodes_.at(poses.rbegin()->first);
SensorData tmpData = tmpS.sensorData();
tmpData.setId(-1);
uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
poses.insert(std::make_pair(-1, poses.rbegin()->second));
}
if(assembledMapClouds_.getNumSubscribers())
{
// generate the assembled cloud!
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// Update maps
poses = mapsManager_.updateMapCaches(
poses,
0,
false,
false,
false,
false,
nodes_);
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = rgbClouds_.find(iter->first);
if(jter != rgbClouds_.end())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
if(assembledCloud->size())
{
if(cloudVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*assembledCloud, *cloudMsg);
cloudMsg->header.stamp = ros::Time::now();
cloudMsg->header.frame_id = msg->header.frame_id;
assembledMapClouds_.publish(cloudMsg);
}
}
if(assembledMapScans_.getNumSubscribers())
{
// generate the assembled scan!
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
if(jter != scans_.end())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
if(assembledCloud->size())
{
if(scanVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*assembledCloud, *cloudMsg);
cloudMsg->header.stamp = ros::Time::now();
cloudMsg->header.frame_id = msg->header.frame_id;
assembledMapScans_.publish(cloudMsg);
}
}
if(occupancyMapPub_.getNumSubscribers())
{
// create the map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
occupancyLocalMaps_,
gridCellSize_, xMin, yMin,
occupancyMapSize_);
if(!pixels.empty())
{
//init
nav_msgs::OccupancyGrid map;
map.info.resolution = gridCellSize_;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = msg->header.frame_id;
map.header.stamp = ros::Time::now();
occupancyMapPub_.publish(map);
}
}
ROS_INFO("Processing data %fs", timer.ticks());
ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
}
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("map_assembler: reset!");
occupancyLocalMaps_.clear();
rgbClouds_.clear();
scans_.clear();
mapsManager_.clear();
return true;
}
private:
int cloudDecimation_;
double cloudMaxDepth_;
double cloudVoxelSize_;
double scanVoxelSize_;
double nodeFilteringAngle_;
double nodeFilteringRadius_;
double noiseFilterRadius_;
double noiseFilterMinNeighbors_;
bool computeOccupancyGrid_;
double gridCellSize_;
double groundMaxAngle_;
int clusterMinSize_;
double maxHeight_;
double occupancyMapSize_;
std::map<int, std::pair<cv::Mat, cv::Mat> > occupancyLocalMaps_; // <ground, obstacles>
MapsManager mapsManager_;
std::map<int, Signature> nodes_;
ros::Subscriber mapDataTopic_;
ros::Publisher assembledMapClouds_;
ros::Publisher assembledMapScans_;
ros::Publisher occupancyMapPub_;
ros::ServiceServer resetService_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > rgbClouds_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
};
src/MapOptimizerNode.cpp
+79 -23
View File
@@ -31,9 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <tf2_ros/transform_broadcaster.h>
@@ -48,8 +50,6 @@ public:
MapOptimizer() :
mapFrameId_("map"),
odomFrameId_("odom"),
iterations_(100),
ignoreVariance_(false),
globalOptimization_(true),
optimizeFromLastNode_(false),
mapToOdom_(rtabmap::Transform::getIdentity()),
@@ -58,14 +58,35 @@ public:
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
double epsilon = 0.0;
bool robust = true;
bool slam2d =false;
int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
int iterations = 100;
bool ignoreVariance = false;
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("iterations", iterations_, iterations_);
pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_);
pnh.param("iterations", iterations, iterations);
pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
pnh.param("global_optimization", globalOptimization_, globalOptimization_);
pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
pnh.param("epsilon", epsilon, epsilon);
pnh.param("robust", robust, robust);
pnh.param("slam_2d", slam2d, slam2d);
pnh.param("strategy", strategy, strategy);
UASSERT(iterations_ > 0);
UASSERT(iterations > 0);
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy)));
parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
parameters.insert(ParametersPair(Parameters::kOptimizerSlam2D(), uBool2Str(slam2d)));
parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
optimizer_ = Optimizer::create(parameters);
double tfDelay = 0.05; // 20 Hz
bool publishTf = true;
@@ -137,8 +158,10 @@ public:
if(iter->second.to() == link.to())
{
edgeAlreadyAdded = true;
if(iter->second.transform() != link.transform())
if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001)
{
ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(),
link.transform().prettyPrint().c_str());
dataChanged = true;
}
}
@@ -149,16 +172,17 @@ public:
}
}
std::map<int, Transform> newPoses;
std::map<int, Signature> newNodeInfos;
// add new odometry poses
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
int id = msg->nodes[i].id;
Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
newPoses.insert(std::make_pair(id, pose));
Signature s = rtabmap_ros::nodeInfoFromROS(msg->nodes[i]);
newNodeInfos.insert(std::make_pair(id, s));
std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
if(!p.second && pose != cachedPoses_.at(id))
std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
{
dataChanged = true;
}
@@ -167,27 +191,27 @@ public:
if(dataChanged)
{
ROS_WARN("Graph data has changed! Reset cache...");
cachedPoses_ = newPoses;
cachedConstraints_ = newConstraints;
cachedNodeInfos_ = newNodeInfos;
}
//match poses in the graph
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, Signature> nodeInfos;
if(globalOptimization_)
{
poses = cachedPoses_;
constraints = cachedConstraints_;
nodeInfos = cachedNodeInfos_;
}
else
{
constraints = newConstraints;
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
{
std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.posesId[i]);
if(iter != cachedPoses_.end())
std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
if(iter != cachedNodeInfos_.end())
{
poses.insert(*iter);
nodeInfos.insert(*iter);
}
else
{
@@ -196,25 +220,31 @@ public:
}
}
}
std::map<int, Transform> poses;
for(std::map<int, Signature>::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter)
{
poses.insert(std::make_pair(iter->first, iter->second.getPose()));
}
// Optimize only if there is a subscriber
if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{
UTimer timer;
std::map<int, Transform> optimizedPoses;
Transform mapCorrection = Transform::getIdentity();
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
if(poses.size() > 1 && constraints.size() > 0)
{
graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
std::map<int, rtabmap::Transform> posesOut;
optimizer.getConnectedGraph(
optimizer_->getConnectedGraph(
fromId,
poses,
constraints,
posesOut,
linksOut);
optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut);
optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
mapToOdomMutex_.lock();
mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
mapToOdom_ = mapCorrection;
@@ -249,6 +279,33 @@ public:
outputDataMsg.header = msg->header;
outputDataMsg.graph = outputGraphMsg;
outputDataMsg.nodes = msg->nodes;
if(posesOut.size() > msg->nodes.size())
{
std::set<int> addedNodes;
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
addedNodes.insert(msg->nodes[i].id);
}
std::list<int> toAdd;
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{
if(addedNodes.find(iter->first) == addedNodes.end())
{
toAdd.push_back(iter->first);
}
}
if(toAdd.size())
{
int oi = outputDataMsg.nodes.size();
outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size());
for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
{
UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
rtabmap_ros::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
++oi;
}
}
}
mapDataPub_.publish(outputDataMsg);
}
@@ -259,10 +316,9 @@ public:
private:
std::string mapFrameId_;
std::string odomFrameId_;
int iterations_;
bool ignoreVariance_;
bool globalOptimization_;
bool optimizeFromLastNode_;
Optimizer * optimizer_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
@@ -272,8 +328,8 @@ private:
ros::Publisher mapDataPub_;
ros::Publisher mapGraphPub_;
std::map<int, Transform> cachedPoses_;
std::multimap<int, Link> cachedConstraints_;
std::map<int, Signature> cachedNodeInfos_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
boost::thread* transformThread_;
src/MapsManager.cpp
+252 -39
View File
@@ -29,23 +29,34 @@
using namespace rtabmap;
MapsManager::MapsManager() :
MapsManager::MapsManager(bool usePublicNamespace) :
cloudDecimation_(4),
cloudMaxDepth_(4.0), // meters
cloudMinDepth_(0.0), // meters
cloudVoxelSize_(0.05), // meters
cloudFloorCullingHeight_(0.0),
cloudCeilingCullingHeight_(0.0),
cloudOutputVoxelized_(false),
cloudFrustumCulling_(false),
cloudNoiseFilteringRadius_(0.0),
cloudNoiseFilteringMinNeighbors_(5),
scanDecimation_(0),
scanVoxelSize_(0.0),
scanOutputVoxelized_(false),
projMaxGroundAngle_(45.0), // degrees
projMinClusterSize_(20),
projMaxHeight_(2.0), // meters
projMaxObstaclesHeight_(2.0), // meters (<=0 disabled)
projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true)
projDetectFlatObstacles_(false),
gridCellSize_(0.05), // meters
gridSize_(0), // meters
gridEroded_(false),
gridUnknownSpaceFilled_(false),
gridMaxUnknownSpaceFilledRange_(6.0),
mapFilterRadius_(0.5),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true)
mapCacheCleanup_(true),
negativePosesIgnored(false)
{
ros::NodeHandle nh;
@@ -54,31 +65,82 @@ MapsManager::MapsManager() :
// cloud map stuff
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_);
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_);
if(cloudFloorCullingHeight_ > 0 &&
cloudCeilingCullingHeight_ > 0 &&
cloudCeilingCullingHeight_ < cloudFloorCullingHeight_)
{
ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled).");
cloudCeilingCullingHeight_ = 0;
}
pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_);
pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_);
pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_);
// scan map stuff
pnh.param("scan_decimation", scanDecimation_, scanDecimation_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_);
//projection map stuff
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_);
pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_);
if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height"))
{
ROS_WARN("Parameter \"proj_max_height\" has been renamed "
"to \"proj_max_obstacles_height\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_);
}
else
{
pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_);
}
pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_);
pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_);
// common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
if(gridCellSize_ <= 0)
{
ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_);
}
pnh.param("grid_size", gridSize_, gridSize_); // m
pnh.param("grid_eroded", gridEroded_, gridEroded_);
pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
// common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
pnh.param("map_negative_poses_ignored", negativePosesIgnored, negativePosesIgnored);
// If true, the last message published on
// the map topics will be saved and sent to new subscribers when they
// connect
bool latch = true;
pnh.param("latch", latch, latch);
// mapping topics
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
if(usePublicNamespace)
{
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
scanMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
}
else
{
cloudMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
projMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
gridMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
scanMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
}
}
MapsManager::~MapsManager() {
@@ -97,7 +159,8 @@ bool MapsManager::hasSubscribers() const
{
return cloudMapPub_.getNumSubscribers() != 0 ||
projMapPub_.getNumSubscribers() != 0 ||
gridMapPub_.getNumSubscribers() != 0;
gridMapPub_.getNumSubscribers() != 0 ||
scanMapPub_.getNumSubscribers() != 0;
}
std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
@@ -117,32 +180,35 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
bool updateCloud,
bool updateProj,
bool updateGrid,
bool updateScan,
const std::map<int, rtabmap::Signature> & signatures)
{
if(!updateCloud && !updateProj && !updateGrid)
if(!updateCloud && !updateProj && !updateGrid && !updateScan)
{
// all false, udpate only those where we have subscribers
updateCloud = cloudMapPub_.getNumSubscribers() != 0;
updateProj = projMapPub_.getNumSubscribers() != 0;
updateGrid = gridMapPub_.getNumSubscribers() != 0;
updateScan = scanMapPub_.getNumSubscribers() != 0;
}
UDEBUG("Updating map caches...");
if(!memory && signatures.size() == 0)
{
ROS_FATAL("Memory should not be null!?");
ROS_ERROR("Memory and signatures should not be both null!?");
return std::map<int, rtabmap::Transform>();
}
std::map<int, rtabmap::Transform> filteredPoses;
// update cache
if(updateCloud || updateProj || updateGrid)
if(updateCloud || updateProj || updateGrid || updateScan)
{
// filter nodes
if(mapFilterRadius_ > 0.0)
{
UDEBUG("Filter nodes...");
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -163,6 +229,21 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
filteredPoses = poses;
}
if(negativePosesIgnored)
{
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();)
{
if(iter->first <= 0)
{
filteredPoses.erase(iter++);
}
else
{
++iter;
}
}
}
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{
if(!iter->second.isNull())
@@ -170,12 +251,15 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
rtabmap::SensorData data;
bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first));
bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first));
bool scanRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first));
if(rgbDepthRequired ||
depthRequired ||
scanRequired)
scanRequired ||
gridRequired)
{
UDEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
if(findIter != signatures.end())
{
@@ -191,26 +275,40 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
{
if(!(data.imageCompressed().empty() && data.imageRaw().empty()) &&
!(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) &&
(data.cameraModels().size() || data.stereoCameraModel().isValid()))
(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection()))
{
// Which data should we decompress?
cv::Mat image, depth, scan;
data.uncompressData(
(rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0,
(rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0,
(rgbDepthRequired||depthRequired) ? &depth:0,
scanRequired?&scan:0);
scanRequired||gridRequired?&scan:0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
if(rgbDepthRequired)
{
UDEBUG("rgbDepthRequired");
if(!image.empty() && !depth.empty())
{
pcl::IndicesPtr validIndices(new std::vector<int>);
cloudRGB = util3d::cloudRGBFromSensorData(
data,
cloudDecimation_,
cloudMaxDepth_,
cloudVoxelSize_);
cloudMinDepth_,
validIndices.get());
if(cloudVoxelSize_)
{
cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_);
}
if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloudRGB, *indices, *tmp);
cloudRGB = tmp;
}
}
else
{
@@ -219,13 +317,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
else if(depthRequired)
{
UDEBUG("depthRequired");
if( !depth.empty())
{
pcl::IndicesPtr validIndices(new std::vector<int>);
cloudXYZ = util3d::cloudFromSensorData(
data,
cloudDecimation_,
cloudMaxDepth_,
gridCellSize_); // use gridCellSize since this cloud is only for the projection map
cloudMinDepth_,
validIndices.get()); // use gridCellSize since this cloud is only for the projection map
UASSERT(gridCellSize_ > 0);
cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_);
if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloudXYZ, *indices, *tmp);
cloudXYZ = tmp;
}
}
else
{
@@ -240,7 +350,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// Make sure that image size is set in camera models.
// The camera models are used when cloud_frustum_culling=true.
std::vector<rtabmap::CameraModel> models;
if(data.stereoCameraModel().isValid())
if(data.stereoCameraModel().isValidForProjection())
{
//insert only the left camera model
rtabmap::CameraModel model = data.stereoCameraModel().left();
@@ -265,40 +375,90 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(depthRequired)
{
UDEBUG("Creating proj map for %d...", iter->first);
cv::Mat ground, obstacles;
if(cloudRGB.get())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
if(cloudClipped->size() && projMaxHeight_ > 0)
if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
{
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxObstaclesHeight_);
}
if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_)
{
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
}
if(cloudClipped->size())
{
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
// add pose rotation without yaw
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
}
}
else if(cloudXYZ.get())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxHeight_ > 0)
if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
{
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxObstaclesHeight_);
}
if(cloudClipped->size())
{
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
// add pose rotation without yaw
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
UDEBUG("util3d::occupancy2DFromCloud3D()");
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
}
}
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
if(scanRequired)
if(scanRequired || gridRequired)
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
{
if(scanDecimation_ > 1)
{
scan = util3d::downsample(scan, scanDecimation_);
}
if(scanRequired || scanVoxelSize_ > 0.0)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
if(scanVoxelSize_ > 0.0)
{
scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_);
if(gridRequired && scan.type() == CV_32FC2)
{
scan = util3d::laserScan2dFromPointCloud(*scanCloud);
}
}
if(scanRequired)
{
uInsert(scans_, std::make_pair(iter->first, scanCloud));
}
}
}
if(gridRequired && scan.type() == CV_32FC2)
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(
scan,
ground,
obstacles,
gridCellSize_,
data.id() < 0 || gridUnknownSpaceFilled_,
data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange());
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
}
}
else
@@ -307,7 +467,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
iter->first,
!(data.imageCompressed().empty() && data.imageRaw().empty())?1:0,
!(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0,
(data.cameraModels().size() || data.stereoCameraModel().isValid())?1:0);
(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0);
}
}
}
@@ -318,6 +478,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
// cleanup not used nodes
UDEBUG("Cleanup not used nodes");
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
iter!=clouds_.end();)
{
@@ -416,32 +577,37 @@ void MapsManager::publishMaps(
{
for(unsigned int i=0; i<kter->second.size(); ++i)
{
if(kter->second[i].isValid())
if(kter->second[i].isValidForProjection())
{
int size = assembledCloud->size();
assembledCloud = util3d::frustumFiltering(
assembledCloud,
iter->second,
iter->second, // FIXME: should include camera local transform
kter->second[i].horizontalFOV(),
kter->second[i].verticalFOV(),
0.0f,
cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
true);
//ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
if(jter->second->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
}
}
}
}
if(cloudFloorCullingHeight_ > 0.0)
if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0))
{
assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f);
assembledCloud = util3d::passThrough(assembledCloud, "z",
cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0,
cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0);
}
if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
{
assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
}
@@ -456,7 +622,7 @@ void MapsManager::publishMaps(
}
else if(poses.size())
{
ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size());
ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size());
}
}
else if(mapCacheCleanup_)
@@ -465,6 +631,53 @@ void MapsManager::publishMaps(
cameraModels_.clear();
}
if(scanMapPub_.getNumSubscribers())
{
// generate the assembled scan cloud!
UTimer time;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
int count = 0;
std::list<std::pair<int, Transform> > negativePoses;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
if(jter != scans_.end() && jter->second->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
++count;
}
}
// negative poses are not used
}
if(assembledCloud->size())
{
if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_)
{
assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
}
ROS_INFO("Assembled %d scans (%fs)", count, time.ticks());
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*assembledCloud, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
scanMapPub_.publish(cloudMsg);
}
else if(poses.size())
{
ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size());
}
}
else if(mapCacheCleanup_)
{
scans_.clear();
}
if(projMapPub_.getNumSubscribers())
{
// create the projection map
src/MapsManager.h
+16 -2
View File
@@ -26,7 +26,7 @@ class Memory;
class MapsManager {
public:
MapsManager();
MapsManager(bool usePublicNamespace);
virtual ~MapsManager();
void clear();
bool hasSubscribers() const;
@@ -40,6 +40,7 @@ public:
bool updateCloud,
bool updateProj,
bool updateGrid,
bool updateScan,
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
void publishMaps(
@@ -67,26 +68,39 @@ private:
// mapping stuff
int cloudDecimation_;
double cloudMaxDepth_;
double cloudMinDepth_;
double cloudVoxelSize_;
double cloudFloorCullingHeight_;
double cloudCeilingCullingHeight_;
bool cloudOutputVoxelized_;
bool cloudFrustumCulling_;
double cloudNoiseFilteringRadius_;
int cloudNoiseFilteringMinNeighbors_;
int scanDecimation_;
double scanVoxelSize_;
bool scanOutputVoxelized_;
double projMaxGroundAngle_;
int projMinClusterSize_;
double projMaxHeight_;
double projMaxObstaclesHeight_;
double projMaxGroundHeight_;
bool projDetectFlatObstacles_;
double gridCellSize_;
double gridSize_;
bool gridEroded_;
bool gridUnknownSpaceFilled_;
double gridMaxUnknownSpaceFilledRange_;
double mapFilterRadius_;
double mapFilterAngle_;
bool mapCacheCleanup_;
bool negativePosesIgnored;
ros::Publisher cloudMapPub_;
ros::Publisher projMapPub_;
ros::Publisher gridMapPub_;
ros::Publisher scanMapPub_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
src/MsgConversion.cpp
+146 -10
View File
@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl_conversions/pcl_conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
namespace rtabmap_ros {
@@ -144,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
// rtabmap_ros::Info
stat.setRefImageId(info.refId);
stat.setLoopClosureId(info.loopClosureId);
stat.setLocalLoopClosureId(info.localLoopClosureId);
stat.setProximityDetectionId(info.proximityDetectionId);
stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
@@ -188,7 +190,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
{
info.refId = stats.refImageId();
info.loopClosureId = stats.loopClosureId();
info.localLoopClosureId = stats.localLoopClosureId();
info.proximityDetectionId = stats.proximityDetectionId();
rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
@@ -293,6 +295,103 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
}
}
cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg)
{
return cv::Point3f(msg.x, msg.y, msg.z);
}
void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg)
{
msg.x = kpt.x;
msg.y = kpt.y;
msg.z = kpt.z;
}
std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg)
{
std::vector<cv::Point3f> v(msg.size());
for(unsigned int i=0; i<msg.size(); ++i)
{
v[i] = point3fFromROS(msg[i]);
}
return v;
}
void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg)
{
msg.resize(kpts.size());
for(unsigned int i=0; i<msg.size(); ++i)
{
point3fToROS(kpts[i], msg[i]);
}
}
rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo,
const rtabmap::Transform & localTransform)
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(camInfo);
return rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform,
0.0,
cv::Size(model.fullResolution().width, model.fullResolution().height));
}
void cameraModelToROS(
const rtabmap::CameraModel & model,
sensor_msgs::CameraInfo & camInfo)
{
UASSERT(model.isValidForRectification());
camInfo.D = std::vector<double>(model.D_raw().cols);
memcpy(camInfo.D.data(), model.D_raw().data, model.D_raw().cols*sizeof(double));
UASSERT(model.K_raw().total() == 9);
memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double));
UASSERT(model.R().total() == 9);
memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double));
UASSERT(model.P().total() == 12);
memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double));
if(camInfo.D.size() > 5)
{
camInfo.distortion_model = "rational_polynomial";
}
else
{
camInfo.distortion_model = "plumb_bob";
}
camInfo.binning_x = 1;
camInfo.binning_y = 1;
camInfo.roi.width = model.imageWidth();
camInfo.roi.height = model.imageHeight();
camInfo.width = model.imageWidth();
camInfo.height = model.imageHeight();
}
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const rtabmap::Transform & localTransform)
{
image_geometry::StereoCameraModel model;
model.fromCameraInfo(leftCamInfo, rightCamInfo);
return rtabmap::StereoCameraModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform,
cv::Size(model.left().fullResolution().width, model.left().fullResolution().height));
}
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses,
@@ -384,13 +483,14 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
{
//Features stuff...
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
std::multimap<int, cv::Point3f> words3D;
pcl::PointCloud<pcl::PointXYZ> cloud;
if(msg.wordPts.data.size() &&
msg.wordPts.data.size() == msg.wordIds.size())
msg.wordPts.height*msg.wordPts.width == msg.wordIds.size())
{
pcl::fromROSMsg(msg.wordPts, cloud);
}
for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
{
cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(i));
@@ -398,7 +498,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
words.insert(std::make_pair(wordId, pt));
if(i< cloud.size())
{
words3D.insert(std::make_pair(wordId, cloud[i]));
words3D.insert(std::make_pair(wordId, cv::Point3f(cloud[i].x, cloud[i].y, cloud[i].z)));
}
}
@@ -455,7 +555,8 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
msg.stamp,
msg.label,
transformFromPoseMsg(msg.pose),
stereoModel.isValid()?
transformFromPoseMsg(msg.groundTruthPose),
stereoModel.isValidForProjection()?
rtabmap::SensorData(
compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
@@ -489,6 +590,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.stamp = signature.getStamp();
msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
@@ -512,7 +614,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
}
}
else if(signature.sensorData().stereoCameraModel().isValid())
else if(signature.sensorData().stereoCameraModel().isValidForProjection())
{
msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
@@ -539,11 +641,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(signature.getWords3().size());
index = 0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=signature.getWords3().begin();
for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().begin();
jter!=signature.getWords3().end();
++jter)
{
cloud[index++] = jter->second;
cloud[index].x = jter->second.x;
cloud[index].y = jter->second.y;
cloud[index++].z = jter->second.z;
}
pcl::toROSMsg(cloud, msg.wordPts);
}
@@ -555,6 +659,30 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
}
}
rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg)
{
rtabmap::Signature s(
msg.id,
msg.mapId,
msg.weight,
msg.stamp,
msg.label,
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.groundTruthPose));
return s;
}
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
{
// add data
msg.id = signature.id();
msg.mapId = signature.mapId();
msg.weight = signature.getWeight();
msg.stamp = signature.getStamp();
msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
}
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
{
rtabmap::OdometryInfo info;
@@ -588,6 +716,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
{
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
return info;
}
@@ -605,7 +739,6 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.interval = info.interval;
msg.distanceTravelled = info.distanceTravelled;
msg.type = info.type;
msg.wordsKeys = uKeys(info.words);
@@ -621,6 +754,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
transformToGeometryMsg(info.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
msg.localMapKeys = uKeys(info.localMap);
points3fToROS(uValues(info.localMap), msg.localMapValues);
}
}
src/OdometryROS.cpp
+180 -122
View File
@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/OdometryF2M.h>
#include <rtabmap/core/OdometryF2F.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
@@ -48,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UFile.h"
#define BAD_COVARIANCE 9999
using namespace rtabmap;
namespace rtabmap_ros {
@@ -60,7 +63,10 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
publishTf_(true),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
paused_(false)
publishNullWhenLost_(true),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0)
{
ros::NodeHandle nh;
@@ -84,6 +90,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("config_path", configPath, configPath);
pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
{
@@ -124,14 +132,14 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
//parameters
parameters_ = this->getDefaultOdometryParameters(stereo);
parameters_ = Parameters::getDefaultOdometryParameters(stereo);
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
{
ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str());
rtabmap::ParametersMap allParameters;
Rtabmap::readParameters(configPath.c_str(), allParameters);
Parameters::readINI(configPath.c_str(), allParameters);
// only update odometry parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
@@ -174,78 +182,58 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
iter->second = uNumber2Str(vInt);
}
if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first);
if(jter!=parameters_.end())
{
ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
jter->second = iter->second;
}
}
// Backward compatibility
std::list<std::string> oldParameterNames;
oldParameterNames.push_back("Odom/Type");
oldParameterNames.push_back("Odom/MaxWords");
oldParameterNames.push_back("Odom/WordsRatio");
oldParameterNames.push_back("Odom/LocalHistory");
oldParameterNames.push_back("Odom/NearestNeighbor");
oldParameterNames.push_back("Odom/NNDR");
oldParameterNames.push_back("GFTT/MaxCorners");
for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
iter!=Parameters::getRemovedParameters().end();
++iter)
{
std::string vStr;
if(pnh.getParam(*iter, vStr))
if(pnh.getParam(iter->first, vStr))
{
if(iter->compare("Odom/Type") == 0)
if(iter->second.first)
{
ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.",
Parameters::kOdomFeatureType().c_str());
parameters_.at(Parameters::kOdomFeatureType())= vStr;
// can be migrated
parameters_.at(iter->second.second)= vStr;
ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
}
else if(iter->compare("Odom/MaxWords") == 0)
else
{
ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.",
Parameters::kOdomMaxFeatures().c_str());
parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
}
else if(iter->compare("Odom/LocalHistory") == 0)
{
ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.",
Parameters::kOdomBowLocalHistorySize().c_str());
parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr;
}
else if(iter->compare("Odom/NearestNeighbor") == 0)
{
ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.",
Parameters::kOdomBowNNType().c_str());
parameters_.at(Parameters::kOdomBowNNType())= vStr;
}
else if(iter->compare("Odom/NNDR") == 0)
{
ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.",
Parameters::kOdomBowNNDR().c_str());
parameters_.at(Parameters::kOdomBowNNDR())= vStr;
}
else if(iter->compare("GFTT/MaxCorners") == 0)
{
ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.",
Parameters::kOdomMaxFeatures().c_str());
parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
if(iter->second.second.empty())
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!",
iter->first.c_str());
}
else
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
iter->first.c_str(), iter->second.second.c_str());
}
}
}
}
int odomStrategy = 0; // BOW
Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
if(odomStrategy == 1)
{
ROS_INFO("Using OdometryOpticalFlow");
odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
}
else
{
ROS_INFO("Using OdometryBOW");
odometry_ = new rtabmap::OdometryBOW(parameters_);
}
Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
odometry_ = Odometry::create(parameters_);
if(!initialPose.isIdentity())
{
odometry_->reset(initialPose);
@@ -255,6 +243,11 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this);
setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this);
setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this);
setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this);
}
OdometryROS::~OdometryROS()
@@ -268,48 +261,13 @@ OdometryROS::~OdometryROS()
delete odometry_;
}
rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo)
{
rtabmap::ParametersMap odomParameters;
rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(uStrContains(group, "Odom") ||
group.compare("Stereo") ||
group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FAST") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
if(stereo)
{
if(iter->first.compare(Parameters::kOdomMaxDepth()) == 0)
{
iter->second = "0"; // infinity
}
else if(iter->first.compare(Parameters::kOdomEstimationType()) == 0)
{
iter->second = "1"; // 3D->2D (PNP)
}
}
odomParameters.insert(*iter);
}
}
return odomParameters;
}
void OdometryROS::processArguments(int argc, char * argv[], bool stereo)
{
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parametersOdom = getDefaultOdometryParameters(stereo);
rtabmap::ParametersMap parametersOdom = Parameters::getDefaultOdometryParameters(stereo);
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
@@ -325,6 +283,14 @@ void OdometryROS::processArguments(int argc, char * argv[], bool stereo)
"argument \"--params\" is detected!");
exit(0);
}
else if(strcmp(argv[i], "--udebug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
}
else if(strcmp(argv[i], "--uinfo") == 0)
{
ULogger::setLevel(ULogger::kInfo);
}
}
}
@@ -378,9 +344,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
// process data
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(data, &info);
SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
if(!pose.isNull())
{
resetCurrentCount_ = resetCountdown_;
//*********************
// Update odometry
//*********************
@@ -410,24 +379,47 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.pose.pose.orientation = poseMsg.transform.rotation;
//set covariance
odom.pose.covariance.at(0) = info.variance; // xx
odom.pose.covariance.at(7) = info.variance; // yy
odom.pose.covariance.at(14) = info.variance; // zz
odom.pose.covariance.at(21) = info.variance; // rr
odom.pose.covariance.at(28) = info.variance; // pp
odom.pose.covariance.at(35) = info.variance; // yawyaw
// libviso2 uses approximately vel variance * 2
odom.pose.covariance.at(0) = info.variance*2; // xx
odom.pose.covariance.at(7) = info.variance*2; // yy
odom.pose.covariance.at(14) = info.variance*2; // zz
odom.pose.covariance.at(21) = info.variance*2; // rr
odom.pose.covariance.at(28) = info.variance*2; // pp
odom.pose.covariance.at(35) = info.variance*2; // yawyaw
//set velocity
bool setTwist = !odometry_->previousVelocityTransform().isNull();
if(setTwist)
{
float x,y,z,roll,pitch,yaw;
odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odom.twist.twist.linear.x = x;
odom.twist.twist.linear.y = y;
odom.twist.twist.linear.z = z;
odom.twist.twist.angular.x = roll;
odom.twist.twist.angular.y = pitch;
odom.twist.twist.angular.z = yaw;
}
odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw
//publish the message
odomPub_.publish(odom);
}
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryBOW*>(odometry_))
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
{
const std::map<int, pcl::PointXYZ> & map = ((OdometryBOW*)odometry_)->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::map<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{
cloud.push_back(iter->second);
cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
@@ -438,18 +430,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(odomLastFrame_.getNumSubscribers())
{
if(dynamic_cast<OdometryBOW*>(odometry_))
if(dynamic_cast<OdometryF2M*>(odometry_))
{
const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature();
if(s)
const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size())
{
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pt);
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::PointCloud2 cloudMsg;
@@ -461,14 +452,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
else
{
//Optical flow
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D();
if(cloud->size())
//Frame to Frame
const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
if(refFrame.getWords3().size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
cloudTransformed = util3d::transformPointCloud(cloud, pose);
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
{
// transform to odom frame
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg);
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
@@ -476,7 +472,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
}
}
else
else if(publishNullWhenLost_)
{
//ROS_WARN("Odometry lost!");
@@ -485,11 +481,47 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx
odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy
odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
//publish the message
odomPub_.publish(odom);
}
if(pose.isNull() && resetCurrentCount_ > 0)
{
ROS_WARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
--resetCurrentCount_;
if(resetCurrentCount_ == 0)
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp);
if(tfPose.isNull())
{
ROS_WARN("Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
}
else
{
ROS_WARN("Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
}
}
}
if(odomInfoPub_.getNumSubscribers())
{
rtabmap_ros::OdomInfo infoMsg;
@@ -502,9 +534,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
}
bool OdometryROS::isOdometryBOW() const
bool OdometryROS::isOdometryF2M() const
{
return dynamic_cast<OdometryBOW*>(odometry_) != 0;
return dynamic_cast<OdometryF2M*>(odometry_) != 0;
}
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -550,4 +582,30 @@ bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
return true;
}
bool OdometryROS::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Debug");
ULogger::setLevel(ULogger::kDebug);
return true;
}
bool OdometryROS::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Info");
ULogger::setLevel(ULogger::kInfo);
return true;
}
bool OdometryROS::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Warning");
ULogger::setLevel(ULogger::kWarning);
return true;
}
bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Error");
ULogger::setLevel(ULogger::kError);
return true;
}
}
src/OdometryROS.h
+12 -2
View File
@@ -49,7 +49,6 @@ namespace rtabmap_ros {
class OdometryROS
{
public:
static rtabmap::ParametersMap getDefaultOdometryParameters(bool stereo = false);
static void processArguments(int argc, char * argv[], bool stereo = false);
public:
@@ -61,13 +60,17 @@ public:
bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
const std::string & frameId() const {return frameId_;}
const std::string & odomFrameId() const {return odomFrameId_;}
const rtabmap::ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;}
bool isOdometryBOW() const;
bool isOdometryF2M() const;
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private:
@@ -80,6 +83,7 @@ private:
bool publishTf_;
bool waitForTransform_;
double waitForTransformDuration_;
bool publishNullWhenLost_;
rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_;
@@ -90,10 +94,16 @@ private:
ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
ros::ServiceServer setLogDebugSrv_;
ros::ServiceServer setLogInfoSrv_;
ros::ServiceServer setLogWarnSrv_;
ros::ServiceServer setLogErrorSrv_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
bool paused_;
int resetCountdown_;
int resetCurrentCount_;
};
}
src/PreferencesDialogROS.cpp
+71 -48
View File
@@ -27,15 +27,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "PreferencesDialogROS.h"
#include <rtabmap/core/Parameters.h>
#include <QtCore/QDir>
#include <QtCore/QFileInfo>
#include <QtCore/QSettings>
#include <QtGui/QHBoxLayout>
#include <QtCore/QTimer>
#include <QtGui/QLabel>
#include <QDir>
#include <QFileInfo>
#include <QSettings>
#include <QHBoxLayout>
#include <QTimer>
#include <QLabel>
#include <rtabmap/core/RtabmapEvent.h>
#include <QtGui/QMessageBox>
#include <QMessageBox>
#include <ros/exceptions.h>
#include <rtabmap/utilite/UStl.h>
using namespace rtabmap;
@@ -76,19 +77,42 @@ QString PreferencesDialogROS::getParamMessage()
bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
{
if(filePath.isEmpty() || filePath.compare(getTmpIniFilePath()) == 0)
QString path = getIniFilePath();
if(!filePath.isEmpty())
{
ros::NodeHandle nh;
ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
bool validParameters = true;
int readCount = 0;
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
path = filePath;
}
ros::NodeHandle nh;
ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
bool validParameters = true;
int readCount = 0;
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0)
{
// use working directory of the GUI, not the one on rosparam server
QSettings settings(path, QSettings::IniFormat);
settings.beginGroup("Core");
QString value = settings.value(rtabmap::Parameters::kRtabmapWorkingDirectory().c_str(), "").toString();
if(!value.isEmpty() && QDir(value).exists())
{
this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), value.toStdString());
}
else
{
// use default one
this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), (QDir::homePath()+"/.ros").toStdString());
}
settings.endGroup();
}
else
{
std::string value;
if(nh.getParam((*i).first,value))
if(nh.getParam(i->first,value))
{
PreferencesDialog::setParameter((*i).first, value);
PreferencesDialog::setParameter(i->first, value);
++readCount;
}
else
@@ -96,51 +120,50 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
validParameters = false;
}
}
}
ROS_INFO("Parameters read = %d", readCount);
ROS_INFO("Parameters read = %d", readCount);
if(validParameters)
{
ROS_INFO("Parameters successfully read.");
}
else
{
if(this->isVisible())
{
QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
ROS_WARN("%s", warning.toStdString().c_str());
QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
}
return false;
}
return true;
if(validParameters)
{
ROS_INFO("Parameters successfully read.");
}
else
{
return PreferencesDialog::readCoreSettings(filePath);
if(this->isVisible())
{
QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
ROS_WARN("%s", warning.toStdString().c_str());
QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
}
return false;
}
return true;
}
void PreferencesDialogROS::writeSettings(const QString & filePath)
void PreferencesDialogROS::writeCoreSettings(const QString & filePath) const
{
writeGuiSettings(filePath);
// This will tell the MainWindow that the
//parameters are updated. The MainWindow will send an Event that
// will be handled by the GuiWrapper where we will write
// parameters in ROS and the rtabmap_node will be notified.
if(_parameters.size())
QString path = getIniFilePath();
if(!filePath.isEmpty())
{
emit settingsChanged(_parameters);
path = filePath;
}
if(_obsoletePanels)
if(QFile::exists(path))
{
emit settingsChanged(_obsoletePanels);
}
rtabmap::ParametersMap parameters = this->getAllParameters();
_parameters = rtabmap::ParametersMap();
_obsoletePanels = kPanelDummy;
std::string workingDir = uValue(parameters, Parameters::kRtabmapWorkingDirectory(), std::string(""));
if(!workingDir.empty())
{
//Just update GUI working directory
QSettings settings(path, QSettings::IniFormat);
settings.beginGroup("Core");
settings.remove("");
settings.setValue(Parameters::kRtabmapWorkingDirectory().c_str(), workingDir.c_str());
settings.endGroup();
}
}
}
src/PreferencesDialogROS.h
+3 -3
View File
@@ -40,15 +40,15 @@ public:
virtual ~PreferencesDialogROS();
virtual QString getIniFilePath() const;
virtual QString getTmpIniFilePath() const;
protected:
virtual QString getParamMessage();
virtual void readCameraSettings(const QString & filePath);
virtual bool readCoreSettings(const QString & filePath);
virtual void writeSettings(const QString & filePath);
virtual QString getTmpIniFilePath() const;
virtual void writeCameraSettings(const QString & filePath) const {}
virtual void writeCoreSettings(const QString & filePath) const;
private:
QString configFile_;
src/RGBDOdometryNode.cpp
+4 -18
View File
@@ -189,16 +189,9 @@ public:
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
rtabmap::CameraModel rtabmapModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
rtabmap::SensorData data(
ptrImage->image,
@@ -321,14 +314,7 @@ public:
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*infoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
}
rtabmap::SensorData data(
src/StereoOdometryNode.cpp
+7 -16
View File
@@ -145,24 +145,15 @@ public:
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
if(model.baseline() <= 0)
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
if(stereoModel.baseline() <= 0)
{
ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline());
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
if(model.baseline() > 10.0)
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
@@ -170,13 +161,13 @@ public:
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer;
//
src/nodelets/data_throttle.cpp
+4 -4
View File
@@ -91,16 +91,16 @@ private:
bool approxSync = true;
if(private_nh.getParam("max_rate", rate_))
{
ROS_WARN("\"max_rate\" is now known as \"rate\".");
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
}
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
src/nodelets/disparity_to_depth.cpp
+1 -1
View File
@@ -63,7 +63,7 @@ private:
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
{
ROS_ERROR("Input type must be disparity=32FC1");
NODELET_ERROR("Input type must be disparity=32FC1");
return;
}
src/nodelets/obstacles_detection.cpp
+42 -13
View File
@@ -67,10 +67,13 @@ class ObstaclesDetection : public nodelet::Nodelet
public:
ObstaclesDetection() :
frameId_("base_link"),
normalEstimationRadius_(0.05),
normalKSearch_(20),
groundNormalAngle_(M_PI_4),
clusterRadius_(0.05),
minClusterSize_(20),
maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
segmentFlatObstacles_(false),
waitForTransform_(false),
optimizeForCloseObjects_(false)
{}
@@ -87,10 +90,24 @@ private:
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
pnh.param("normal_k", normalKSearch_, normalKSearch_);
pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
{
NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
"to \"cluster_radius\"! Your value is still copied to "
"corresponding parameter. Instead of normal radius, nearest neighbors count "
"\"normal_k\" is used instead (default 20).");
pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
}
else
{
pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
}
pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
@@ -104,7 +121,7 @@ private:
void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
ros::Time time = ros::Time::now();
ros::WallTime time = ros::WallTime::now();
if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
{
@@ -119,7 +136,7 @@ private:
{
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
{
ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
}
}
@@ -129,7 +146,7 @@ private:
}
catch(tf::TransformException & ex)
{
ROS_ERROR("%s",ex.what());
NODELET_ERROR("%s",ex.what());
return;
}
@@ -158,9 +175,12 @@ private:
originalCloud,
ground,
obstacles,
normalEstimationRadius_,
normalKSearch_,
groundNormalAngle_,
minClusterSize_);
clusterRadius_,
minClusterSize_,
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
@@ -190,9 +210,12 @@ private:
originalCloud_near,
ground,
obstacles,
normalEstimationRadius_,
normalKSearch_,
groundNormalAngle_,
minClusterSize_);
clusterRadius_,
minClusterSize_,
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
@@ -211,9 +234,12 @@ private:
originalCloud_far,
ground,
obstacles,
3.*normalEstimationRadius_,
normalKSearch_,
2.*groundNormalAngle_,
minClusterSize_);
3.*clusterRadius_,
minClusterSize_,
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
@@ -255,15 +281,18 @@ private:
obstaclesPub_.publish(rosCloud);
}
//ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec());
//NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
}
private:
std::string frameId_;
double normalEstimationRadius_;
int normalKSearch_;
double groundNormalAngle_;
double clusterRadius_;
int minClusterSize_;
double maxObstaclesHeight_;
double maxGroundHeight_;
bool segmentFlatObstacles_;
bool waitForTransform_;
bool optimizeForCloseObjects_;
src/nodelets/point_cloud_xyz.cpp
+13 -14
View File
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/MsgConversion.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
@@ -62,6 +64,7 @@ class PointCloudXYZ : public nodelet::Nodelet
public:
PointCloudXYZ() :
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
@@ -98,6 +101,7 @@ private:
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
@@ -106,7 +110,7 @@ private:
pnh.param("cut_right", cut_right_, cut_right_);
pnh.param("special_filter_close_object", create_close_obstacle_if_depth_is_missing_, create_close_obstacle_if_depth_is_missing_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
@@ -147,7 +151,7 @@ private:
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
{
ROS_ERROR("Input type depth=32FC1,16UC1,MONO16");
NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
return;
}
@@ -222,7 +226,7 @@ private:
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
ROS_ERROR("Input type must be disparity=32FC1 or 16SC1");
NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
return;
}
@@ -238,18 +242,12 @@ private:
if(cloudPub_.getNumSubscribers())
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float cx = model.cx();
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx(), leftModel.cy(), disparityMsg->T);
pclCloud = rtabmap::util3d::cloudFromDisparity(
disparity,
cx,
cy,
disparityMsg->f,
disparityMsg->T,
stereoModel,
decimation_);
processAndPublish(pclCloud, disparityMsg->header);
@@ -258,9 +256,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header)
{
if(pclCloud->size() && maxDepth_ > 0)
if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
{
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -288,6 +286,7 @@ private:
private:
double maxDepth_;
double minDepth_;
double voxelSize_;
int decimation_;
double noiseFilterRadius_;
src/nodelets/point_cloud_xyzrgb.cpp
+26 -18
View File
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap_ros/MsgConversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
@@ -62,6 +64,7 @@ class PointCloudXYZRGB : public nodelet::Nodelet
public:
PointCloudXYZRGB() :
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
@@ -95,12 +98,13 @@ private:
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
@@ -165,13 +169,27 @@ private:
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
if(cloudPub_.getNumSubscribers())
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
imagePtr = cv_bridge::toCvShare(image);
}
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
imagePtr = cv_bridge::toCvShare(image, "mono8");
}
else
{
imagePtr = cv_bridge::toCvShare(image, "bgr8");
}
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model;
@@ -210,7 +228,7 @@ private:
imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
return;
}
@@ -228,22 +246,11 @@ private:
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*camInfoLeft, *camInfoRight);
float fx = model.left().fx();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
ptrRightImage->image,
cx,
cy,
fx,
baseline,
rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
decimation_);
processAndPublish(pclCloud, imageLeft->header);
@@ -252,9 +259,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
{
if(pclCloud->size() && maxDepth_ > 0)
if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
{
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -282,6 +289,7 @@ private:
private:
double maxDepth_;
double minDepth_;
double voxelSize_;
int decimation_;
double noiseFilterRadius_;
src/nodelets/stereo_throttle.cpp
+3 -3
View File
@@ -93,9 +93,9 @@ private:
pnh.param("queue_size", queueSize, queueSize);
pnh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
src/rviz/InfoDisplay.cpp
+7 -7
View File
@@ -51,8 +51,8 @@ void InfoDisplay::onInitialize()
this->setStatusStd(rviz::StatusProperty::Ok, "Info", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0");
this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0");
this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
spinner_.start();
}
@@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
boost::mutex::scoped_lock lock(info_mutex_);
if(msg->loopClosureId)
{
info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId);
info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
globalCount_ += 1;
}
else if(msg->localLoopClosureId)
else if(msg->proximityDetectionId)
{
info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId);
info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
localCount_ += 1;
}
else
@@ -103,8 +103,8 @@ void InfoDisplay::update( float wall_dt, float ros_dt )
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
}
this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
{
src/rviz/MapCloudDisplay.cpp
+37 -14
View File
@@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <QtGui/QApplication>
#include <QtGui/QMessageBox>
#include <QtCore/QTimer>
#include <QApplication>
#include <QMessageBox>
#include <QTimer>
#include <OgreSceneNode.h>
#include <OgreSceneManager.h>
@@ -143,6 +143,12 @@ MapCloudDisplay::MapCloudDisplay()
cloud_max_depth_->setMin( 0.0f );
cloud_max_depth_->setMax( 999.0f );
cloud_min_depth_ = new rviz::FloatProperty( "Cloud min depth (m)", 0.0f,
"Minimum depth of the generated clouds.",
this, SLOT( updateCloudParameters() ), this );
cloud_min_depth_->setMin( 0.0f );
cloud_min_depth_->setMax( 999.0f );
cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f,
"Voxel size of the generated clouds.",
this, SLOT( updateCloudParameters() ), this );
@@ -156,6 +162,13 @@ MapCloudDisplay::MapCloudDisplay()
cloud_filter_floor_height_->setMin( 0.0f );
cloud_filter_floor_height_->setMax( 999.0f );
cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f,
"Filter the ceiling at the specified height set here "
"(only appropriate for 2D mapping).",
this, SLOT( updateCloudParameters() ), this );
cloud_filter_ceiling_height_->setMin( 0.0f );
cloud_filter_ceiling_height_->setMax( 999.0f );
node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
"(Disabled=0) Only keep one node in the specified radius.",
this, SLOT( updateCloudParameters() ), this );
@@ -249,17 +262,24 @@ void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg )
void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
{
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
// Add new clouds...
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{
int id = map.nodes[i].id;
if(cloud_infos_.find(id) == cloud_infos_.end())
if(poses.find(id) != poses.end() &&
cloud_infos_.find(id) == cloud_infos_.end())
{
// Cloud not added to RVIZ, add it!
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
{
cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0);
@@ -268,17 +288,26 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr validIndices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloud_decimation_->getInt(),
cloud_max_depth_->getFloat(),
cloud_voxel_size_->getFloat());
cloud_min_depth_->getFloat(),
validIndices.get());
if(cloud_voxel_size_->getFloat())
{
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
}
if(cloud->size())
{
if(cloud_filter_floor_height_->getFloat() > 0.0f)
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
cloud = rtabmap::util3d::passThrough(cloud, "z",
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -302,12 +331,6 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
}
// Update graph
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = rtabmap::graph::radiusPosesFiltering(poses,
src/rviz/MapCloudDisplay.h
+6
View File
@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef MAP_CLOUD_DISPLAY_H
#define MAP_CLOUD_DISPLAY_H
#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
#include <deque>
#include <queue>
#include <vector>
@@ -42,6 +44,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rviz/message_filter_display.h>
#include <rviz/default_plugin/point_cloud_transformer.h>
#endif
namespace rviz {
class IntProperty;
class BoolProperty;
@@ -106,8 +110,10 @@ public:
rviz::EnumProperty* style_property_;
rviz::IntProperty* cloud_decimation_;
rviz::FloatProperty* cloud_max_depth_;
rviz::FloatProperty* cloud_min_depth_;
rviz::FloatProperty* cloud_voxel_size_;
rviz::FloatProperty* cloud_filter_floor_height_;
rviz::FloatProperty* cloud_filter_ceiling_height_;
rviz::FloatProperty* node_filtering_radius_;
rviz::FloatProperty* node_filtering_angle_;
rviz::BoolProperty* download_map_;
src/rviz/MapGraphDisplay.cpp
+7 -1
View File
@@ -52,7 +52,9 @@ namespace rtabmap_ros
MapGraphDisplay::MapGraphDisplay()
{
color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue,
"Color to draw neighbor links.", this );
"Color to draw neighbor links.", this );
color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0),
"Color to draw merged neighbor links.", this );
color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red,
"Color to draw global loop closure links.", this );
color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow,
@@ -139,6 +141,10 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg
{
color = color_neighbor_property_->getOgreColor();
}
else if(iter->second.type() == rtabmap::Link::kNeighborMerged)
{
color = color_neighbor_merged_property_->getOgreColor();
}
else if(iter->second.type() == rtabmap::Link::kVirtualClosure)
{
color = color_virtual_property_->getOgreColor();
src/rviz/MapGraphDisplay.h
+1
View File
@@ -76,6 +76,7 @@ private:
std::vector<Ogre::ManualObject*> manual_objects_;
ColorProperty* color_neighbor_property_;
ColorProperty* color_neighbor_merged_property_;
ColorProperty* color_global_property_;
ColorProperty* color_local_property_;
ColorProperty* color_user_property_;
srv/SetGoal.srv
+2 -1
View File
@@ -5,4 +5,5 @@ string node_label
---
#response
int32[] path_ids
geometry_msgs/Pose[] path_poses
geometry_msgs/Pose[] path_poses
float32 planning_time