i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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
+95 -31
View File
@@ -7,23 +7,41 @@ project(rtabmap_ros)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs 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 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 genmsg stereo_msgs move_base_msgs
) )
# Optional components # Optional components
find_package(costmap_2d) find_package(costmap_2d)
find_package(octomap_ros) find_package(octomap_ros)
find_package(rviz)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.10.10 REQUIRED) find_package(RTABMap 0.11.5 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
#Qt stuff #Qt stuff
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) # If librtabmap_gui.so is found, rtabmapviz will be built
INCLUDE(${QT_USE_FILE}) # 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 ## We also use Ogre
include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
@@ -52,6 +70,7 @@ add_message_files(
Link.msg Link.msg
OdomInfo.msg OdomInfo.msg
Point2f.msg Point2f.msg
Point3f.msg
Goal.msg Goal.msg
) )
@@ -91,8 +110,9 @@ catkin_package(
LIBRARIES rtabmap_ros LIBRARIES rtabmap_ros
CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs 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 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
stereo_msgs move_base_msgs stereo_msgs move_base_msgs
DEPENDS RTABMap OpenCV
) )
########### ###########
@@ -117,20 +137,6 @@ SET(Libraries
${rviz_DEFAULT_PLUGIN_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 SET(rtabmap_ros_lib_src
src/nodelets/data_throttle.cpp src/nodelets/data_throttle.cpp
src/nodelets/stereo_throttle.cpp src/nodelets/stereo_throttle.cpp
@@ -142,11 +148,7 @@ SET(rtabmap_ros_lib_src
src/nodelets/point_cloud_aggregator.cpp src/nodelets/point_cloud_aggregator.cpp
src/MsgConversion.cpp src/MsgConversion.cpp
src/OdometryROS.cpp src/OdometryROS.cpp
src/rviz/MapCloudDisplay.cpp src/MapsManager.cpp
src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp
src/rviz/OrbitOrientedViewController.cpp
${MOC_FILES}
) )
# If costmap_2d is found, add the plugin # If costmap_2d is found, add the plugin
@@ -163,15 +165,72 @@ SET(rtabmap_ros_lib_src
) )
ENDIF(costmap_2d_FOUND) 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 ## Declare a cpp library
############################
add_library(rtabmap_ros add_library(rtabmap_ros
${rtabmap_ros_lib_src} ${rtabmap_ros_lib_src}
) )
target_link_libraries(rtabmap_ros target_link_libraries(rtabmap_ros
${Libraries} ${Libraries}
${QT_LIBRARIES}
${OGRE_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}) add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
# If octomap is found, add definition # If octomap is found, add definition
@@ -187,7 +246,7 @@ SET(Libraries
add_definitions(-DWITH_OCTOMAP) add_definitions(-DWITH_OCTOMAP)
ENDIF(octomap_ros_FOUND) 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}) target_link_libraries(rtabmap rtabmap_ros ${Libraries})
add_executable(rgbd_odometry src/RGBDOdometryNode.cpp) 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) add_executable(map_assembler src/MapAssemblerNode.cpp)
target_link_libraries(map_assembler rtabmap_ros ${Libraries}) 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_executable(camera src/CameraNode.cpp)
add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS}) add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(camera ${Libraries}) target_link_libraries(camera ${Libraries})
@@ -212,6 +268,9 @@ target_link_libraries(camera ${Libraries})
IF(RTABMAP_GUI) IF(RTABMAP_GUI)
add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp) add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries}) target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries})
IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmapviz Widgets Core Gui)
ENDIF()
ELSE() ELSE()
MESSAGE(WARNING "Found RTAB-Map built without its GUI library. Node rtabmapviz will not be built!") MESSAGE(WARNING "Found RTAB-Map built without its GUI library. Node rtabmapviz will not be built!")
ENDIF() ENDIF()
@@ -245,7 +304,6 @@ install(TARGETS
rgbd_odometry rgbd_odometry
stereo_odometry stereo_odometry
map_assembler map_assembler
grid_map_assembler
map_optimizer map_optimizer
data_player data_player
camera camera
@@ -260,7 +318,6 @@ install(TARGETS
rgbd_odometry rgbd_odometry
stereo_odometry stereo_odometry
map_assembler map_assembler
grid_map_assembler
map_optimizer map_optimizer
data_player data_player
camera camera
@@ -279,6 +336,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
## Mark other files for installation (e.g. launch and bag files, etc.) ## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES install(FILES
launch/rtabmap.launch
launch/rgbd_mapping.launch launch/rgbd_mapping.launch
launch/stereo_mapping.launch launch/stereo_mapping.launch
launch/data_recorder.launch launch/data_recorder.launch
@@ -299,6 +357,12 @@ install(FILES
costmap_plugins.xml costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
) )
IF(costmap_2d_FOUND)
install(FILES
costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
ENDIF(costmap_2d_FOUND)
############# #############
## Testing ## ## 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: * 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 ```bash
source /opt/ros/indigo/setup.bash $ source /opt/ros/indigo/setup.bash
source ~/catkin_ws/devel/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 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. * 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 ```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 $ 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 <tf/tf.h>
#include <geometry_msgs/Transform.h> #include <geometry_msgs/Transform.h>
#include <geometry_msgs/Pose.h> #include <geometry_msgs/Pose.h>
#include <sensor_msgs/CameraInfo.h>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.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/Signature.h>
#include <rtabmap/core/OdometryInfo.h> #include <rtabmap/core/OdometryInfo.h>
#include <rtabmap/core/Statistics.h> #include <rtabmap/core/Statistics.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap_ros/Link.h> #include <rtabmap_ros/Link.h>
#include <rtabmap_ros/KeyPoint.h> #include <rtabmap_ros/KeyPoint.h>
#include <rtabmap_ros/Point2f.h> #include <rtabmap_ros/Point2f.h>
#include <rtabmap_ros/Point3f.h>
#include <rtabmap_ros/MapData.h> #include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/MapGraph.h> #include <rtabmap_ros/MapGraph.h>
#include <rtabmap_ros/NodeData.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); 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); 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( void mapDataFromROS(
const rtabmap_ros::MapData & msg, const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses, std::map<int, rtabmap::Transform> & poses,
@@ -110,6 +131,9 @@ void mapGraphToROS(
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg); rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
void nodeDataToROS(const rtabmap::Signature & signature, 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); rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
launch/azimut3/az3_mapping_robot_kinect_scan.launch
+111 -49
View File
@@ -1,69 +1,131 @@
<launch> <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)/az3_standalone.launch"/>
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
<!-- OpenNI --> <!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/> <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_laserScan" type="bool" value="true"/>
<param name="use_action_for_goal" type="bool" value="true"/>
<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 --> <!-- Throttling messages -->
<group ns="camera"> <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.0"/> <param name="rate" type="double" value="3"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/> <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="rgb/image_out" to="data_throttled_image"/> <remap from="rgb/image_out" to="throttled_image"/>
<remap from="depth/image_out" to="data_throttled_image_depth"/> <remap from="depth/image_out" to="throttled_image_depth"/>
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/> <remap from="rgb/camera_info_out" to="throttled_camera_info"/>
</node> </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"> <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="image" to="depth_registered/image_raw"/>
<remap from="camera_info" to="depth_registered/camera_info"/> <remap from="camera_info" to="depth_registered/camera_info"/>
<remap from="scan" to="/kinect_scan"/> <remap from="scan" to="/kinect_scan"/>
<param name="range_max" type="double" value="4"/> <param name="range_max" type="double" value="4"/>
</node> </node>
</group> </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"/>
<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"/>
</node>
</group>
</launch> </launch>
launch/azimut3/az3_mapping_robot_nav.launch → launch/azimut3/az3_nav.launch
+48 -45
View File
@@ -1,8 +1,10 @@
<launch> <launch>
<!-- args: "delete_db_on_start" and "udebug" --> <!-- Localization-only mode -->
<arg name="rtabmap_args" default="" /> <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 --> <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/> <include file="$(find az3_bringup)/az3_standalone.launch"/>
@@ -13,59 +15,61 @@
<!-- SLAM (robot side) --> <!-- SLAM (robot side) -->
<group ns="rtabmap"> <group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)"> <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="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_laserScan" type="bool" value="true"/> <param name="subscribe_scan" type="bool" value="true"/>
<param name="use_action_for_goal" 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="cloud_decimation" type="int" value="1"/> <!-- we already decimate in memory below -->
<param name="grid_eroded" type="bool" value="true"/> <param name="grid_eroded" type="bool" value="true"/>
<param name="grid_cell_size" type="double" value="0.05"/> <param name="grid_cell_size" type="double" value="0.05"/>
<remap from="odom" to="/base_controller/odom"/> <remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/> <remap from="scan" to="/base_scan"/>
<remap from="mapData" to="mapData"/> <remap from="mapData" to="mapData"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/> <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="move_base" to="/planner/move_base"/>
<remap from="grid_map" to="/map"/> <remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters --> <!-- RTAB-Map's parameters -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/> <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/LocalLoopDetectionSpace" 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/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving --> <param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="RGBD/LocalRadius" type="string" value="5"/> <param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/> <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/> <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/ImageDecimation" type="string" value="4"/> <param name="Mem/ImageDecimation" type="string" value="4"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/> <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
<param name="Rtabmap/TimeThr" type="string" value="600"/> <param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <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="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="Optimizer/Slam2D" type="string" value="true"/>
<param name="RGBD/OptimizeIterations" type="string" value="100"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/WordsPerImage" type="string" value="200"/> <param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="Kp/NNStrategy" type="string" value="1"/>
<param name="SURF/HessianThreshold" type="string" value="500"/> <param name="SURF/HessianThreshold" type="string" value="500"/>
<param name="LccBow/Force2D" type="string" value="true"/> <param name="Reg/Force3DoF" type="string" value="true"/>
<param name="LccBow/MaxDepth" type="string" value="5"/> <param name="Vis/MaxDepth" type="string" value="5"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <param name="Vis/MinInliers" type="string" value="5"/>
<param name="LccBow/InlierDistance" type="string" value="0.1"/>
<!-- 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> </node>
</group> </group>
@@ -79,17 +83,16 @@
<!-- ROS navigation stack move_base --> <!-- ROS navigation stack move_base -->
<group ns="planner"> <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="obstacles_cloud" to="/obstacles_cloud"/>
<remap from="ground_cloud" to="/ground_cloud"/> <remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/map"/> <remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/> <remap from="move_base_simple/goal" to="/planner_goal"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen"> <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/> <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_2d.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/costmap_common_params_2d.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/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/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" /> <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node> </node>
@@ -109,7 +112,7 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <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="rate" type="double" value="5"/>
<param name="decimation" type="int" value="2"/> <param name="decimation" type="int" value="2"/>
@@ -128,7 +131,7 @@
<remap from="depth/camera_info" to="data_resized_camera_info"/> <remap from="depth/camera_info" to="data_resized_camera_info"/>
<remap from="cloud" to="cloudXYZ" /> <remap from="cloud" to="cloudXYZ" />
<param name="decimation" type="int" value="1"/> <!-- already decimated above --> <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"/> <param name="voxel_size" type="double" value="0.02"/>
</node> </node>
@@ -137,10 +140,10 @@
<remap from="obstacles" to="/obstacles_cloud"/> <remap from="obstacles" to="/obstacles_cloud"/>
<remap from="ground" to="/ground_cloud"/> <remap from="ground" to="/ground_cloud"/>
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="map_frame_id" type="string" value="map"/> <param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/> <param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/> <param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/> <param name="max_obstacles_height" type="double" value="0.4"/>
</node> </node>
</group> </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
+1
View File
@@ -2,6 +2,7 @@
<launch> <launch>
<!-- Xtion --> <!-- Xtion -->
<param name="/camera/driver/data_skip" value="1" />
<include file="$(find openni2_launch)/launch/openni2.launch"> <include file="$(find openni2_launch)/launch/openni2.launch">
<arg name="depth_registration" value="True" /> <arg name="depth_registration" value="True" />
<arg name="rgb_camera_info_url" <arg name="rgb_camera_info_url"
launch/azimut3/config/azimut3_nav.rviz
+160 -16
View File
@@ -7,7 +7,7 @@ Panels:
- /Global Options1 - /Global Options1
- /TF1/Frames1 - /TF1/Frames1
Splitter Ratio: 0.601881 Splitter Ratio: 0.601881
Tree Height: 187 Tree Height: 353
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Views - Class: rviz/Views
@@ -19,7 +19,7 @@ Panels:
Experimental: false Experimental: false
Name: Time Name: Time
SyncMode: 0 SyncMode: 0
SyncSource: "" SyncSource: Info
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
Expanded: Expanded:
- /2D Pose Estimate1 - /2D Pose Estimate1
@@ -52,13 +52,73 @@ Visualization Manager:
Frame Timeout: 15 Frame Timeout: 15
Frames: Frames:
All Enabled: false 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 Marker Scale: 1
Name: TF Name: TF
Show Arrows: true Show Arrows: true
Show Axes: true Show Axes: true
Show Names: true Show Names: true
Tree: 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 Update Interval: 0
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -102,18 +162,18 @@ Visualization Manager:
Class: rviz/Map Class: rviz/Map
Color Scheme: costmap Color Scheme: costmap
Draw Behind: false Draw Behind: false
Enabled: false Enabled: true
Name: Global costmap Name: Global costmap
Topic: /planner/move_base/global_costmap/costmap Topic: /planner/move_base/global_costmap/costmap
Value: false Value: true
- Alpha: 0.7 - Alpha: 0.7
Class: rviz/Map Class: rviz/Map
Color Scheme: costmap Color Scheme: costmap
Draw Behind: false Draw Behind: false
Enabled: true Enabled: false
Name: Local costmap Name: Local costmap
Topic: /planner/move_base/local_costmap/costmap Topic: /planner/move_base/local_costmap/costmap
Value: true Value: false
- Alpha: 1 - Alpha: 1
Class: rviz/RobotModel Class: rviz/RobotModel
Collision Enabled: false Collision Enabled: false
@@ -124,6 +184,61 @@ Visualization Manager:
Expand Link Details: false Expand Link Details: false
Expand Tree: false Expand Tree: false
Link Tree Style: Links in Alphabetic Order 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 Name: RobotModel
Robot Description: robot_description Robot Description: robot_description
TF Prefix: "" TF Prefix: ""
@@ -132,7 +247,7 @@ Visualization Manager:
Visual Enabled: true Visual Enabled: true
- Class: rviz/Image - Class: rviz/Image
Enabled: true Enabled: true
Image Topic: /camera/data_resized_image_relay Image Topic: /camera/throttled_image
Max Value: 1 Max Value: 1
Median window: 5 Median window: 5
Min Value: 0 Min Value: 0
@@ -171,7 +286,7 @@ Visualization Manager:
Size (Pixels): 3 Size (Pixels): 3
Size (m): 0.01 Size (m): 0.01
Style: Points Style: Points
Topic: /rtabmap/mapData_relay Topic: /rtabmap/mapData
Use Fixed Frame: true Use Fixed Frame: true
Use rainbow: true Use rainbow: true
Value: true Value: true
@@ -185,7 +300,13 @@ Visualization Manager:
Class: rviz/Path Class: rviz/Path
Color: 255; 149; 57 Color: 255; 149; 57
Enabled: true Enabled: true
Line Style: Lines
Line Width: 0.03
Name: move_base global plan Name: move_base global plan
Offset:
X: 0
Y: 0
Z: 0
Topic: /planner/move_base/NavfnROS/plan Topic: /planner/move_base/NavfnROS/plan
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -193,7 +314,13 @@ Visualization Manager:
Class: rviz/Path Class: rviz/Path
Color: 2; 14; 255 Color: 2; 14; 255
Enabled: true Enabled: true
Line Style: Lines
Line Width: 0.03
Name: move_base local plan Name: move_base local plan
Offset:
X: 0
Y: 0
Z: 0
Topic: /planner/move_base/TrajectoryPlannerROS/local_plan Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -227,17 +354,28 @@ Visualization Manager:
Value: true Value: true
- Alpha: 1 - Alpha: 1
Class: rtabmap_ros/MapGraph Class: rtabmap_ros/MapGraph
Color: 0; 0; 255
Enabled: true Enabled: true
Global loop closure: 255; 0; 0
Local loop closure: 255; 255; 0
Merged neighbor: 255; 170; 0
Name: MapGraph Name: MapGraph
Topic: /rtabmap/mapData_relay Neighbor: 0; 0; 255
Topic: /rtabmap/mapGraph
User: 255; 0; 0
Value: true Value: true
Virtual: 255; 0; 255
- Alpha: 1 - Alpha: 1
Buffer Length: 1 Buffer Length: 1
Class: rviz/Path Class: rviz/Path
Color: 255; 0; 255 Color: 255; 0; 255
Enabled: true Enabled: true
Line Style: Lines
Line Width: 0.03
Name: Rtabmap global path Name: Rtabmap global path
Offset:
X: 0
Y: 0
Z: 0
Topic: /rtabmap/global_path Topic: /rtabmap/global_path
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -245,7 +383,13 @@ Visualization Manager:
Class: rviz/Path Class: rviz/Path
Color: 85; 255; 255 Color: 85; 255; 255
Enabled: true Enabled: true
Line Style: Lines
Line Width: 0.03
Name: Rtabmap local path Name: Rtabmap local path
Offset:
X: 0
Y: 0
Z: 0
Topic: /rtabmap/local_path Topic: /rtabmap/local_path
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -308,10 +452,10 @@ Visualization Manager:
Z: -0.0856586 Z: -0.0856586
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.01
Pitch: 0.884797 Pitch: 1.2448
Target Frame: base_footprint Target Frame: base_footprint
Value: Orbit (rviz) Value: Orbit (rviz)
Yaw: 3.81544 Yaw: 3.76044
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
@@ -321,7 +465,7 @@ Window Geometry:
Hide Right Dock: false Hide Right Dock: false
Image: Image:
collapsed: false collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a2000000dd00fffffffb0000000a0049006d00610067006501000001d0000000870000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@@ -331,5 +475,5 @@ Window Geometry:
Views: Views:
collapsed: false collapsed: false
Width: 1228 Width: 1228
X: 406 X: 396
Y: 154 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: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.02 footprint_padding: 0.04
#robot_radius: 0.38
#robot_radius: ir_of_robot
inflation_layer: inflation_layer:
inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
transform_tolerance: 2 transform_tolerance: 2
@@ -16,8 +14,8 @@ obstacle_layer:
laser_scan_sensor: { laser_scan_sensor: {
data_type: LaserScan, data_type: LaserScan,
topic: base_scan, topic: scan,
expected_update_rate: 0.2, expected_update_rate: 0.1,
marking: true, marking: true,
clearing: true clearing: true
} }
@@ -39,7 +37,7 @@ obstacle_layer:
expected_update_rate: 0.5, expected_update_rate: 0.5,
marking: false, marking: false,
clearing: true, 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 global_frame: map
robot_base_frame: base_footprint robot_base_frame: base_footprint
update_frequency: 1 update_frequency: 1
publish_frequency: 2 publish_frequency: 1
always_send_full_costmap: false always_send_full_costmap: false
plugins: plugins:
- {name: static_layer, type: "rtabmap_ros::StaticLayer"} - {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\loggerType=1
General\loggerPrintTime=true General\loggerPrintTime=true
General\verticalLayoutUsed=false General\verticalLayoutUsed=false
General\imageFlipped=false
General\imageRejectedShown=true General\imageRejectedShown=true
General\imageHighestHypShown=true General\imageHighestHypShown=true
General\beep=false General\beep=false
General\keypointsOpacity=16 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)"
General\voxelSize=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\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)
General\showClouds0=true General\showClouds0=true
General\voxelSize0=0
General\decimation0=4 General\decimation0=4
General\maxDepth0=4 General\maxDepth0=4
General\showScans0=true General\showScans0=true
@@ -25,7 +20,6 @@ General\ptSize0=1
General\opacityScan0=1 General\opacityScan0=1
General\ptSizeScan0=1 General\ptSizeScan0=1
General\showClouds1=true General\showClouds1=true
General\voxelSize1=0
General\decimation1=2 General\decimation1=2
General\maxDepth1=0 General\maxDepth1=0
General\showScans1=true General\showScans1=true
@@ -33,12 +27,140 @@ General\opacity1=1
General\ptSize1=1 General\ptSize1=1
General\opacityScan1=1 General\opacityScan1=1
General\ptSizeScan1=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\cloudFiltering=false
General\cloudFilteringRadius=0.5 General\cloudFilteringRadius=0.5
General\cloudFilteringAngle=30 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: Expanded:
- /Global Options1 - /Global Options1
- /Status1 - /Status1
- /Info1
Splitter Ratio: 0.5 Splitter Ratio: 0.5
Tree Height: 438 Tree Height: 438
- Class: rviz/Selection - Class: rviz/Selection
@@ -134,6 +133,7 @@ Visualization Manager:
Download graph: false Download graph: false
Download map: false Download map: false
Enabled: true Enabled: true
Filter ceiling (m): 0
Filter floor (m): 0 Filter floor (m): 0
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
@@ -147,17 +147,22 @@ Visualization Manager:
Size (Pixels): 3 Size (Pixels): 3
Size (m): 0.01 Size (m): 0.01
Style: Points Style: Points
Topic: /rtabmap/mapData_optimized Topic: /rtabmap/mapData
Use Fixed Frame: true Use Fixed Frame: true
Use rainbow: true Use rainbow: true
Value: true Value: true
- Alpha: 1 - Alpha: 1
Class: rtabmap_ros/MapGraph Class: rtabmap_ros/MapGraph
Color: 25; 255; 0
Enabled: true Enabled: true
Global loop closure: 255; 0; 0
Local loop closure: 255; 255; 0
Merged neighbor: 255; 170; 0
Name: MapGraph Name: MapGraph
Topic: /rtabmap/mapDataGraph_optimized Neighbor: 0; 0; 255
Topic: /rtabmap/mapGraph
User: 255; 0; 0
Value: true Value: true
Virtual: 255; 0; 255
- Class: rviz/Image - Class: rviz/Image
Enabled: true Enabled: true
Image Topic: /stereo_camera/left/image_rect_color Image Topic: /stereo_camera/left/image_rect_color
@@ -173,15 +178,73 @@ Visualization Manager:
Class: rviz/Map Class: rviz/Map
Color Scheme: map Color Scheme: map
Draw Behind: false Draw Behind: false
Enabled: false Enabled: true
Name: Map Name: Map
Topic: /map Topic: /rtabmap/proj_map
Value: false Value: true
- Class: rtabmap_ros/Info - Class: rtabmap_ros/Info
Enabled: true Enabled: true
Name: Info Name: Info
Topic: /rtabmap/info Topic: /rtabmap/info
Value: true 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 Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
@@ -206,7 +269,7 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rtabmap_ros/OrbitOriented Class: rtabmap_ros/OrbitOriented
Distance: 6.60197 Distance: 8.28384
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.06
Stereo Focal Distance: 1 Stereo Focal Distance: 1
@@ -218,10 +281,10 @@ Visualization Manager:
Z: 0.113349 Z: 0.113349
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.01
Pitch: 0.455398 Pitch: 0.635398
Target Frame: base_footprint Target Frame: base_footprint
Value: OrbitOriented (rtabmap) Value: OrbitOriented (rtabmap)
Yaw: 3.1304 Yaw: 3.0704
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
@@ -241,5 +304,5 @@ Window Geometry:
Views: Views:
collapsed: false collapsed: false
Width: 1341 Width: 1341
X: 147 X: 97
Y: 48 Y: 14
launch/data_recorder.launch
+13 -7
View File
@@ -4,7 +4,8 @@
<arg name="subscribe_odometry" default="false"/> <arg name="subscribe_odometry" default="false"/>
<arg name="subscribe_depth" default="true"/> <arg name="subscribe_depth" default="true"/>
<arg name="subscribe_stereo" default="false"/> <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="frame_id" default="camera_link"/>
<arg name="odom_frame_id" default=""/> <!-- use topic when not set, otherwise use TF if set --> <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"> <node name="data_recorder" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<!-- Disable any processing --> <!-- Disable any processing -->
<param name="Mem/RehearsalSimilarity" type="string" value="1.0"/> <!-- desactivate rehearsal --> <param name="Mem/RehearsalSimilarity" type="string" value="1.0"/> <!-- deactivate rehearsal -->
<param name="Kp/WordsPerImage" type="string" value="-1"/> <!-- desactivate keypoints extraction --> <param name="Kp/MaxFeatures" type="string" value="-1"/> <!-- deactivate keypoints extraction -->
<param name="Rtabmap/MaxRetrieved" type="string" value="0"/> <!-- desactivate global retrieval --> <param name="Rtabmap/MaxRetrieved" type="string" value="0"/> <!-- deactivate global retrieval -->
<param name="RGBD/MaxLocalRetrieved" type="string" value="0"/> <!-- desactivate local retrieval --> <param name="RGBD/MaxLocalRetrieved" type="string" value="0"/> <!-- deactivate local retrieval -->
<param name="Rtabmap/MemoryThr" type="string" value="1"/> <!-- keep the WM empty --> <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="Mem/STMSize" type="string" value="1"/> <!-- STM=1 -->
<param name="publish_tf" type="bool" value="false"/> <!-- don't publish TF --> <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 unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
<param name="Rtabmap/DetectionRate" type="string" value="$(arg max_rate)"/> <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="database_path" type="string" value="$(arg output_path)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="$(arg subscribe_depth)"/> <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="subscribe_stereo" type="bool" value="$(arg subscribe_stereo)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <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 --> <!-- 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)"/> <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 --> <!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... --> <!-- 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"> <group ns="rtabmap">
<!-- args: "delete_db_on_start" and "udebug" --> <!-- 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="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 --> <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. --> <!-- 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="RGBD/Enabled" type="string" value="false"/> <!-- False: appearance-based -->
<param name="Rtabmap/ImageBufferSize" type="string" value="0"/> <!-- process all images --> <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="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/RehearsalSimilarity" type="string" value="0.4"/> <!-- 40% -->
<param name="Mem/STMSize" type="string" value="15"/> <!-- 15 locations in short-term memory --> <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/RehearsalIdUpdatedToNewOne" type="string" value="true"/> <!-- On merging, update to new ID--> <param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
<param name="Mem/BadSignaturesIgnored" type="string" value="true"/> <param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree --> <!-- 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> </node>
<!-- visualization of the "infoEx" topic sent by rtabmap 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 --> <!-- 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"> <node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
<remap from="image" to="image"/> <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="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="frame_rate" value="2.0" type="double"/>
<param name="width" value="0" type="int"/> <param name="width" value="0" type="int"/>
<param name="height" 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="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
</node> </node>
</launch> </launch>
launch/demo/demo_data_recorder.launch
+1 -2
View File
@@ -10,7 +10,7 @@
<arg name="subscribe_odometry" value="true"/> <arg name="subscribe_odometry" value="true"/>
<arg name="subscribe_depth" value="true"/> <arg name="subscribe_depth" value="true"/>
<arg name="subscribe_stereo" value="false"/> <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="frame_id" value="base_footprint"/>
<arg name="odom_frame_id" value=""/> <!-- use topic when not set, otherwise use TF if set --> <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="queue_size" value="10"/>
<arg name="max_rate" value="0"/> <arg name="max_rate" value="0"/>
<arg name="camera_prefix" value="/camera"/>
<arg name="odom_topic" value="/az3/base_controller/odom"/> <arg name="odom_topic" value="/az3/base_controller/odom"/>
<arg name="scan_topic" value="/jn0/base_scan"/> <arg name="scan_topic" value="/jn0/base_scan"/>
</include> </include>
launch/demo/demo_find_object.launch
+51 -31
View File
@@ -1,6 +1,10 @@
<launch> <launch>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<param name="use_sim_time" type="bool" value="True"/> <param name="use_sim_time" type="bool" value="True"/>
<!-- SLAM (robot side) --> <!-- SLAM (robot side) -->
@@ -10,39 +14,56 @@
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<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="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/> <remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/> <remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/> <remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/> <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/> <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="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/> <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. --> <!-- 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/NeighborLinkRefining" 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/ProximityBySpace" type="string" value="true"/> <!-- Proximity 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="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="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D --> <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.9"/> <param name="Icp/Iterations" type="string" value="30"/>
<param name="LccIcp2/MaxFitness" type="string" value="0.1"/> <param name="Icp/VoxelSize" type="string" value="0.025"/>
<param name="LccIcp2/Iterations" type="string" value="100"/> <param name="Vis/MinInliers" type="string" value="10"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="LccIcp2/VoxelSize" type="string" value="0"/> <param name="Vis/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/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="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity --> <param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance --> <param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving --> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving --> <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/> <param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/> <param name="Reg/Force3DoF" type="string" value="true"/>
</node> </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> </group>
<!-- send AZIMUT 3 urdf to param server --> <!-- send AZIMUT 3 urdf to param server -->
@@ -51,16 +72,15 @@
--> -->
<!-- Visualisation --> <!-- 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="standalone rtabmap_ros/point_cloud_xyzrgb">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="/camera/data_throttled_image"/> <remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/> <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" /> <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="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
@@ -69,16 +89,16 @@
<!-- Find-Object --> <!-- Find-Object -->
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen"> <node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/> <param name="gui" value="true" type="bool"/>
<param name="settings_path" value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/> <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="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="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/image_raw" to="/camera/data_throttled_image_depth"/>
<remap from="depth_registered/camera_info" to="/camera/data_throttled_camera_info"/> <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"/> <param name="depth_registered/image_transport" type="string" value="compressedDepth"/>
</node> </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="frame_id" type="string" value="base_footprint"/>
<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="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/scanmatch_odom"/> <remap from="odom" to="/scanmatch_odom"/>
<remap from="scan" to="/jn0/base_scan"/> <remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/> <remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/> <remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <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="depth/image_transport" type="string" value="compressedDepth"/>
<!-- RTAB-Map's parameters --> <!-- RTAB-Map's parameters -->
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D --> <param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="LccBow/MaxDepth" type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity --> <param name="Vis/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="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> </node>
<!-- Visualisation RTAB-Map --> <!-- 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"> <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="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="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/> <remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/jn0/base_scan"/> <remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/scanmatch_odom"/> <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"/> <param name="depth/image_transport" type="string" value="compressedDepth"/>
</node> </node>
@@ -93,7 +95,7 @@
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" /> <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="depth/image_transport" type="string" value="compressedDepth"/>
<param name="voxel_size" type="double" value="0.01"/> <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="frame_id" type="string" value="base_footprint"/>
<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="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/> <remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/> <remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/> <remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/> <remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <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="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/> <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. --> <!-- 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/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/> <param name="RGBD/ProximityBySpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <param name="RGBD/ProximityByTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <param name="Reg/Strategy" type="string" value="1"/>
<param name="LccIcp/Type" type="string" value="2"/> <param name="Icp/Iterations" type="string" value="30"/>
<param name="LccIcp2/Iterations" type="string" value="100"/> <param name="Icp/VoxelSize" type="string" value="0"/>
<param name="LccIcp2/VoxelSize" type="string" value="0"/> <param name="Vis/MinInliers" type="string" value="5"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <param name="Vis/MaxDepth" type="string" value="4.0"/>
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Kp/TfIdfLikelihoodUsed" type="string" value="false"/>
<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="Bayes/FullPredictionUpdate" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF --> <param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree --> <param name="Kp/MaxFeatures" type="string" value="400"/>
<param name="Kp/WordsPerImage" type="string" value="400"/> <param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- 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"> <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="subscribe_scan" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
<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="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/> <remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/base_scan"/> <remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/> <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"/> <param name="depth/image_transport" type="string" value="compressedDepth"/>
</node> </node>
@@ -81,7 +80,7 @@
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" /> <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="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/> <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
+34 -22
View File
@@ -11,50 +11,62 @@
<param name="use_sim_time" type="bool" value="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"> <group ns="rtabmap">
<!-- SLAM (robot side) --> <!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" --> <!-- 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="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/> <param name="wait_for_transform" type="bool" value="true"/>
<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="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/az3/base_controller/odom"/> <remap from="odom" to="/az3/base_controller/odom"/>
<remap from="scan" to="/jn0/base_scan"/> <remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/> <remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/> <remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <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="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. --> <!-- 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/NeighborLinkRefining" 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/ProximityBySpace" 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/ProximityByTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> <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="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance --> <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="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
</node> <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 --> <!-- 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"> <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="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/> <param name="wait_for_transform" type="bool" value="true"/>
<remap from="rgb/image" to="/data_throttled_image"/> <remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/> <remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/jn0/base_scan"/> <remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/az3/base_controller/odom"/> <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"/> <param name="depth/image_transport" type="string" value="compressedDepth"/>
</node> </node>
</group> </group>
@@ -67,7 +79,7 @@
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" /> <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="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
launch/demo/demo_stereo_outdoor.launch
+46 -65
View File
@@ -21,7 +21,7 @@
<param name="use_sim_time" type="bool" value="True"/> <param name="use_sim_time" type="bool" value="True"/>
<!-- Just to uncompress images for stereo_image_rect --> <!-- 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" /> <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 --> <!-- Run the ROS package stereo_image_proc for image rectification -->
@@ -37,87 +37,68 @@
</node> </node>
</group> </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"> <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" --> <!-- Visual SLAM: 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="--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_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="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/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="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/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"/> <param name="queue_size" type="int" value="30"/>
<!-- RTAB-Map's parameters --> <!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="Kp/MaxDepth" type="string" value="10"/>
<param name="Kp/WordsPerImage" type="string" value="200"/> <param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> <param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF --> <param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree --> <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="10"/>
<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"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- 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"> <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="subscribe_odom_info" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/> <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/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/> <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="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom_info" to="/odom_info"/> <remap from="odom_info" to="odom_info"/>
<remap from="odom" to="/odometry"/> <remap from="odom" to="/stereo_odometry"/>
<remap from="mapData" to="mapData_optimized"/> <remap from="mapData" to="mapData"/>
</node> </node>
</group> </group>
launch/demo/demo_turtlebot_mapping.launch
+23 -22
View File
@@ -25,9 +25,13 @@
<arg name="localization" default="false"/> <arg name="localization" default="false"/>
<arg name="rgbd_odometry" default="false"/> <arg name="rgbd_odometry" default="false"/>
<arg name="args" default=""/> <arg name="args" default=""/>
<arg name="version083" default="false"/>
<arg name="rtabmapviz" 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) --> <!-- Navigation stuff (move_base) -->
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/> <include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>
@@ -42,7 +46,7 @@
<param name="odom_frame_id" type="string" value="odom"/> <param name="odom_frame_id" type="string" value="odom"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<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="subscribe_scan" type="bool" value="true"/>
<!-- inputs --> <!-- inputs -->
<remap from="scan" to="/scan"/> <remap from="scan" to="/scan"/>
@@ -51,21 +55,22 @@
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<!-- output --> <!-- output -->
<remap unless="$(arg version083)" from="grid_map" to="/map"/> <remap from="grid_map" to="/map"/>
<!-- <remap unless="$(arg version083)" from="proj_map" to="/map"/> -->
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. --> <!-- 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="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="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="Reg/Strategy" type="string" value="1"/> <!-- Loop closure transformation refining with ICP: 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.3"/> <param name="Icp/CoprrespondenceRatio" type="string" value="0.3"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure --> <param name="Vis/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="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/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/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/> <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 --> <!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/> <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. --> <!-- 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"> <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="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/Force2D" type="string" value="true"/> <param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Odom/InlierDistance" type="string" value="0.05"/> <param name="Vis/InlierDistance" type="string" value="0.05"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node> </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 --> <!-- 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"> <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="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> <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" /> <arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS: <!-- 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 -"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 -"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF Set to 1 for float descriptor like SIFT/SURF
@@ -63,12 +63,12 @@
<param name="depth_cameras" type="int" value="2"/> <param name="depth_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/> <param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/> <param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/> <param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/> <param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/> <param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/> <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/> <param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node> </node>
@@ -89,8 +89,8 @@
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/> <remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/> <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="Vis/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/> <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
launch/rgbd_mapping.launch
+40 -46
View File
@@ -9,6 +9,9 @@
<arg name="rviz" default="false" /> <arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" /> <arg name="rtabmapviz" default="true" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- Corresponding config files --> <!-- Corresponding config files -->
<arg name="rtabmapviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" /> <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" /> <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="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="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="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false --> <arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/> <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 --> <!-- Nodes -->
<group ns="$(arg namespace)"> <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)" /> <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 --> <!-- 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="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <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="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/> <param name="Odom/FillInfoData" type="string" value="true"/>
<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)"/>
</node> </node>
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)"> <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_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/> <param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<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="database_path" type="string" value="$(arg database_path)"/> <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="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_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)"/> <remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/> <param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="LccBow/EstimationType" type="string" value="$(arg estimation)"/> <!-- localization mode -->
<param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/> <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/> <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 --> <!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D" type="string" value="true"/> <param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="LccIcp/Type" type="string" value="2"/> <param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/>
<param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio" type="string" value="0.25"/> <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> </node>
<!-- Visualisation RTAB-Map --> <!-- 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)"> <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_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/> <param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/> <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <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="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/> <remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_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)"/> <remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node> </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" /> <arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<!-- ODOMETRY MAIN ARGUMENTS: <!-- 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 -"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 -"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF Set to 1 for float descriptor like SIFT/SURF
@@ -63,14 +63,14 @@
<param name="approx_sync" type="bool" value="true"/> <param name="approx_sync" type="bool" value="true"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/> <param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/> <param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/> <param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/> <param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/> <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/> <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)"/> <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
</node> </node>
@@ -86,8 +86,8 @@
<param name="approx_sync" type="bool" value="true"/> <param name="approx_sync" type="bool" value="true"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/> <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- 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
+42 -46
View File
@@ -9,6 +9,9 @@
<arg name="rtabmapviz" default="true" /> <arg name="rtabmapviz" default="true" />
<arg name="rviz" default="false" /> <arg name="rviz" default="false" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- Corresponding config files --> <!-- Corresponding config files -->
<arg name="rtabmapviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" /> <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" /> <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="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="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/>
<arg name="stereo_namespace" default="/stereo_camera"/> <arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" /> <arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
@@ -26,27 +30,19 @@
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" /> <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="approximate_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/> <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="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="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="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false --> <arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/> <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 --> <!-- Nodes -->
<group ns="$(arg namespace)"> <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)" /> <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 --> <!-- 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="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_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="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="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/> <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/FillInfoData" type="string" value="true"/>
<param name="Odom/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
</node> </node>
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" --> <!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)"> <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_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/> <param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/> <param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <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="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/> <param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/> <param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/> <remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_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="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_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)"/> <remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/> <param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="LccBow/EstimationType" type="string" value="$(arg estimation)"/> <!-- localization mode -->
<param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/> <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 --> <!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D" type="string" value="true"/> <param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="LccIcp/Type" type="string" value="2"/> <param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/>
<param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio" type="string" value="0.25"/> <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> </node>
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen"> <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_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/> <param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/> <param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/> <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <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="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/> <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="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_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)"/> <remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node> </node>
launch/tests/bumblebee.launch
+7 -7
View File
@@ -10,15 +10,16 @@
<param name="camera_info_url_right" value="" /> <param name="camera_info_url_right" value="" />
</node> </node>
<arg name="gen_depth" default="false"/>
<arg name="pi/2" value="1.5707963267948966" /> <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" <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" /> 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) --> <!-- Run the ROS package stereo_image_proc (throttle to 10 Hz to avoid rectifying all images) -->
<group ns="/stereo_camera" > <group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle"> <node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
<remap from="left/image" to="left/image_raw"/> <remap from="left/image" to="left/image_raw"/>
<remap from="right/image" to="right/image_raw"/> <remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/> <remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/> <remap from="right/camera_info" to="right/camera_info"/>
@@ -28,13 +29,12 @@
</node> </node>
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"> <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="left/camera_info" to="left/camera_info_throttle"/>
<remap from="right/image_raw" to="right/image_raw_throttle"/> <remap from="right/image_raw" to="right/image_raw_throttle"/>
<remap from="right/camera_info" to="right/camera_info_throttle"/> <remap from="right/camera_info" to="right/camera_info_throttle"/>
</node> </node>
<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
</group> </group>
</launch> </launch>
launch/tests/rgbdslam_datasets.launch
+19 -22
View File
@@ -11,19 +11,6 @@
<param name="use_sim_time" type="bool" value="True"/> <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 --> <!-- Choose visualization -->
<arg name="rviz" default="true" /> <arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" /> <arg name="rtabmapviz" default="false" />
@@ -40,13 +27,18 @@
<remap from="rgb/image" to="/camera/rgb/image_color"/> <remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/> <remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <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/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/> <param name="Vis/CorType" type="string" value="0"/> <!-- 0=features matching 1=Optical Flow -->
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/> <param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/> <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="frame_id" type="string" value="kinect"/>
<param name="publish_tf" type="bool" value="false"/> <param name="publish_tf" type="bool" value="false"/>
<param name="queue_size" type="int" value="30"/> <param name="queue_size" type="int" value="30"/>
@@ -64,15 +56,20 @@
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/> <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="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="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/> <remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="odom"/> <remap from="odom" to="vis_odom"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.05"/>
<param name="queue_size" type="int" value="30"/> <param name="queue_size" type="int" value="30"/>
</node> </node>
@@ -89,7 +86,7 @@
<remap from="rgb/image" to="/camera/rgb/image_color"/> <remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/> <remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="odom"/> <remap from="odom" to="vis_odom"/>
</node> </node>
</group> </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 refId
int32 loopClosureId int32 loopClosureId
int32 localLoopClosureId int32 proximityDetectionId
geometry_msgs/Transform loopClosureTransform geometry_msgs/Transform loopClosureTransform
msg/NodeData.msg
+3
View File
@@ -8,6 +8,9 @@ string label
# Pose from odometry not corrected # Pose from odometry not corrected
geometry_msgs/Pose pose geometry_msgs/Pose pose
# Ground truth (optional)
geometry_msgs/Pose groundTruthPose
# compressed image in /camera_link frame # compressed image in /camera_link frame
# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h" # use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
uint8[] image uint8[] image
msg/OdomInfo.msg
+2
View File
@@ -42,6 +42,8 @@ int32[] wordsKeys
KeyPoint[] wordsValues KeyPoint[] wordsValues
int32[] wordMatches int32[] wordMatches
int32[] wordInliers int32[] wordInliers
int32[] localMapKeys
Point3f[] localMapValues
Point2f[] refCorners Point2f[] refCorners
Point2f[] newCorners 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"?> <?xml version="1.0"?>
<package> <package>
<name>rtabmap_ros</name> <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> <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> <maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author> <author>Mathieu Labbe</author>
@@ -37,7 +37,8 @@
<build_depend>class_loader</build_depend> <build_depend>class_loader</build_depend>
<build_depend>rtabmap</build_depend> <build_depend>rtabmap</build_depend>
<build_depend>move_base_msgs</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_ros</build_depend>
<build_depend>octomap</build_depend> <build_depend>octomap</build_depend>
@@ -67,7 +68,7 @@
<run_depend>class_loader</run_depend> <run_depend>class_loader</run_depend>
<run_depend>rtabmap</run_depend> <run_depend>rtabmap</run_depend>
<run_depend>move_base_msgs</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_ros</run_depend>
<run_depend>octomap</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)) if(!path.empty() && UDirectory::exists(path))
{ {
//images //images
camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate); camera_ = new rtabmap::CameraImages(path, frameRate);
} }
else if(!path.empty() && UFile::exists(path)) 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; 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) else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
{ {
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -94,14 +86,11 @@ int main(int argc, char** argv)
"argument \"--params\" is detected!"); "argument \"--params\" is detected!");
exit(0); 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_INFO("rtabmap %s started...", RTABMAP_VERSION);
ros::spin(); 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 class CoreWrapper
{ {
public: public:
CoreWrapper(bool deleteDbOnStart); CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters);
virtual ~CoreWrapper(); virtual ~CoreWrapper();
private: private:
void setupCallbacks( void setupCallbacks(
bool subscribeDepth, bool subscribeDepth,
bool subscribeLaserScan, bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeStereo, bool subscribeStereo,
int queueSize, int queueSize,
bool stereoApproxSync, bool stereoApproxSync,
@@ -102,20 +103,23 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg); const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
void commonDepthCallback( void commonDepthCallback(
const std::string & odomFrameId, const std::string & odomFrameId,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs, const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs, const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs, const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg); const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
void commonStereoCallback( void commonStereoCallback(
const std::string & odomFrameId, const std::string & odomFrameId,
const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg); const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
// with odom msg // with odom msg
void depthCallback( void depthCallback(
@@ -129,6 +133,12 @@ private:
const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg); 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( void stereoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -142,6 +152,13 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg); 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( void depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg, const sensor_msgs::ImageConstPtr& image1Msg,
@@ -161,6 +178,11 @@ private:
const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg); 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( void stereoTFCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -172,8 +194,14 @@ private:
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg); 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 goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg); void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
void updateGoal(const ros::Time & stamp); void updateGoal(const ros::Time & stamp);
@@ -183,8 +211,8 @@ private:
const rtabmap::SensorData & data, const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(), const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "", const std::string & odomFrameId = "",
double odomRotationalVariance = 1.0, float odomRotationalVariance = 1.0,
double odomTransitionalVariance = 1.0); float odomTransitionalVariance = 1.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(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 backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeLocalizationCallback(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 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 getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::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); bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
@@ -225,8 +257,9 @@ private:
bool paused_; bool paused_;
rtabmap::Transform lastPose_; rtabmap::Transform lastPose_;
ros::Time lastPoseStamp_; ros::Time lastPoseStamp_;
double rotVariance_; bool lastPoseIntermediate_;
double transVariance_; float rotVariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_; rtabmap::Transform currentMetricGoal_;
bool latestNodeWasReached_; bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
@@ -234,6 +267,7 @@ private:
std::string frameId_; std::string frameId_;
std::string mapFrameId_; std::string mapFrameId_;
std::string odomFrameId_; std::string odomFrameId_;
std::string groundTruthFrameId_;
std::string configPath_; std::string configPath_;
std::string databasePath_; std::string databasePath_;
bool waitForTransform_; bool waitForTransform_;
@@ -241,6 +275,7 @@ private:
bool useActionForGoal_; bool useActionForGoal_;
bool genScan_; bool genScan_;
double genScanMaxDepth_; double genScanMaxDepth_;
double genScanMinDepth_;
rtabmap::Transform mapToOdom_; rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_; boost::mutex mapToOdomMutex_;
@@ -276,6 +311,7 @@ private:
message_filters::Subscriber<nav_msgs::Odometry> odomSub_; message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_; message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
@@ -285,6 +321,14 @@ private:
sensor_msgs::LaserScan> MyDepthScanSyncPolicy; sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
nav_msgs::Odometry, nav_msgs::Odometry,
@@ -301,6 +345,15 @@ private:
nav_msgs::Odometry> MyStereoScanSyncPolicy; nav_msgs::Odometry> MyStereoScanSyncPolicy;
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
@@ -335,6 +388,13 @@ private:
sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy; sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
@@ -349,6 +409,14 @@ private:
sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy; sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
@@ -374,6 +442,10 @@ private:
ros::ServiceServer backupDatabase_; ros::ServiceServer backupDatabase_;
ros::ServiceServer setModeLocalizationSrv_; ros::ServiceServer setModeLocalizationSrv_;
ros::ServiceServer setModeMappingSrv_; ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer setLogDebugSrv_;
ros::ServiceServer setLogInfoSrv_;
ros::ServiceServer setLogWarnSrv_;
ros::ServiceServer setLogErrorSrv_;
ros::ServiceServer getMapDataSrv_; ros::ServiceServer getMapDataSrv_;
ros::ServiceServer getProjMapSrv_; ros::ServiceServer getProjMapSrv_;
ros::ServiceServer getGridMapSrv_; ros::ServiceServer getGridMapSrv_;
@@ -392,7 +464,9 @@ private:
boost::thread* transformThread_; boost::thread* transformThread_;
float rate_; float rate_;
bool createIntermediateNodes_;
ros::Time time_; ros::Time time_;
ros::Time previousStamp_;
}; };
#endif /* COREWRAPPER_H_ */ #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 odomFrameId = "odom";
std::string cameraFrameId = "camera_optical_link"; std::string cameraFrameId = "camera_optical_link";
std::string scanFrameId = "base_laser_link"; std::string scanFrameId = "base_laser_link";
double rate = 1.0f; double rate = -1.0f;
std::string databasePath = ""; std::string databasePath = "";
bool publishTf = true; bool publishTf = true;
int startId = 0; 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) else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{ {
//stereo //stereo
if(odom.data().stereoCameraModel().isValid()) if(odom.data().stereoCameraModel().isValidForProjection())
{ {
camInfoA.D.resize(8,0); camInfoA.D.resize(8,0);
@@ -257,14 +257,12 @@ int main(int argc, char** argv)
// publish transforms first // publish transforms first
if(publishTf) if(publishTf)
{ {
ros::Time tfExpiration = time + ros::Duration(rate>0?1.0/rate:acquisitionTime);
rtabmap::Transform localTransform; rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1) if(odom.data().cameraModels().size() == 1)
{ {
localTransform = odom.data().cameraModels()[0].localTransform(); localTransform = odom.data().cameraModels()[0].localTransform();
} }
else if(odom.data().stereoCameraModel().isValid()) else if(odom.data().stereoCameraModel().isValidForProjection())
{ {
localTransform = odom.data().stereoCameraModel().left().localTransform(); localTransform = odom.data().stereoCameraModel().left().localTransform();
} }
@@ -273,7 +271,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped baseToCamera; geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId; baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId; baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = tfExpiration; baseToCamera.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform); rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera); tfBroadcaster.sendTransform(baseToCamera);
} }
@@ -283,7 +281,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped odomToBase; geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId; odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId; odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = tfExpiration; odomToBase.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform); rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase); tfBroadcaster.sendTransform(odomToBase);
} }
@@ -293,7 +291,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped baseToLaserScan; geometry_msgs::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.child_frame_id = scanFrameId;
baseToLaserScan.header.frame_id = frameId; 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); rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform);
tfBroadcaster.sendTransform(baseToLaserScan); 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 <rtabmap/utilite/ULogger.h>
#include <signal.h> #include <signal.h>
QApplication * app = 0;
ros::AsyncSpinner * spinner = 0;
void my_handler(int s){ 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) int main(int argc, char** argv)
@@ -46,7 +51,10 @@ int main(int argc, char** argv)
ros::init(argc, argv, "rtabmapviz"); 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 // Catch ctrl-c to close the gui
// (Place this after QApplication's constructor) // (Place this after QApplication's constructor)
@@ -57,15 +65,18 @@ int main(int argc, char** argv)
sigaction(SIGINT, &sigIntHandler, NULL); sigaction(SIGINT, &sigIntHandler, NULL);
// Here start the ROS events loop // Here start the ROS events loop
ros::AsyncSpinner spinner(4); // Use 4 threads spinner = new ros::AsyncSpinner(1); // Use 1 thread
spinner.start(); spinner->start();
ROS_INFO("rtabmapviz started."); ROS_INFO("rtabmapviz started.");
// Now wait for application to finish // 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..."); ROS_INFO("rtabmapviz: All done! Closing...");
return r; 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 "GuiWrapper.h"
#include <QtGui/QApplication> #include <QApplication>
#include <QtCore/QDir> #include <QDir>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include <std_srvs/Empty.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 <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/gui/MainWindow.h>
#include <rtabmap/core/RtabmapEvent.h> #include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.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) : GuiWrapper::GuiWrapper(int & argc, char** argv) :
app_(0),
mainWindow_(0), mainWindow_(0),
frameId_("base_link"), frameId_("base_link"),
waitForTransform_(true), waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms waitForTransformDuration_(0.2), // 200 ms
cameraNodeName_(""), cameraNodeName_(""),
lastOdomInfoUpdateTime_(0), lastOdomInfoUpdateTime_(0),
depthScanSync_(0), depthScanSync_(0),
@@ -88,7 +84,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
depthOdomInfo2Sync_(0) depthOdomInfo2Sync_(0)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
app_ = new QApplication(argc, argv);
QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini"; QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
for(int i=1; i<argc; ++i) for(int i=1; i<argc; ++i)
@@ -114,12 +109,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
bool paused = false; bool paused = false;
nh.param("is_rtabmap_paused", paused, paused); nh.param("is_rtabmap_paused", paused, paused);
mainWindow_->setMonitoringState(paused); mainWindow_->setMonitoringState(paused);
app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
// To receive odometry events // To receive odometry events
bool subscribeLaserScan = false; bool subscribeLaserScan2d = false;
bool subscribeLaserScan3d = false;
bool subscribeDepth = false; bool subscribeDepth = false;
bool subscribeOdomInfo = false; bool subscribeOdomInfo = false;
bool subscribeStereo = false; bool subscribeStereo = false;
@@ -130,7 +125,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); 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_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
pnh.param("depth_cameras", depthCameras, depthCameras); pnh.param("depth_cameras", depthCameras, depthCameras);
@@ -181,7 +181,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
this->setupCallbacks( this->setupCallbacks(
subscribeDepth, subscribeDepth,
subscribeLaserScan, subscribeLaserScan2d,
subscribeLaserScan3d,
subscribeOdomInfo, subscribeOdomInfo,
subscribeStereo, subscribeStereo,
queueSize, queueSize,
@@ -210,6 +211,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
GuiWrapper::~GuiWrapper() GuiWrapper::~GuiWrapper()
{ {
UDEBUG("");
if(depthSync_) if(depthSync_)
delete depthSync_; delete depthSync_;
if(depth2Sync_) if(depth2Sync_)
@@ -245,12 +247,6 @@ GuiWrapper::~GuiWrapper()
delete infoMapSync_; delete infoMapSync_;
delete mainWindow_; delete mainWindow_;
delete app_;
}
int GuiWrapper::exec()
{
return app_->exec();
} }
void GuiWrapper::infoMapCallback( void GuiWrapper::infoMapCallback(
@@ -292,7 +288,7 @@ void GuiWrapper::goalPathCallback(
poses[i].first = -int(i)-1; poses[i].first = -int(i)-1;
poses[i].second = rtabmap_ros::transformFromPoseMsg(pathMsg->poses[i].pose); 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( void GuiWrapper::goalReachedCallback(
@@ -441,7 +437,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
poses[i].first = setGoalSrv.response.path_ids[i]; poses[i].first = setGoalSrv.response.path_ids[i];
poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[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) 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(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_))) 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; return transform;
} }
} }
@@ -510,7 +507,8 @@ void GuiWrapper::commonDepthCallback(
const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, 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) const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{ {
std::vector<sensor_msgs::ImageConstPtr> imageMsgs; std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
@@ -519,7 +517,7 @@ void GuiWrapper::commonDepthCallback(
imageMsgs.push_back(imageMsg); imageMsgs.push_back(imageMsg);
depthMsgs.push_back(depthMsg); depthMsgs.push_back(depthMsg);
cameraInfoMsgs.push_back(cameraInfoMsg); cameraInfoMsgs.push_back(cameraInfoMsg);
commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg); commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg);
} }
void GuiWrapper::commonDepthCallback( void GuiWrapper::commonDepthCallback(
@@ -527,7 +525,8 @@ void GuiWrapper::commonDepthCallback(
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs, const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs, const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs, 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) const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{ {
if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 && if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
@@ -547,9 +546,13 @@ void GuiWrapper::commonDepthCallback(
} }
else 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()) else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
{ {
@@ -679,22 +682,15 @@ void GuiWrapper::commonDepthCallback(
return; return;
} }
image_geometry::PinholeCameraModel model; cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
} }
} }
cv::Mat scan; cv::Mat scan;
if(scanMsg.get() != 0) if(scan2dMsg.get() != 0)
{ {
// make sure the frame of the laser is updated too // 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; return;
} }
@@ -702,16 +698,16 @@ void GuiWrapper::commonDepthCallback(
//transform in frameId_ frame //transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut; sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection; 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::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan); pcl::fromROSMsg(scanOut, *pclScan);
// sync with odometry stamp // sync with odometry stamp
if(odomHeader.stamp != scanMsg->header.stamp) if(odomHeader.stamp != scan2dMsg->header.stamp)
{ {
if(!odomT.isNull()) 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()) if(sensorT.isNull())
{ {
return; return;
@@ -723,6 +719,12 @@ void GuiWrapper::commonDepthCallback(
} }
scan = util3d::laserScanFromPointCloud(*pclScan); 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; rtabmap::OdometryInfo info;
if(odomInfoMsg.get()) if(odomInfoMsg.get())
@@ -733,8 +735,8 @@ void GuiWrapper::commonDepthCallback(
rtabmap::OdometryEvent odomEvent( rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData( rtabmap::SensorData(
scan, scan,
scanMsg.get()?(int)scanMsg->ranges.size():0, scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scanMsg.get()?(int)scanMsg->range_max:0, scan2dMsg.get()?(int)scan2dMsg->range_max:0,
rgb, rgb,
depth, depth,
cameraModels, cameraModels,
@@ -754,7 +756,8 @@ void GuiWrapper::commonStereoCallback(
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, 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) const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{ {
// limit 10 Hz max // limit 10 Hz max
@@ -787,9 +790,13 @@ void GuiWrapper::commonStereoCallback(
} }
else else
{ {
if(scanMsg.get()) if(scan2dMsg.get())
{ {
odomHeader = scanMsg->header; odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
} }
else else
{ {
@@ -838,17 +845,9 @@ void GuiWrapper::commonStereoCallback(
} }
} }
image_geometry::StereoCameraModel model; rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
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; static bool shown = false;
if(!shown) if(!shown)
@@ -856,7 +855,7 @@ void GuiWrapper::commonStereoCallback(
ROS_WARN("Detected baseline (%f m) is quite large! Is your " ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that " "right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.", "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline()); stereoModel.baseline());
shown = true; shown = true;
} }
} }
@@ -878,10 +877,10 @@ void GuiWrapper::commonStereoCallback(
cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
cv::Mat scan; cv::Mat scan;
if(scanMsg.get() != 0) if(scan2dMsg.get() != 0)
{ {
// make sure the frame of the laser is updated too // 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; return;
} }
@@ -889,16 +888,16 @@ void GuiWrapper::commonStereoCallback(
//transform in frameId_ frame //transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut; sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection; 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::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan); pcl::fromROSMsg(scanOut, *pclScan);
// sync with odometry stamp // sync with odometry stamp
if(odomHeader.stamp != scanMsg->header.stamp) if(odomHeader.stamp != scan2dMsg->header.stamp)
{ {
if(!odomT.isNull()) 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()) if(sensorT.isNull())
{ {
return; 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); scan = util3d::laserScanFromPointCloud(*pclScan);
} }
@@ -920,8 +925,8 @@ void GuiWrapper::commonStereoCallback(
rtabmap::OdometryEvent odomEvent( rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData( rtabmap::SensorData(
scan, scan,
scanMsg.get()?(int)scanMsg->ranges.size():0, scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scanMsg.get()?(int)scanMsg->range_max:0, scan2dMsg.get()?(int)scan2dMsg->range_max:0,
left, left,
right, right,
stereoModel, stereoModel,
@@ -944,6 +949,7 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
sensor_msgs::ImageConstPtr(), sensor_msgs::ImageConstPtr(),
sensor_msgs::CameraInfoConstPtr(), sensor_msgs::CameraInfoConstPtr(),
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -959,6 +965,7 @@ void GuiWrapper::depthCallback(
depthMsg, depthMsg,
cameraInfoMsg, cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -987,6 +994,7 @@ void GuiWrapper::depth2Callback(
depthMsgs, depthMsgs,
cameraInfoMsgs, cameraInfoMsgs,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -1003,6 +1011,7 @@ void GuiWrapper::depthOdomInfoCallback(
depthMsg, depthMsg,
cameraInfoMsg, cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg); odomInfoMsg);
} }
@@ -1032,6 +1041,7 @@ void GuiWrapper::depthOdomInfo2Callback(
depthMsgs, depthMsgs,
cameraInfoMsgs, cameraInfoMsgs,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg); odomInfoMsg);
} }
@@ -1048,9 +1058,63 @@ void GuiWrapper::depthScanCallback(
depthMsg, depthMsg,
cameraInfoMsg, cameraInfoMsg,
scanMsg, scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); 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( void GuiWrapper::stereoScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1066,9 +1130,69 @@ void GuiWrapper::stereoScanCallback(
leftCameraInfoMsg, leftCameraInfoMsg,
rightCameraInfoMsg, rightCameraInfoMsg,
scanMsg, scanMsg,
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); 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( void GuiWrapper::stereoOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1084,6 +1208,7 @@ void GuiWrapper::stereoOdomInfoCallback(
leftCameraInfoMsg, leftCameraInfoMsg,
rightCameraInfoMsg, rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg); odomInfoMsg);
} }
@@ -1101,6 +1226,7 @@ void GuiWrapper::stereoCallback(
leftCameraInfoMsg, leftCameraInfoMsg,
rightCameraInfoMsg, rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -1116,6 +1242,7 @@ void GuiWrapper::depthTFCallback(
depthMsg, depthMsg,
cameraInfoMsg, cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -1131,6 +1258,7 @@ void GuiWrapper::depthOdomInfoTFCallback(
depthMsg, depthMsg,
cameraInfoMsg, cameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg); odomInfoMsg);
} }
@@ -1146,6 +1274,23 @@ void GuiWrapper::depthScanTFCallback(
depthMsg, depthMsg,
cameraInfoMsg, cameraInfoMsg,
scanMsg, 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()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -1163,6 +1308,25 @@ void GuiWrapper::stereoScanTFCallback(
leftCameraInfoMsg, leftCameraInfoMsg,
rightCameraInfoMsg, rightCameraInfoMsg,
scanMsg, 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()); rtabmap_ros::OdomInfoConstPtr());
} }
@@ -1180,6 +1344,7 @@ void GuiWrapper::stereoOdomInfoTFCallback(
leftCameraInfoMsg, leftCameraInfoMsg,
rightCameraInfoMsg, rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
odomInfoMsg); odomInfoMsg);
} }
@@ -1196,12 +1361,14 @@ void GuiWrapper::stereoTFCallback(
leftCameraInfoMsg, leftCameraInfoMsg,
rightCameraInfoMsg, rightCameraInfoMsg,
sensor_msgs::LaserScanConstPtr(), sensor_msgs::LaserScanConstPtr(),
sensor_msgs::PointCloud2ConstPtr(),
rtabmap_ros::OdomInfoConstPtr()); rtabmap_ros::OdomInfoConstPtr());
} }
void GuiWrapper::setupCallbacks( void GuiWrapper::setupCallbacks(
bool subscribeDepth, bool subscribeDepth,
bool subscribeLaserScan, bool subscribeLaserScan2d,
bool subscribeLaserScan3d,
bool subscribeOdomInfo, bool subscribeOdomInfo,
bool subscribeStereo, bool subscribeStereo,
int queueSize, int queueSize,
@@ -1212,9 +1379,11 @@ void GuiWrapper::setupCallbacks(
if(subscribeDepth && subscribeStereo) 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..."); ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
} }
@@ -1231,7 +1400,7 @@ void GuiWrapper::setupCallbacks(
if(subscribeDepth) if(subscribeDepth)
{ {
UASSERT(depthCameras >= 1 && depthCameras <= 2); 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); imageSubs_.resize(depthCameras);
imageDepthSubs_.resize(depthCameras); imageDepthSubs_.resize(depthCameras);
@@ -1265,25 +1434,95 @@ void GuiWrapper::setupCallbacks(
if(odomFrameId_.empty()) if(odomFrameId_.empty())
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeLaserScan) if(subscribeLaserScan2d)
{ {
scanSub_.subscribe(nh, "scan", 1); scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>( if(subscribeOdomInfo)
MyDepthScanSyncPolicy(queueSize), {
scanSub_, odomInfoSub_.subscribe(nh, "odom_info", 1);
odomSub_, depthScanOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy>(
*imageSubs_[0], MyDepthScanOdomInfoSyncPolicy(queueSize),
*imageDepthSubs_[0], odomInfoSub_,
*cameraInfoSubs_[0]); scanSub_,
depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); 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_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(), imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(), odomSub_.getTopic().c_str(),
scanSub_.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) else if(subscribeOdomInfo)
{ {
@@ -1380,7 +1619,7 @@ void GuiWrapper::setupCallbacks(
else else
{ {
// use TF as odom // use TF as odom
if(subscribeLaserScan) if(subscribeLaserScan2d)
{ {
scanSub_.subscribe(nh, "scan", 1); scanSub_.subscribe(nh, "scan", 1);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>( depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
@@ -1398,6 +1637,24 @@ void GuiWrapper::setupCallbacks(
cameraInfoSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(),
scanSub_.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) else if(subscribeOdomInfo)
{ {
odomInfoSub_.subscribe(nh, "odom_info", 1); odomInfoSub_.subscribe(nh, "odom_info", 1);
@@ -1452,27 +1709,103 @@ void GuiWrapper::setupCallbacks(
if(odomFrameId_.empty()) if(odomFrameId_.empty())
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeLaserScan) if(subscribeLaserScan2d)
{ {
scanSub_.subscribe(nh, "scan", 1); scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>( if(subscribeOdomInfo)
MyStereoScanSyncPolicy(queueSize), {
scanSub_, odomInfoSub_.subscribe(nh, "odom_info", 1);
odomSub_, stereoScanOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy>(
imageRectLeft_, MyStereoScanOdomInfoSyncPolicy(queueSize),
imageRectRight_, odomInfoSub_,
cameraInfoLeft_, scanSub_,
cameraInfoRight_); odomSub_,
stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6)); 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_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(), ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(), imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(), imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(), cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(), odomSub_.getTopic().c_str(),
scanSub_.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) else if(subscribeOdomInfo)
{ {
@@ -1519,7 +1852,7 @@ void GuiWrapper::setupCallbacks(
else else
{ {
//use odom TF //use odom TF
if(subscribeLaserScan) if(subscribeLaserScan2d)
{ {
scanSub_.subscribe(nh, "scan", 1); scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>( stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
@@ -1539,6 +1872,26 @@ void GuiWrapper::setupCallbacks(
cameraInfoRight_.getTopic().c_str(), cameraInfoRight_.getTopic().c_str(),
scanSub_.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) else if(subscribeOdomInfo)
{ {
odomInfoSub_.subscribe(nh, "odom_info", 1); odomInfoSub_.subscribe(nh, "odom_info", 1);
src/GuiWrapper.h
+133 -7
View File
@@ -68,8 +68,6 @@ public:
GuiWrapper(int & argc, char** argv); GuiWrapper(int & argc, char** argv);
virtual ~GuiWrapper(); virtual ~GuiWrapper();
int exec();
protected: protected:
virtual void handleEvent(UEvent * anEvent); virtual void handleEvent(UEvent * anEvent);
@@ -80,7 +78,8 @@ private:
void setupCallbacks( void setupCallbacks(
bool subscribeDepth, bool subscribeDepth,
bool subscribeLaserScan, bool subscribeLaserScan2d,
bool subscribeLaserScan3d,
bool subscribeOdomInfo, bool subscribeOdomInfo,
bool subscribeStereo, bool subscribeStereo,
int queueSize, int queueSize,
@@ -91,14 +90,16 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, 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); const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonDepthCallback( void commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs, const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs, const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs, 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); const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonStereoCallback( void commonStereoCallback(
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
@@ -106,7 +107,8 @@ private:
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, 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); const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -146,6 +148,26 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg); 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( void stereoScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -154,6 +176,29 @@ private:
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); 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( void stereoOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
@@ -182,6 +227,11 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs:: CameraInfoConstPtr& camInfoMsg); 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( void stereoScanTFCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -189,6 +239,12 @@ private:
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); 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( void stereoOdomInfoTFCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg, 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; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private: private:
QApplication * app_;
rtabmap::MainWindow * mainWindow_; rtabmap::MainWindow * mainWindow_;
std::string cameraNodeName_; std::string cameraNodeName_;
double lastOdomInfoUpdateTime_; double lastOdomInfoUpdateTime_;
@@ -231,6 +286,7 @@ private:
message_filters::Subscriber<nav_msgs::Odometry> odomSub_; message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_; message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_; message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
image_transport::SubscriberFilter imageRectLeft_; image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_; image_transport::SubscriberFilter imageRectRight_;
@@ -256,6 +312,32 @@ private:
sensor_msgs::CameraInfo> MyDepthScanSyncPolicy; sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry, nav_msgs::Odometry,
sensor_msgs::Image, sensor_msgs::Image,
@@ -288,6 +370,35 @@ private:
sensor_msgs::CameraInfo> MyStereoScanSyncPolicy; sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo, rtabmap_ros::OdomInfo,
nav_msgs::Odometry, nav_msgs::Odometry,
@@ -326,6 +437,13 @@ private:
sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy; sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
@@ -354,6 +472,14 @@ private:
sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy; sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_; 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< typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo, rtabmap_ros::OdomInfo,
sensor_msgs::Image, 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 <ros/ros.h>
#include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/MsgConversion.h"
#include "MapsManager.h"
#include <rtabmap/core/util3d_transforms.h> #include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h> #include <rtabmap/core/util3d_filtering.h>
@@ -49,54 +50,13 @@ class MapAssembler
public: public:
MapAssembler() : MapAssembler() :
cloudDecimation_(4), mapsManager_(false)
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)
{ {
ros::NodeHandle pnh("~"); 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; ros::NodeHandle nh;
mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); 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 // private service
resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this); resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
} }
@@ -108,239 +68,60 @@ public:
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{ {
UTimer timer; 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) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
int id = msg->nodes[i].id; if(msg->nodes[i].image.size() ||
if(!uContains(rgbClouds_, id)) msg->nodes[i].depth.size() ||
msg->nodes[i].laserScan.size())
{ {
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]); uInsert(nodes_, std::make_pair(msg->nodes[i].id, 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));
}
}
} }
} }
// filter poses // create a tmp signature with latest sensory data
std::map<int, Transform> poses; if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
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]))); Signature tmpS = nodes_.at(poses.rbegin()->first);
} SensorData tmpData = tmpS.sensorData();
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0) tmpData.setId(-1);
{ uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0); poses.insert(std::make_pair(-1, poses.rbegin()->second));
} }
if(assembledMapClouds_.getNumSubscribers()) // Update maps
{ poses = mapsManager_.updateMapCaches(
// generate the assembled cloud! poses,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>); 0,
false,
false,
false,
false,
nodes_);
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter) mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
{
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;
}
}
if(assembledCloud->size()) ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
{
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());
} }
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{ {
ROS_INFO("map_assembler: reset!"); ROS_INFO("map_assembler: reset!");
occupancyLocalMaps_.clear(); mapsManager_.clear();
rgbClouds_.clear();
scans_.clear();
return true; return true;
} }
private: private:
int cloudDecimation_; MapsManager mapsManager_;
double cloudMaxDepth_; std::map<int, Signature> nodes_;
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>
ros::Subscriber mapDataTopic_; ros::Subscriber mapDataTopic_;
ros::Publisher assembledMapClouds_;
ros::Publisher assembledMapScans_;
ros::Publisher occupancyMapPub_;
ros::ServiceServer resetService_; 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_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d.h>
#include <rtabmap/core/Graph.h> #include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <ros/subscriber.h> #include <ros/subscriber.h>
#include <ros/publisher.h> #include <ros/publisher.h>
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
@@ -48,8 +50,6 @@ public:
MapOptimizer() : MapOptimizer() :
mapFrameId_("map"), mapFrameId_("map"),
odomFrameId_("odom"), odomFrameId_("odom"),
iterations_(100),
ignoreVariance_(false),
globalOptimization_(true), globalOptimization_(true),
optimizeFromLastNode_(false), optimizeFromLastNode_(false),
mapToOdom_(rtabmap::Transform::getIdentity()), mapToOdom_(rtabmap::Transform::getIdentity()),
@@ -58,14 +58,35 @@ public:
ros::NodeHandle nh; ros::NodeHandle nh;
ros::NodeHandle pnh("~"); 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("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("iterations", iterations_, iterations_); pnh.param("iterations", iterations, iterations);
pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_); pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
pnh.param("global_optimization", globalOptimization_, globalOptimization_); pnh.param("global_optimization", globalOptimization_, globalOptimization_);
pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_); 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 double tfDelay = 0.05; // 20 Hz
bool publishTf = true; bool publishTf = true;
@@ -137,8 +158,10 @@ public:
if(iter->second.to() == link.to()) if(iter->second.to() == link.to())
{ {
edgeAlreadyAdded = true; 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; dataChanged = true;
} }
} }
@@ -149,16 +172,17 @@ public:
} }
} }
std::map<int, Transform> newPoses; std::map<int, Signature> newNodeInfos;
// add new odometry poses // add new odometry poses
for(unsigned int i=0; i<msg->nodes.size(); ++i) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
int id = msg->nodes[i].id; int id = msg->nodes[i].id;
Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose); 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)); std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
if(!p.second && pose != cachedPoses_.at(id)) if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
{ {
dataChanged = true; dataChanged = true;
} }
@@ -167,27 +191,27 @@ public:
if(dataChanged) if(dataChanged)
{ {
ROS_WARN("Graph data has changed! Reset cache..."); ROS_WARN("Graph data has changed! Reset cache...");
cachedPoses_ = newPoses;
cachedConstraints_ = newConstraints; cachedConstraints_ = newConstraints;
cachedNodeInfos_ = newNodeInfos;
} }
//match poses in the graph //match poses in the graph
std::map<int, Transform> poses;
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
std::map<int, Signature> nodeInfos;
if(globalOptimization_) if(globalOptimization_)
{ {
poses = cachedPoses_;
constraints = cachedConstraints_; constraints = cachedConstraints_;
nodeInfos = cachedNodeInfos_;
} }
else else
{ {
constraints = newConstraints; constraints = newConstraints;
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i) for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
{ {
std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.posesId[i]); std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
if(iter != cachedPoses_.end()) if(iter != cachedNodeInfos_.end())
{ {
poses.insert(*iter); nodeInfos.insert(*iter);
} }
else 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 // Optimize only if there is a subscriber
if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers()) if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{ {
UTimer timer; UTimer timer;
std::map<int, Transform> optimizedPoses; std::map<int, Transform> optimizedPoses;
Transform mapCorrection = Transform::getIdentity(); Transform mapCorrection = Transform::getIdentity();
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut; std::multimap<int, rtabmap::Link> linksOut;
if(poses.size() > 1 && constraints.size() > 0) if(poses.size() > 1 && constraints.size() > 0)
{ {
graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first; int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
std::map<int, rtabmap::Transform> posesOut; optimizer_->getConnectedGraph(
optimizer.getConnectedGraph(
fromId, fromId,
poses, poses,
constraints, constraints,
posesOut, posesOut,
linksOut); linksOut);
optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut); optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
mapToOdomMutex_.lock(); mapToOdomMutex_.lock();
mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse(); mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
mapToOdom_ = mapCorrection; mapToOdom_ = mapCorrection;
@@ -249,6 +279,33 @@ public:
outputDataMsg.header = msg->header; outputDataMsg.header = msg->header;
outputDataMsg.graph = outputGraphMsg; outputDataMsg.graph = outputGraphMsg;
outputDataMsg.nodes = msg->nodes; 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); mapDataPub_.publish(outputDataMsg);
} }
@@ -259,10 +316,9 @@ public:
private: private:
std::string mapFrameId_; std::string mapFrameId_;
std::string odomFrameId_; std::string odomFrameId_;
int iterations_;
bool ignoreVariance_;
bool globalOptimization_; bool globalOptimization_;
bool optimizeFromLastNode_; bool optimizeFromLastNode_;
Optimizer * optimizer_;
rtabmap::Transform mapToOdom_; rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_; boost::mutex mapToOdomMutex_;
@@ -272,8 +328,8 @@ private:
ros::Publisher mapDataPub_; ros::Publisher mapDataPub_;
ros::Publisher mapGraphPub_; ros::Publisher mapGraphPub_;
std::map<int, Transform> cachedPoses_;
std::multimap<int, Link> cachedConstraints_; std::multimap<int, Link> cachedConstraints_;
std::map<int, Signature> cachedNodeInfos_;
tf2_ros::TransformBroadcaster tfBroadcaster_; tf2_ros::TransformBroadcaster tfBroadcaster_;
boost::thread* transformThread_; boost::thread* transformThread_;
src/MapsManager.cpp
+252 -39
View File
@@ -29,23 +29,34 @@
using namespace rtabmap; using namespace rtabmap;
MapsManager::MapsManager() : MapsManager::MapsManager(bool usePublicNamespace) :
cloudDecimation_(4), cloudDecimation_(4),
cloudMaxDepth_(4.0), // meters cloudMaxDepth_(4.0), // meters
cloudMinDepth_(0.0), // meters
cloudVoxelSize_(0.05), // meters cloudVoxelSize_(0.05), // meters
cloudFloorCullingHeight_(0.0), cloudFloorCullingHeight_(0.0),
cloudCeilingCullingHeight_(0.0),
cloudOutputVoxelized_(false), cloudOutputVoxelized_(false),
cloudFrustumCulling_(false), cloudFrustumCulling_(false),
cloudNoiseFilteringRadius_(0.0),
cloudNoiseFilteringMinNeighbors_(5),
scanDecimation_(0),
scanVoxelSize_(0.0),
scanOutputVoxelized_(false),
projMaxGroundAngle_(45.0), // degrees projMaxGroundAngle_(45.0), // degrees
projMinClusterSize_(20), 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 gridCellSize_(0.05), // meters
gridSize_(0), // meters gridSize_(0), // meters
gridEroded_(false), gridEroded_(false),
gridUnknownSpaceFilled_(false), gridUnknownSpaceFilled_(false),
gridMaxUnknownSpaceFilledRange_(6.0),
mapFilterRadius_(0.5), mapFilterRadius_(0.5),
mapFilterAngle_(30.0), // degrees mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true) mapCacheCleanup_(true),
negativePosesIgnored(false)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
@@ -54,31 +65,82 @@ MapsManager::MapsManager() :
// cloud map stuff // cloud map stuff
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_);
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); 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_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_); 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 //projection map stuff
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_); pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_); 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 // common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m 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_size", gridSize_, gridSize_); // m
pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_eroded", gridEroded_, gridEroded_);
pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
// common map stuff // common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_); pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); 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 // mapping topics
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1); if(usePublicNamespace)
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1); {
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1); 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() { MapsManager::~MapsManager() {
@@ -97,7 +159,8 @@ bool MapsManager::hasSubscribers() const
{ {
return cloudMapPub_.getNumSubscribers() != 0 || return cloudMapPub_.getNumSubscribers() != 0 ||
projMapPub_.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) 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 updateCloud,
bool updateProj, bool updateProj,
bool updateGrid, bool updateGrid,
bool updateScan,
const std::map<int, rtabmap::Signature> & signatures) 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 // all false, udpate only those where we have subscribers
updateCloud = cloudMapPub_.getNumSubscribers() != 0; updateCloud = cloudMapPub_.getNumSubscribers() != 0;
updateProj = projMapPub_.getNumSubscribers() != 0; updateProj = projMapPub_.getNumSubscribers() != 0;
updateGrid = gridMapPub_.getNumSubscribers() != 0; updateGrid = gridMapPub_.getNumSubscribers() != 0;
updateScan = scanMapPub_.getNumSubscribers() != 0;
} }
UDEBUG("Updating map caches..."); UDEBUG("Updating map caches...");
if(!memory && signatures.size() == 0) 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>(); return std::map<int, rtabmap::Transform>();
} }
std::map<int, rtabmap::Transform> filteredPoses; std::map<int, rtabmap::Transform> filteredPoses;
// update cache // update cache
if(updateCloud || updateProj || updateGrid) if(updateCloud || updateProj || updateGrid || updateScan)
{ {
// filter nodes // filter nodes
if(mapFilterRadius_ > 0.0) if(mapFilterRadius_ > 0.0)
{ {
UDEBUG("Filter nodes...");
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0; double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle); filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) 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; 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) for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{ {
if(!iter->second.isNull()) if(!iter->second.isNull())
@@ -170,12 +251,15 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
rtabmap::SensorData data; rtabmap::SensorData data;
bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first)); bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first));
bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, 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 || if(rgbDepthRequired ||
depthRequired || depthRequired ||
scanRequired) scanRequired ||
gridRequired)
{ {
UDEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first); std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
if(findIter != signatures.end()) if(findIter != signatures.end())
{ {
@@ -191,26 +275,40 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
{ {
if(!(data.imageCompressed().empty() && data.imageRaw().empty()) && if(!(data.imageCompressed().empty() && data.imageRaw().empty()) &&
!(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) && !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) &&
(data.cameraModels().size() || data.stereoCameraModel().isValid())) (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection()))
{ {
// Which data should we decompress? // Which data should we decompress?
cv::Mat image, depth, scan; cv::Mat image, depth, scan;
data.uncompressData( data.uncompressData(
(rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0, (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0,
(rgbDepthRequired||depthRequired) ? &depth:0, (rgbDepthRequired||depthRequired) ? &depth:0,
scanRequired?&scan:0); scanRequired||gridRequired?&scan:0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
if(rgbDepthRequired) if(rgbDepthRequired)
{ {
UDEBUG("rgbDepthRequired");
if(!image.empty() && !depth.empty()) if(!image.empty() && !depth.empty())
{ {
pcl::IndicesPtr validIndices(new std::vector<int>);
cloudRGB = util3d::cloudRGBFromSensorData( cloudRGB = util3d::cloudRGBFromSensorData(
data, data,
cloudDecimation_, cloudDecimation_,
cloudMaxDepth_, 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 else
{ {
@@ -219,13 +317,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
} }
else if(depthRequired) else if(depthRequired)
{ {
UDEBUG("depthRequired");
if( !depth.empty()) if( !depth.empty())
{ {
pcl::IndicesPtr validIndices(new std::vector<int>);
cloudXYZ = util3d::cloudFromSensorData( cloudXYZ = util3d::cloudFromSensorData(
data, data,
cloudDecimation_, cloudDecimation_,
cloudMaxDepth_, 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 else
{ {
@@ -240,7 +350,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// Make sure that image size is set in camera models. // Make sure that image size is set in camera models.
// The camera models are used when cloud_frustum_culling=true. // The camera models are used when cloud_frustum_culling=true.
std::vector<rtabmap::CameraModel> models; std::vector<rtabmap::CameraModel> models;
if(data.stereoCameraModel().isValid()) if(data.stereoCameraModel().isValidForProjection())
{ {
//insert only the left camera model //insert only the left camera model
rtabmap::CameraModel model = data.stereoCameraModel().left(); rtabmap::CameraModel model = data.stereoCameraModel().left();
@@ -265,40 +375,90 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(depthRequired) if(depthRequired)
{ {
UDEBUG("Creating proj map for %d...", iter->first);
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
if(cloudRGB.get()) if(cloudRGB.get())
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB; 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()) if(cloudClipped->size())
{ {
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); // add pose rotation without yaw
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); 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()) else if(cloudXYZ.get())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ; 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()) 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))); uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
} }
if(scanRequired) if(scanRequired || gridRequired)
{ {
cv::Mat ground, obstacles; if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
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(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 else
@@ -307,7 +467,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
iter->first, iter->first,
!(data.imageCompressed().empty() && data.imageRaw().empty())?1:0, !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0,
!(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().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 // cleanup not used nodes
UDEBUG("Cleanup not used nodes");
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin(); for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
iter!=clouds_.end();) iter!=clouds_.end();)
{ {
@@ -416,32 +577,37 @@ void MapsManager::publishMaps(
{ {
for(unsigned int i=0; i<kter->second.size(); ++i) for(unsigned int i=0; i<kter->second.size(); ++i)
{ {
if(kter->second[i].isValid()) if(kter->second[i].isValidForProjection())
{ {
int size = assembledCloud->size(); int size = assembledCloud->size();
assembledCloud = util3d::frustumFiltering( assembledCloud = util3d::frustumFiltering(
assembledCloud, assembledCloud,
iter->second, iter->second, // FIXME: should include camera local transform
kter->second[i].horizontalFOV(), kter->second[i].horizontalFOV(),
kter->second[i].verticalFOV(), kter->second[i].verticalFOV(),
0.0f, 0.0f,
cloudMaxDepth_>0.0?cloudMaxDepth_:999999., cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
true); true);
//ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); if(jter->second->size())
*assembledCloud+=*transformed; {
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_); assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
} }
@@ -456,7 +622,7 @@ void MapsManager::publishMaps(
} }
else if(poses.size()) 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_) else if(mapCacheCleanup_)
@@ -465,6 +631,53 @@ void MapsManager::publishMaps(
cameraModels_.clear(); 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()) if(projMapPub_.getNumSubscribers())
{ {
// create the projection map // create the projection map
src/MapsManager.h
+16 -2
View File
@@ -26,7 +26,7 @@ class Memory;
class MapsManager { class MapsManager {
public: public:
MapsManager(); MapsManager(bool usePublicNamespace);
virtual ~MapsManager(); virtual ~MapsManager();
void clear(); void clear();
bool hasSubscribers() const; bool hasSubscribers() const;
@@ -40,6 +40,7 @@ public:
bool updateCloud, bool updateCloud,
bool updateProj, bool updateProj,
bool updateGrid, bool updateGrid,
bool updateScan,
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>()); const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
void publishMaps( void publishMaps(
@@ -67,26 +68,39 @@ private:
// mapping stuff // mapping stuff
int cloudDecimation_; int cloudDecimation_;
double cloudMaxDepth_; double cloudMaxDepth_;
double cloudMinDepth_;
double cloudVoxelSize_; double cloudVoxelSize_;
double cloudFloorCullingHeight_; double cloudFloorCullingHeight_;
double cloudCeilingCullingHeight_;
bool cloudOutputVoxelized_; bool cloudOutputVoxelized_;
bool cloudFrustumCulling_; bool cloudFrustumCulling_;
double cloudNoiseFilteringRadius_;
int cloudNoiseFilteringMinNeighbors_;
int scanDecimation_;
double scanVoxelSize_;
bool scanOutputVoxelized_;
double projMaxGroundAngle_; double projMaxGroundAngle_;
int projMinClusterSize_; int projMinClusterSize_;
double projMaxHeight_; double projMaxObstaclesHeight_;
double projMaxGroundHeight_;
bool projDetectFlatObstacles_;
double gridCellSize_; double gridCellSize_;
double gridSize_; double gridSize_;
bool gridEroded_; bool gridEroded_;
bool gridUnknownSpaceFilled_; bool gridUnknownSpaceFilled_;
double gridMaxUnknownSpaceFilledRange_;
double mapFilterRadius_; double mapFilterRadius_;
double mapFilterAngle_; double mapFilterAngle_;
bool mapCacheCleanup_; bool mapCacheCleanup_;
bool negativePosesIgnored;
ros::Publisher cloudMapPub_; ros::Publisher cloudMapPub_;
ros::Publisher projMapPub_; ros::Publisher projMapPub_;
ros::Publisher gridMapPub_; ros::Publisher gridMapPub_;
ros::Publisher scanMapPub_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_; 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::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> > projMaps_; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <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 <pcl_conversions/pcl_conversions.h>
#include <eigen_conversions/eigen_msg.h> #include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h> #include <tf_conversions/tf_eigen.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
namespace rtabmap_ros { namespace rtabmap_ros {
@@ -144,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
// rtabmap_ros::Info // rtabmap_ros::Info
stat.setRefImageId(info.refId); stat.setRefImageId(info.refId);
stat.setLoopClosureId(info.loopClosureId); stat.setLoopClosureId(info.loopClosureId);
stat.setLocalLoopClosureId(info.localLoopClosureId); stat.setProximityDetectionId(info.proximityDetectionId);
stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform)); 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.refId = stats.refImageId();
info.loopClosureId = stats.loopClosureId(); info.loopClosureId = stats.loopClosureId();
info.localLoopClosureId = stats.localLoopClosureId(); info.proximityDetectionId = stats.proximityDetectionId();
rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform); 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( void mapDataFromROS(
const rtabmap_ros::MapData & msg, const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses, std::map<int, rtabmap::Transform> & poses,
@@ -384,13 +483,14 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
{ {
//Features stuff... //Features stuff...
std::multimap<int, cv::KeyPoint> words; std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D; std::multimap<int, cv::Point3f> words3D;
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
if(msg.wordPts.data.size() && 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); pcl::fromROSMsg(msg.wordPts, cloud);
} }
for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i) for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
{ {
cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(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)); words.insert(std::make_pair(wordId, pt));
if(i< cloud.size()) 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.stamp,
msg.label, msg.label,
transformFromPoseMsg(msg.pose), transformFromPoseMsg(msg.pose),
stereoModel.isValid()? transformFromPoseMsg(msg.groundTruthPose),
stereoModel.isValidForProjection()?
rtabmap::SensorData( rtabmap::SensorData(
compressedMatFromBytes(msg.laserScan), compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts, msg.laserScanMaxPts,
@@ -489,6 +590,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.stamp = signature.getStamp(); msg.stamp = signature.getStamp();
msg.label = signature.getLabel(); msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose); transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image); compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth); compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan); 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]); 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.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy()); 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; pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(signature.getWords3().size()); cloud.resize(signature.getWords3().size());
index = 0; 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!=signature.getWords3().end();
++jter) ++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); 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 odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
{ {
rtabmap::OdometryInfo info; rtabmap::OdometryInfo info;
@@ -588,6 +716,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.transform = transformFromGeometryMsg(msg.transform); info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered); 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; return info;
} }
@@ -605,7 +739,6 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.interval = info.interval; msg.interval = info.interval;
msg.distanceTravelled = info.distanceTravelled; msg.distanceTravelled = info.distanceTravelled;
msg.type = info.type; msg.type = info.type;
msg.wordsKeys = uKeys(info.words); 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.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered); 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 <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Rtabmap.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/util3d_transforms.h>
#include <rtabmap/core/Memory.h> #include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.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/UStl.h"
#include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/UFile.h"
#define BAD_COVARIANCE 9999
using namespace rtabmap; using namespace rtabmap;
namespace rtabmap_ros { namespace rtabmap_ros {
@@ -60,7 +63,10 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
publishTf_(true), publishTf_(true),
waitForTransform_(true), waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms waitForTransformDuration_(0.1), // 100 ms
paused_(false) publishNullWhenLost_(true),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0)
{ {
ros::NodeHandle nh; 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("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("config_path", configPath, configPath); pnh.param("config_path", configPath, configPath);
pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/') if(configPath.size() && configPath.at(0) != '/')
{ {
@@ -124,14 +132,14 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
//parameters //parameters
parameters_ = this->getDefaultOdometryParameters(stereo); parameters_ = Parameters::getDefaultOdometryParameters(stereo);
if(!configPath.empty()) if(!configPath.empty())
{ {
if(UFile::exists(configPath.c_str())) if(UFile::exists(configPath.c_str()))
{ {
ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str()); ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str());
rtabmap::ParametersMap allParameters; rtabmap::ParametersMap allParameters;
Rtabmap::readParameters(configPath.c_str(), allParameters); Parameters::readINI(configPath.c_str(), allParameters);
// only update odometry parameters // only update odometry parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) 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); 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..."); ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(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 // Backward compatibility
std::list<std::string> oldParameterNames; for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
oldParameterNames.push_back("Odom/Type"); iter!=Parameters::getRemovedParameters().end();
oldParameterNames.push_back("Odom/MaxWords"); ++iter)
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)
{ {
std::string vStr; 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.", // can be migrated
Parameters::kOdomFeatureType().c_str()); parameters_.at(iter->second.second)= vStr;
parameters_.at(Parameters::kOdomFeatureType())= 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.", if(iter->second.second.empty())
Parameters::kOdomMaxFeatures().c_str()); {
parameters_.at(Parameters::kOdomMaxFeatures())= vStr; ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!",
} iter->first.c_str());
else if(iter->compare("Odom/LocalHistory") == 0) }
{ else
ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.", {
Parameters::kOdomBowLocalHistorySize().c_str()); ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr; iter->first.c_str(), iter->second.second.c_str());
} }
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;
} }
} }
} }
int odomStrategy = 0; // BOW Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
if(odomStrategy == 1) odometry_ = Odometry::create(parameters_);
{
ROS_INFO("Using OdometryOpticalFlow");
odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
}
else
{
ROS_INFO("Using OdometryBOW");
odometry_ = new rtabmap::OdometryBOW(parameters_);
}
if(!initialPose.isIdentity()) if(!initialPose.isIdentity())
{ {
odometry_->reset(initialPose); 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); resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this); pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, 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() OdometryROS::~OdometryROS()
@@ -268,48 +261,13 @@ OdometryROS::~OdometryROS()
delete odometry_; 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) void OdometryROS::processArguments(int argc, char * argv[], bool stereo)
{ {
for(int i=1;i<argc;++i) for(int i=1;i<argc;++i)
{ {
if(strcmp(argv[i], "--params") == 0) 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) for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{ {
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\""; 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!"); "argument \"--params\" is detected!");
exit(0); 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 // process data
ros::WallTime time = ros::WallTime::now(); ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info; rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(data, &info); SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
if(!pose.isNull()) if(!pose.isNull())
{ {
resetCurrentCount_ = resetCountdown_;
//********************* //*********************
// Update odometry // Update odometry
//********************* //*********************
@@ -410,24 +379,47 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.pose.pose.orientation = poseMsg.transform.rotation; odom.pose.pose.orientation = poseMsg.transform.rotation;
//set covariance //set covariance
odom.pose.covariance.at(0) = info.variance; // xx // libviso2 uses approximately vel variance * 2
odom.pose.covariance.at(7) = info.variance; // yy odom.pose.covariance.at(0) = info.variance*2; // xx
odom.pose.covariance.at(14) = info.variance; // zz odom.pose.covariance.at(7) = info.variance*2; // yy
odom.pose.covariance.at(21) = info.variance; // rr odom.pose.covariance.at(14) = info.variance*2; // zz
odom.pose.covariance.at(28) = info.variance; // pp odom.pose.covariance.at(21) = info.variance*2; // rr
odom.pose.covariance.at(35) = info.variance; // yawyaw 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 //publish the message
odomPub_.publish(odom); 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; 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; sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg); pcl::toROSMsg(cloud, cloudMsg);
@@ -438,18 +430,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(odomLastFrame_.getNumSubscribers()) if(odomLastFrame_.getNumSubscribers())
{ {
if(dynamic_cast<OdometryBOW*>(odometry_)) if(dynamic_cast<OdometryF2M*>(odometry_))
{ {
const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature(); const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(s) if(words3.size())
{ {
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud; 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 // transform to odom frame
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose); cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pt); cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
} }
sensor_msgs::PointCloud2 cloudMsg; sensor_msgs::PointCloud2 cloudMsg;
@@ -461,14 +452,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
} }
else else
{ {
//Optical flow //Frame to Frame
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D(); const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
if(cloud->size()) if(refFrame.getWords3().size())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed; pcl::PointCloud<pcl::PointXYZ> cloud;
cloudTransformed = util3d::transformPointCloud(cloud, pose); 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; sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg); pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_; cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg); 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!"); //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.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_; odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_; 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 //publish the message
odomPub_.publish(odom); 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()) if(odomInfoPub_.getNumSubscribers())
{ {
rtabmap_ros::OdomInfo infoMsg; 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()); 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&) 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; 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 class OdometryROS
{ {
public: public:
static rtabmap::ParametersMap getDefaultOdometryParameters(bool stereo = false);
static void processArguments(int argc, char * argv[], bool stereo = false); static void processArguments(int argc, char * argv[], bool stereo = false);
public: public:
@@ -61,13 +60,17 @@ public:
bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&); bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resume(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 & frameId() const {return frameId_;}
const std::string & odomFrameId() const {return odomFrameId_;} const std::string & odomFrameId() const {return odomFrameId_;}
const rtabmap::ParametersMap & parameters() const {return parameters_;} const rtabmap::ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;} const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;} 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; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private: private:
@@ -80,6 +83,7 @@ private:
bool publishTf_; bool publishTf_;
bool waitForTransform_; bool waitForTransform_;
double waitForTransformDuration_; double waitForTransformDuration_;
bool publishNullWhenLost_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_; ros::Publisher odomPub_;
@@ -90,10 +94,16 @@ private:
ros::ServiceServer resetToPoseSrv_; ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_; ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_; ros::ServiceServer resumeSrv_;
ros::ServiceServer setLogDebugSrv_;
ros::ServiceServer setLogInfoSrv_;
ros::ServiceServer setLogWarnSrv_;
ros::ServiceServer setLogErrorSrv_;
tf2_ros::TransformBroadcaster tfBroadcaster_; tf2_ros::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_; tf::TransformListener tfListener_;
bool paused_; 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 "PreferencesDialogROS.h"
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <QtCore/QDir> #include <QDir>
#include <QtCore/QFileInfo> #include <QFileInfo>
#include <QtCore/QSettings> #include <QSettings>
#include <QtGui/QHBoxLayout> #include <QHBoxLayout>
#include <QtCore/QTimer> #include <QTimer>
#include <QtGui/QLabel> #include <QLabel>
#include <rtabmap/core/RtabmapEvent.h> #include <rtabmap/core/RtabmapEvent.h>
#include <QtGui/QMessageBox> #include <QMessageBox>
#include <ros/exceptions.h> #include <ros/exceptions.h>
#include <rtabmap/utilite/UStl.h>
using namespace rtabmap; using namespace rtabmap;
@@ -76,19 +77,42 @@ QString PreferencesDialogROS::getParamMessage()
bool PreferencesDialogROS::readCoreSettings(const QString & filePath) bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
{ {
if(filePath.isEmpty() || filePath.compare(getTmpIniFilePath()) == 0) QString path = getIniFilePath();
if(!filePath.isEmpty())
{ {
ros::NodeHandle nh; path = filePath;
ROS_INFO("%s", this->getParamMessage().toStdString().c_str()); }
bool validParameters = true;
int readCount = 0; ros::NodeHandle nh;
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) 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; 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; ++readCount;
} }
else else
@@ -96,51 +120,50 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
validParameters = false; validParameters = false;
} }
} }
}
ROS_INFO("Parameters read = %d", readCount); ROS_INFO("Parameters read = %d", readCount);
if(validParameters) if(validParameters)
{ {
ROS_INFO("Parameters successfully read."); 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;
} }
else 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); QString path = getIniFilePath();
if(!filePath.isEmpty())
// 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())
{ {
emit settingsChanged(_parameters); path = filePath;
} }
if(_obsoletePanels) if(QFile::exists(path))
{ {
emit settingsChanged(_obsoletePanels); rtabmap::ParametersMap parameters = this->getAllParameters();
}
_parameters = rtabmap::ParametersMap(); std::string workingDir = uValue(parameters, Parameters::kRtabmapWorkingDirectory(), std::string(""));
_obsoletePanels = kPanelDummy;
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 ~PreferencesDialogROS();
virtual QString getIniFilePath() const; virtual QString getIniFilePath() const;
virtual QString getTmpIniFilePath() const;
protected: protected:
virtual QString getParamMessage(); virtual QString getParamMessage();
virtual void readCameraSettings(const QString & filePath); virtual void readCameraSettings(const QString & filePath);
virtual bool readCoreSettings(const QString & filePath); virtual bool readCoreSettings(const QString & filePath);
virtual void writeSettings(const QString & filePath); virtual void writeCameraSettings(const QString & filePath) const {}
virtual void writeCoreSettings(const QString & filePath) const;
virtual QString getTmpIniFilePath() const;
private: private:
QString configFile_; 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) if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{ {
image_geometry::PinholeCameraModel model; rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
model.fromCameraInfo(*cameraInfo); cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
rtabmap::CameraModel rtabmapModel( cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
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::SensorData data( rtabmap::SensorData data(
ptrImage->image, ptrImage->image,
@@ -321,14 +314,7 @@ public:
return; return;
} }
image_geometry::PinholeCameraModel model; cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
model.fromCameraInfo(*infoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
} }
rtabmap::SensorData data( rtabmap::SensorData data(
src/StereoOdometryNode.cpp
+7 -16
View File
@@ -145,24 +145,15 @@ public:
int quality = -1; int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size()) if(imageRectLeft->data.size() && imageRectRight->data.size())
{ {
image_geometry::StereoCameraModel model; rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); if(stereoModel.baseline() <= 0)
if(model.baseline() <= 0)
{ {
ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " 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; return;
} }
rtabmap::StereoCameraModel stereoModel( if(stereoModel.baseline() > 10.0)
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
if(model.baseline() > 10.0)
{ {
static bool shown = false; static bool shown = false;
if(!shown) if(!shown)
@@ -170,13 +161,13 @@ public:
ROS_WARN("Detected baseline (%f m) is quite large! Is your " ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that " "right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.", "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline()); stereoModel.baseline());
shown = true; shown = true;
} }
} }
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8"); cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8"); cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer; UTimer stepTimer;
// //
src/nodelets/data_throttle.cpp
+4 -4
View File
@@ -91,16 +91,16 @@ private:
bool approxSync = true; bool approxSync = true;
if(private_nh.getParam("max_rate", rate_)) 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("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize); private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync); private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("decimation", decimation_, decimation_); private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1); ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_); NODELET_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_); NODELET_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync) 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) 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; return;
} }
src/nodelets/obstacles_detection.cpp
+42 -13
View File
@@ -67,10 +67,13 @@ class ObstaclesDetection : public nodelet::Nodelet
public: public:
ObstaclesDetection() : ObstaclesDetection() :
frameId_("base_link"), frameId_("base_link"),
normalEstimationRadius_(0.05), normalKSearch_(20),
groundNormalAngle_(M_PI_4), groundNormalAngle_(M_PI_4),
clusterRadius_(0.05),
minClusterSize_(20), minClusterSize_(20),
maxObstaclesHeight_(0.0), // if<=0.0 -> disabled 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), waitForTransform_(false),
optimizeForCloseObjects_(false) optimizeForCloseObjects_(false)
{} {}
@@ -87,10 +90,24 @@ private:
int queueSize = 10; int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_); 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_); 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("min_cluster_size", minClusterSize_, minClusterSize_);
pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); 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("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
@@ -104,7 +121,7 @@ private:
void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) 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) 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))) 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; return;
} }
} }
@@ -129,7 +146,7 @@ private:
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
ROS_ERROR("%s",ex.what()); NODELET_ERROR("%s",ex.what());
return; return;
} }
@@ -158,9 +175,12 @@ private:
originalCloud, originalCloud,
ground, ground,
obstacles, obstacles,
normalEstimationRadius_, normalKSearch_,
groundNormalAngle_, groundNormalAngle_,
minClusterSize_); clusterRadius_,
minClusterSize_,
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{ {
@@ -190,9 +210,12 @@ private:
originalCloud_near, originalCloud_near,
ground, ground,
obstacles, obstacles,
normalEstimationRadius_, normalKSearch_,
groundNormalAngle_, groundNormalAngle_,
minClusterSize_); clusterRadius_,
minClusterSize_,
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{ {
@@ -211,9 +234,12 @@ private:
originalCloud_far, originalCloud_far,
ground, ground,
obstacles, obstacles,
3.*normalEstimationRadius_, normalKSearch_,
2.*groundNormalAngle_, 2.*groundNormalAngle_,
minClusterSize_); 3.*clusterRadius_,
minClusterSize_,
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{ {
@@ -255,15 +281,18 @@ private:
obstaclesPub_.publish(rosCloud); 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: private:
std::string frameId_; std::string frameId_;
double normalEstimationRadius_; int normalKSearch_;
double groundNormalAngle_; double groundNormalAngle_;
double clusterRadius_;
int minClusterSize_; int minClusterSize_;
double maxObstaclesHeight_; double maxObstaclesHeight_;
double maxGroundHeight_;
bool segmentFlatObstacles_;
bool waitForTransform_; bool waitForTransform_;
bool optimizeForCloseObjects_; 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 <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h> #include <nodelet/nodelet.h>
#include <rtabmap_ros/MsgConversion.h>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
@@ -62,6 +64,7 @@ class PointCloudXYZ : public nodelet::Nodelet
public: public:
PointCloudXYZ() : PointCloudXYZ() :
maxDepth_(0.0), maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0), voxelSize_(0.0),
decimation_(1), decimation_(1),
noiseFilterRadius_(0.0), noiseFilterRadius_(0.0),
@@ -98,6 +101,7 @@ private:
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
@@ -106,7 +110,7 @@ private:
pnh.param("cut_right", cut_right_, cut_right_); 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_); 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) if(approxSync)
{ {
@@ -147,7 +151,7 @@ private:
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=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; return;
} }
@@ -222,7 +226,7 @@ private:
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=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; return;
} }
@@ -238,18 +242,12 @@ private:
if(cloudPub_.getNumSubscribers()) if(cloudPub_.getNumSubscribers())
{ {
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float cx = model.cx();
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud; 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( pclCloud = rtabmap::util3d::cloudFromDisparity(
disparity, disparity,
cx, stereoModel,
cy,
disparityMsg->f,
disparityMsg->T,
decimation_); decimation_);
processAndPublish(pclCloud, disparityMsg->header); processAndPublish(pclCloud, disparityMsg->header);
@@ -258,9 +256,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header) 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) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -288,6 +286,7 @@ private:
private: private:
double maxDepth_; double maxDepth_;
double minDepth_;
double voxelSize_; double voxelSize_;
int decimation_; int decimation_;
double noiseFilterRadius_; 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/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <rtabmap_ros/MsgConversion.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h> #include <sensor_msgs/image_encodings.h>
@@ -62,6 +64,7 @@ class PointCloudXYZRGB : public nodelet::Nodelet
public: public:
PointCloudXYZRGB() : PointCloudXYZRGB() :
maxDepth_(0.0), maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0), voxelSize_(0.0),
decimation_(1), decimation_(1),
noiseFilterRadius_(0.0), noiseFilterRadius_(0.0),
@@ -95,12 +98,13 @@ private:
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); 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); 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::TYPE_32FC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==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; return;
} }
if(cloudPub_.getNumSubscribers()) 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); cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model; 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::BGR8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 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; return;
} }
@@ -228,22 +246,11 @@ private:
} }
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); 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; pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromStereoImages( pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image, ptrLeftImage->image,
ptrRightImage->image, ptrRightImage->image,
cx, rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
cy,
fx,
baseline,
decimation_); decimation_);
processAndPublish(pclCloud, imageLeft->header); processAndPublish(pclCloud, imageLeft->header);
@@ -252,9 +259,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header) 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) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -282,6 +289,7 @@ private:
private: private:
double maxDepth_; double maxDepth_;
double minDepth_;
double voxelSize_; double voxelSize_;
int decimation_; int decimation_;
double noiseFilterRadius_; double noiseFilterRadius_;
src/nodelets/stereo_throttle.cpp
+3 -3
View File
@@ -93,9 +93,9 @@ private:
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1); ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_); NODELET_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_); NODELET_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync) 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, "Info", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0"); this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0"); this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
spinner_.start(); spinner_.start();
} }
@@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
boost::mutex::scoped_lock lock(info_mutex_); boost::mutex::scoped_lock lock(info_mutex_);
if(msg->loopClosureId) 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; 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; localCount_ += 1;
} }
else 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, "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, "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, "Loop closures", tr("%1").arg(globalCount_).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).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) 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. SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <QtGui/QApplication> #include <QApplication>
#include <QtGui/QMessageBox> #include <QMessageBox>
#include <QtCore/QTimer> #include <QTimer>
#include <OgreSceneNode.h> #include <OgreSceneNode.h>
#include <OgreSceneManager.h> #include <OgreSceneManager.h>
@@ -143,6 +143,12 @@ MapCloudDisplay::MapCloudDisplay()
cloud_max_depth_->setMin( 0.0f ); cloud_max_depth_->setMin( 0.0f );
cloud_max_depth_->setMax( 999.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, cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f,
"Voxel size of the generated clouds.", "Voxel size of the generated clouds.",
this, SLOT( updateCloudParameters() ), this ); this, SLOT( updateCloudParameters() ), this );
@@ -156,6 +162,13 @@ MapCloudDisplay::MapCloudDisplay()
cloud_filter_floor_height_->setMin( 0.0f ); cloud_filter_floor_height_->setMin( 0.0f );
cloud_filter_floor_height_->setMax( 999.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, node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
"(Disabled=0) Only keep one node in the specified radius.", "(Disabled=0) Only keep one node in the specified radius.",
this, SLOT( updateCloudParameters() ), this ); this, SLOT( updateCloudParameters() ), this );
@@ -249,17 +262,24 @@ void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg )
void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) 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... // Add new clouds...
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i) for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{ {
int id = map.nodes[i].id; 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! // Cloud not added to RVIZ, add it!
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]); rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() && if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().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; cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0); 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()) if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr validIndices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData( cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(), s.sensorData(),
cloud_decimation_->getInt(), cloud_decimation_->getInt(),
cloud_max_depth_->getFloat(), 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->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); sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -302,12 +331,6 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
} }
// Update graph // 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) if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{ {
poses = rtabmap::graph::radiusPosesFiltering(poses, 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 #ifndef MAP_CLOUD_DISPLAY_H
#define 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 <deque>
#include <queue> #include <queue>
#include <vector> #include <vector>
@@ -42,6 +44,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rviz/message_filter_display.h> #include <rviz/message_filter_display.h>
#include <rviz/default_plugin/point_cloud_transformer.h> #include <rviz/default_plugin/point_cloud_transformer.h>
#endif
namespace rviz { namespace rviz {
class IntProperty; class IntProperty;
class BoolProperty; class BoolProperty;
@@ -106,8 +110,10 @@ public:
rviz::EnumProperty* style_property_; rviz::EnumProperty* style_property_;
rviz::IntProperty* cloud_decimation_; rviz::IntProperty* cloud_decimation_;
rviz::FloatProperty* cloud_max_depth_; rviz::FloatProperty* cloud_max_depth_;
rviz::FloatProperty* cloud_min_depth_;
rviz::FloatProperty* cloud_voxel_size_; rviz::FloatProperty* cloud_voxel_size_;
rviz::FloatProperty* cloud_filter_floor_height_; rviz::FloatProperty* cloud_filter_floor_height_;
rviz::FloatProperty* cloud_filter_ceiling_height_;
rviz::FloatProperty* node_filtering_radius_; rviz::FloatProperty* node_filtering_radius_;
rviz::FloatProperty* node_filtering_angle_; rviz::FloatProperty* node_filtering_angle_;
rviz::BoolProperty* download_map_; rviz::BoolProperty* download_map_;
src/rviz/MapGraphDisplay.cpp
+7 -1
View File
@@ -52,7 +52,9 @@ namespace rtabmap_ros
MapGraphDisplay::MapGraphDisplay() MapGraphDisplay::MapGraphDisplay()
{ {
color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue, 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_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red,
"Color to draw global loop closure links.", this ); "Color to draw global loop closure links.", this );
color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow, 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(); 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) else if(iter->second.type() == rtabmap::Link::kVirtualClosure)
{ {
color = color_virtual_property_->getOgreColor(); color = color_virtual_property_->getOgreColor();
src/rviz/MapGraphDisplay.h
+1
View File
@@ -76,6 +76,7 @@ private:
std::vector<Ogre::ManualObject*> manual_objects_; std::vector<Ogre::ManualObject*> manual_objects_;
ColorProperty* color_neighbor_property_; ColorProperty* color_neighbor_property_;
ColorProperty* color_neighbor_merged_property_;
ColorProperty* color_global_property_; ColorProperty* color_global_property_;
ColorProperty* color_local_property_; ColorProperty* color_local_property_;
ColorProperty* color_user_property_; ColorProperty* color_user_property_;
srv/SetGoal.srv
+1
View File
@@ -6,3 +6,4 @@ string node_label
#response #response
int32[] path_ids int32[] path_ids
geometry_msgs/Pose[] path_poses geometry_msgs/Pose[] path_poses
float32 planning_time