mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
First version rtabmap_launch working
This commit is contained in:
matlabbe
@@ -0,0 +1,69 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(rtabmap_legacy)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure image_transport rtabmap_conversions rtabmap_msgs
|
||||
)
|
||||
|
||||
generate_dynamic_reconfigure_options(cfg/Camera.cfg)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES rtabmap_legacy_plugins
|
||||
CATKIN_DEPENDS dynamic_reconfigure image_transport rtabmap_conversions rtabmap_msgs
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
SET(rtabmap_legacy_plugins_lib_src
|
||||
src/nodelets/data_throttle.cpp
|
||||
src/nodelets/stereo_throttle.cpp
|
||||
src/nodelets/data_odom_sync.cpp
|
||||
src/nodelets/obstacles_detection_old.cpp
|
||||
src/nodelets/undistort_depth.cpp
|
||||
)
|
||||
|
||||
############################
|
||||
## Declare a cpp library
|
||||
############################
|
||||
add_library(rtabmap_legacy_plugins
|
||||
${rtabmap_legacy_plugins_lib_src}
|
||||
)
|
||||
target_link_libraries(rtabmap_legacy_plugins
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(rtabmap_camera src/CameraNode.cpp)
|
||||
add_dependencies(rtabmap_camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
target_link_libraries(rtabmap_camera ${catkin_LIBRARIES})
|
||||
set_target_properties(rtabmap_camera PROPERTIES OUTPUT_NAME "camera")
|
||||
|
||||
add_executable(rtabmap_stereo_camera src/StereoCameraNode.cpp)
|
||||
target_link_libraries(rtabmap_stereo_camera ${catkin_LIBRARIES})
|
||||
set_target_properties(rtabmap_stereo_camera PROPERTIES OUTPUT_NAME "stereo_camera")
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
install(TARGETS
|
||||
rtabmap_legacy_plugins
|
||||
rtabmap_camera
|
||||
rtabmap_stereo_camera
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
install(FILES
|
||||
nodelet_plugins.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
Executable
+13
@@ -0,0 +1,13 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "rtabmap_legacy"
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("device_id", int_t, 0, "Camera device ID", 0, 0, 7)
|
||||
gen.add("frame_rate", double_t, 0, "Frame rate", 15.0, 0.0, 100.0)
|
||||
gen.add("video_or_images_path", str_t, 0, "Video or images directory path", "")
|
||||
gen.add("pause", bool_t, 0, "Pause", False)
|
||||
|
||||
exit(gen.generate(PACKAGE, "rtabmap_legacy", "Camera"))
|
||||
@@ -0,0 +1,10 @@
|
||||
#!/bin/bash
|
||||
|
||||
## Source ROS setup.sh (adjust to your version : boxturtle, cturtle, diamondback, e...)
|
||||
source /opt/ros/groovy/setup.bash
|
||||
|
||||
## Setup ROS_PACKAGE_PATH
|
||||
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/workspace/ros-pkg
|
||||
|
||||
## Start eclipse
|
||||
/usr/bin/eclipse
|
||||
@@ -0,0 +1,10 @@
|
||||
[Desktop Entry]
|
||||
Name=Eclipse
|
||||
GenericName=eclipse
|
||||
Comment=eclipse
|
||||
Keywords=eclipse
|
||||
Exec=/PATH/TO/THIS/DIRECTORY/eclipse-launch.sh
|
||||
Terminal=false
|
||||
Type=Application
|
||||
StartupNotify=true
|
||||
Icon=/PATH/TO/THIS/DIRECTORY/eclipse48.png
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 3.0 KiB |
@@ -0,0 +1,53 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<param name="use_sim_time" type="bool" value="True"/>
|
||||
|
||||
<!-- 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="/base_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="rgb/image_transport" type="string" value="compressed"/>
|
||||
<param name="depth/image_transport" type="string" value="compressedDepth"/>
|
||||
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
|
||||
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
|
||||
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
|
||||
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
|
||||
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
|
||||
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
|
||||
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
|
||||
<param name="LccIcp2/Iterations" type="string" value="100"/>
|
||||
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
|
||||
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
|
||||
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
|
||||
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
|
||||
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="Rtabmap/TimeThr" type="string" value="700"/>
|
||||
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
|
||||
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- send AZIMUT 3 urdf to param server -->
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" />
|
||||
|
||||
<!-- Visualisation -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_mapping_client.launch"/>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,37 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- rosbag record camera/data_throttled_image/compressed camera/data_throttled_image_depth/compressedDepth camera/data_throttled_camera_info tf base_scan /base_controller/odom -->
|
||||
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
|
||||
<!-- To control with only one joystick -->
|
||||
<include file="$(find turtlebot_teleop)/launch/includes/velocity_smoother.launch.xml"/>
|
||||
|
||||
<node pkg="turtlebot_teleop" type="turtlebot_teleop_joy" name="turtlebot_teleop_joystick">
|
||||
<param name="scale_angular" value="1.5"/>
|
||||
<param name="scale_linear" value="0.5"/>
|
||||
<remap from="turtlebot_teleop_joystick/cmd_vel" to="base_controller/cmd_vel"/>
|
||||
</node>
|
||||
|
||||
<node pkg="joy" type="joy_node" name="joystick"/>
|
||||
|
||||
<!-- OpenNI -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="10.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,47 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- record data to RTAB-Map database format (like a ROS bag, but usable in RTAB-Map for Windows/Mac OS X) -->
|
||||
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
<include file="$(find az3_bringup)/joystick.launch"/>
|
||||
|
||||
<!-- OpenNI -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="10.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="rgb/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<node name="data_recorder" pkg="rtabmap_ros" type="data_recorder" output="screen">
|
||||
<param name="output_file_name" value="az3_record.db" type="string"/>
|
||||
|
||||
<param name="frame_id" type="string" value="base_footprint"/>
|
||||
|
||||
<param name="subscribe_odometry" type="bool" value="true"/>
|
||||
<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="/base_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"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Remote teleop -->
|
||||
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
|
||||
|
||||
<!-- We have two nodes (grid_map_assembler and rviz) subscribing to /rtabmap/mapData, so use -->
|
||||
<!-- a relay on this machine, same for images -->
|
||||
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
|
||||
<node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
|
||||
<node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
|
||||
<node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
|
||||
|
||||
<!-- Grid map assembler for rviz -->
|
||||
<node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen">
|
||||
<remap from="mapData" to="rtabmap/mapData_relay"/>
|
||||
<remap from="grid_map" to="rtabmap/grid_map"/>
|
||||
</node>
|
||||
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3.rviz"/>
|
||||
|
||||
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
|
||||
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
|
||||
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
|
||||
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
|
||||
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
|
||||
<remap from="cloud" to="voxel_cloud" />
|
||||
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
<param name="voxel_size" type="double" value="0.01"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,10 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- relays -->
|
||||
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData_optimized /rtabmap/mapData_relay"/>
|
||||
<node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/stereo_camera/left/image_rect_color raw out:=/stereo_camera/left/image_rect_color_relay" />
|
||||
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_stereo_nav.rviz"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,63 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
|
||||
|
||||
<!-- OpenNI -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="5.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
</node>
|
||||
</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="/base_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/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
|
||||
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
|
||||
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
|
||||
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
|
||||
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
|
||||
<param name="LccIcp2/Iterations" type="string" value="100"/>
|
||||
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
|
||||
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
|
||||
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
|
||||
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
|
||||
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="Rtabmap/TimeThr" type="string" value="700"/>
|
||||
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
|
||||
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
@@ -0,0 +1,51 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
|
||||
|
||||
<!-- OpenNI -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="5.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
</node>
|
||||
</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"/>
|
||||
|
||||
<remap from="odom" to="/base_controller/odom"/>
|
||||
<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/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="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="Rtabmap/TimeThr" type="string" value="700"/>
|
||||
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
|
||||
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- "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_mapping_robot_kinect_odom.launch"/>
|
||||
|
||||
<!-- 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="/base_controller/odom"/>
|
||||
|
||||
<param name="Odom/MinInliers" type="string" value="10"/>
|
||||
<param name="Odom/InlierDistance" type="string" value="0.01"/>
|
||||
|
||||
<param name="frame_id" type="string" value="base_footprint"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,132 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="rtabmap_args" default="" />
|
||||
<arg name="localization" default="false" />
|
||||
|
||||
<!-- AZIMUT 3 bringup: launch motors/odometry -->
|
||||
<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_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 -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
|
||||
<param name="rate" type="double" value="3"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="throttled_image"/>
|
||||
<remap from="depth/image_out" to="throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="throttled_camera_info"/>
|
||||
</node>
|
||||
|
||||
<!-- for the planner -->
|
||||
<node pkg="nodelet" type="nodelet" name="obstacle_nodelet_manager" args="manager" output="screen"/>
|
||||
<node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz obstacle_nodelet_manager">
|
||||
<remap from="depth/image" to="throttled_image_depth"/>
|
||||
<remap from="depth/camera_info" to="throttled_camera_info"/>
|
||||
<remap from="cloud" to="cloudXYZ" />
|
||||
<param name="decimation" type="int" value="2"/>
|
||||
<param name="max_depth" type="double" value="4.0"/>
|
||||
<param name="voxel_size" type="double" value="0.02"/>
|
||||
</node>
|
||||
|
||||
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacle_nodelet_manager">
|
||||
<remap from="cloud" to="cloudXYZ"/>
|
||||
<remap from="obstacles" to="/obstacles_cloud"/>
|
||||
<remap from="ground" to="/ground_cloud"/>
|
||||
|
||||
<param name="frame_id" type="string" value="base_footprint"/>
|
||||
<param name="map_frame_id" type="string" value="map"/>
|
||||
<param name="wait_for_transform" type="bool" value="true"/>
|
||||
<param name="min_cluster_size" type="int" value="20"/>
|
||||
<param name="max_obstacles_height" type="double" value="0.4"/>
|
||||
<param name="ground_normal_angle" type="double" value="0.1"/>
|
||||
</node>
|
||||
|
||||
<!-- scan from the camera -->
|
||||
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet camera_nodelet_manager">
|
||||
<remap from="image" to="depth_registered/image_raw"/>
|
||||
<remap from="camera_info" to="depth_registered/camera_info"/>
|
||||
<remap from="scan" to="/kinect_scan"/>
|
||||
<param name="range_max" type="double" value="4"/>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
@@ -0,0 +1,167 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- "Disable" wheel 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"/>
|
||||
|
||||
<!-- AZIMUT 3 bringup: launch motors and TF -->
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
|
||||
<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="openni_points" to="/planner_cloud"/>
|
||||
<remap from="base_scan" to="/base_scan"/>
|
||||
<remap from="map" to="/rtabmap/proj_map"/>
|
||||
<remap from="move_base_simple/goal" to="/planner_goal"/>
|
||||
|
||||
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_3d.yaml" command="load" />
|
||||
<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"/>
|
||||
|
||||
<!-- Stereo camera -->
|
||||
<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>
|
||||
|
||||
<!-- TF transforms for the stereo camera -->
|
||||
<arg name="pi/2" value="1.5707963267948966" />
|
||||
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="stereo_camera_base_link"
|
||||
args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="base_to_stereo_camera_base_link"
|
||||
args="0.01 0.06 0.90 0 0.37 0 base_link stereo_camera_base 100" />
|
||||
|
||||
<!-- Run the ROS package stereo_image_proc for image rectification-->
|
||||
<group ns="/stereo_camera" >
|
||||
<node pkg="nodelet" type="nodelet" name="stereo_nodelet" args="manager"/>
|
||||
<!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
|
||||
Throttle camera images -->
|
||||
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle stereo_nodelet">
|
||||
<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"/>
|
||||
<param name="disparity_range" value="128"/>
|
||||
</node>
|
||||
|
||||
<!-- Create point cloud for the planner -->
|
||||
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
|
||||
<remap from="disparity/image" to="disparity"/>
|
||||
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
|
||||
<remap from="cloud" to="cloudXYZ"/>
|
||||
|
||||
<param name="voxel_size" type="double" value="0.05"/>
|
||||
<param name="decimation" type="int" value="4"/>
|
||||
<param name="max_depth" type="double" value="4"/>
|
||||
</node>
|
||||
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
|
||||
<remap from="cloud" to="cloudXYZ"/>
|
||||
<remap from="obstacles" to="/planner_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.0"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- Visual 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="GFTT/MaxCorners" type="string" value="500"/>
|
||||
<param name="GFTT/MinDistance" type="string" value="5"/>
|
||||
</node>
|
||||
|
||||
<group ns="rtabmap">
|
||||
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
|
||||
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
|
||||
<param name="frame_id" type="string" value="base_footprint"/>
|
||||
<param name="subscribe_stereo" type="bool" value="true"/>
|
||||
<param name="subscribe_depth" type="bool" value="false"/>
|
||||
|
||||
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
|
||||
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
|
||||
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
|
||||
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
|
||||
|
||||
<remap from="odom" to="/odometry"/>
|
||||
|
||||
<param name="queue_size" type="int" value="30"/>
|
||||
|
||||
<!-- RTAB-Map's parameters -->
|
||||
<param name="Rtabmap/TimeThr" type="string" value="700"/>
|
||||
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
|
||||
|
||||
<param name="Kp/WordsPerImage" type="string" value="200"/>
|
||||
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
|
||||
|
||||
<param name="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.05"/>
|
||||
|
||||
<param name="LccReextract/Activated" type="string" value="true"/>
|
||||
<param name="LccReextract/MaxWords" type="string" value="500"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,152 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<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="true"/>
|
||||
<param name="use_action_for_goal" type="bool" value="true"/>
|
||||
<param name="cloud_decimation" type="int" value="1"/> <!-- we already decimate in memory below -->
|
||||
<param name="grid_eroded" type="bool" value="true"/>
|
||||
<param name="grid_cell_size" type="double" value="0.05"/>
|
||||
|
||||
<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="grid_map" to="/map"/>
|
||||
|
||||
<!-- RTAB-Map's parameters -->
|
||||
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
|
||||
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
|
||||
|
||||
<param name="Reg/Strategy" type="string" value="1"/>
|
||||
|
||||
<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/ImagePostDecimation" type="string" value="4"/>
|
||||
|
||||
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
|
||||
<param name="Rtabmap/TimeThr" type="string" value="600"/>
|
||||
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
|
||||
|
||||
<param name="Bayes/PredictionLC" type="string" value="0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.00035"/>
|
||||
|
||||
<param name="Optimizer/Slam2D" type="string" value="true"/>
|
||||
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
|
||||
<param name="Optimizer/Strategy" type="string" value="0"/>
|
||||
|
||||
<param name="Kp/DetectorStrategy" type="string" value="0"/>
|
||||
<param name="Kp/MaxFeatures" type="string" value="200"/>
|
||||
<param name="SURF/HessianThreshold" type="string" value="500"/>
|
||||
|
||||
<param name="Reg/Force3DoF" type="string" value="true"/>
|
||||
<param name="Vis/MaxDepth" type="string" value="5"/>
|
||||
<param name="Vis/MinInliers" type="string" value="5"/>
|
||||
<param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
|
||||
|
||||
<!-- 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"/>
|
||||
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
|
||||
<param name="base_global_planner" value="navfn/NavfnROS"/>
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
|
||||
</node>
|
||||
|
||||
<param name="cmd_vel/abtr_priority" value="10"/>
|
||||
</group>
|
||||
|
||||
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
|
||||
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
|
||||
</node>
|
||||
|
||||
<!-- Arbitration between teleop and planner -->
|
||||
<node name="register_cmd_eta" pkg="abtr_priority" type="register"
|
||||
args="/cmd_eta /teleop/cmd_eta"/>
|
||||
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
|
||||
args="/cmd_vel /planner/cmd_vel"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
|
||||
<param name="rate" type="double" value="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>
|
||||
@@ -0,0 +1,48 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Choose visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<arg name="rtabmapviz" default="false" />
|
||||
<arg name="sub_data" default="false"/>
|
||||
|
||||
<!-- use a relay on this machine -->
|
||||
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
|
||||
<param name="lazy" type="bool" value="true"/>
|
||||
</node>
|
||||
<node if="$(arg sub_data)" name="scan_relay" type="relay" pkg="topic_tools" args="/base_scan /base_scan_relay">
|
||||
<param name="lazy" type="bool" value="true"/>
|
||||
</node>
|
||||
|
||||
<node if="$(arg sub_data)" name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_resized_image raw out:=/camera/data_resized_image_relay" />
|
||||
<node if="$(arg sub_data)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_resized_image_depth raw out:=/camera/data_resized_image_depth_relay" />
|
||||
|
||||
<node if="$(arg sub_data)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
|
||||
<remap from="rgb/image" to="/camera/data_resized_image_relay"/>
|
||||
<remap from="depth/image" to="/camera/data_resized_image_depth_relay"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_resized_camera_info"/>
|
||||
<remap from="cloud" to="/voxel_cloud" />
|
||||
</node>
|
||||
|
||||
<group ns="rtabmap">
|
||||
<!-- 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="queue_size" type="int" value="10"/>
|
||||
<param name="frame_id" type="string" value="base_footprint"/>
|
||||
<remap from="odom" to="/base_controller/odom"/>
|
||||
<remap from="mapData" to="mapData_relay"/>
|
||||
|
||||
<param name="subscribe_depth" type="bool" value="$(arg sub_data)"/>
|
||||
<remap from="rgb/image" to="/camera/data_resized_image_relay"/>
|
||||
<remap from="depth/image" to="/camera/data_resized_image_depth_relay"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_resized_camera_info"/>
|
||||
|
||||
<param name="subscribe_laserScan" type="bool" value="$(arg sub_data)"/>
|
||||
<remap from="scan" to="/base_scan_relay"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- Visualisation RVIZ -->
|
||||
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<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>
|
||||
|
||||
@@ -0,0 +1,162 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<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>
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Xtion -->
|
||||
<param name="/camera/driver/data_skip" value="1" />
|
||||
<include file="$(find openni2_launch)/launch/openni2.launch">
|
||||
<arg name="depth_registration" value="True" />
|
||||
<arg name="rgb_camera_info_url"
|
||||
value="file://$(find rtabmap_ros)/launch/calibration/rgb_PS1080_PrimeSense.yaml" />
|
||||
<arg name="depth_camera_info_url"
|
||||
value="file://$(find rtabmap_ros)/launch/calibration/depth_PS1080_PrimeSense.yaml" />
|
||||
</include>
|
||||
|
||||
<!-- Xtion frame -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
|
||||
args="0.057 0.087 0.185 0.0 0.0 0.0 /base_link /camera_link 100" />
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
|
||||
<param name="gui" value="true" type="bool"/>
|
||||
<param name="settings_path" value="$(find rtabmap_ros)/launch/azimut3/config/azimut3_find_object.ini" type="str"/>
|
||||
<param name="subscribe_depth" value="true" type="bool"/>
|
||||
<param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
|
||||
|
||||
<remap from="rgb/image_rect_color" to="camera/data_throttled_image_relay"/>
|
||||
<remap from="depth_registered/image_raw" to="camera/data_throttled_image_depth_relay"/>
|
||||
<remap from="depth_registered/camera_info" to="camera/data_throttled_camera_info_relay"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
|
||||
|
||||
<!-- OpenNI -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="5.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- SLAM is done on client side...-->
|
||||
</launch>
|
||||
@@ -0,0 +1,70 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Remote teleop -->
|
||||
<include file="$(find az3_bringup)/joystick.launch"/>
|
||||
|
||||
<!-- Visualization and SLAM nodes use same data, so just subscribe once and relay messages -->
|
||||
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
|
||||
<node name="odom_relay" type="relay" pkg="topic_tools" args="/base_controller/odom /base_controller/odom_relay"/>
|
||||
<node name="scan_relay" type="relay" pkg="topic_tools" args="/base_scan /base_scan_relay"/>
|
||||
<node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
|
||||
<node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
|
||||
<node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
|
||||
|
||||
<!-- SLAM client 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_relay"/>
|
||||
<remap from="scan" to="/base_scan_relay"/>
|
||||
|
||||
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
|
||||
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
|
||||
|
||||
<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/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
|
||||
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
|
||||
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
|
||||
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
|
||||
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
|
||||
<param name="LccIcp2/Iterations" type="string" value="100"/>
|
||||
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
|
||||
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure -->
|
||||
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
|
||||
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
|
||||
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
|
||||
<param name="Rtabmap/TimeThr" type="string" value="700"/>
|
||||
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
|
||||
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
|
||||
</node>
|
||||
|
||||
<!-- Grid map assembler for rviz -->
|
||||
<node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
|
||||
</group>
|
||||
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3.rviz"/>
|
||||
|
||||
<!-- Below, construct point cloud of the latest throttled data -->
|
||||
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
|
||||
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
|
||||
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
|
||||
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
|
||||
<remap from="cloud" to="voxel_cloud" />
|
||||
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
<param name="voxel_size" type="double" value="0.01"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,370 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.434783
|
||||
Tree Height: 410
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
- /Current View1/Focal Point1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: LaserScan
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.03
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: map
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.02684
|
||||
Min Value: -1.12646
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 2.34177e-38
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 9.21942e-41
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /voxel_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
az3_base_link:
|
||||
Value: true
|
||||
az3_odom:
|
||||
Value: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
stereo_camera:
|
||||
Value: true
|
||||
stereo_camera_base:
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Value: true
|
||||
wheelRF_wheel_link:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
map:
|
||||
odom:
|
||||
base_footprint:
|
||||
base_link:
|
||||
base_laser_link:
|
||||
{}
|
||||
stereo_camera_base:
|
||||
stereo_camera:
|
||||
{}
|
||||
wheelLB_linkWheel_link:
|
||||
wheelLB_wheel_link:
|
||||
{}
|
||||
wheelLF_linkWheel_link:
|
||||
wheelLF_wheel_link:
|
||||
{}
|
||||
wheelRB_linkWheel_link:
|
||||
wheelRB_wheel_link:
|
||||
{}
|
||||
wheelRF_linkWheel_link:
|
||||
wheelRF_wheel_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 3.66965
|
||||
Min Value: -0.144665
|
||||
Value: true
|
||||
Axis: X
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
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: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 5
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /base_scan
|
||||
Use Fixed Frame: false
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /rtabmap/grid_map
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRF_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /camera/data_throttled_image_relay
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
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: rtabmap_ros/MapCloud
|
||||
Cloud decimation: 4
|
||||
Cloud max depth (m): 3
|
||||
Cloud voxel size (m): 0.02
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Download graph: false
|
||||
Download map: false
|
||||
Enabled: true
|
||||
Filter floor (m): 0
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: MapCloud
|
||||
Node filtering angle (degrees): 20
|
||||
Node filtering radius (m): 0.2
|
||||
Position Transformer: XYZ
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /rtabmap/mapData
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- class: rtabmap_ros/Info
|
||||
Enabled: true
|
||||
Name: Info
|
||||
Topic: /rtabmap/info
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /rtabmap/grid_projection_map
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
class: rtabmap_ros/OrbitOriented
|
||||
Distance: 7.83194
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.0199225
|
||||
Y: 0.23516
|
||||
Z: -0.0896359
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 0.699796
|
||||
Target Frame: base_link
|
||||
Value: OrbitOriented (rtabmap)
|
||||
Yaw: 3.19875
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 763
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000229000000dd00fffffffb0000000a0049006d006100670065010000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1561
|
||||
X: 42
|
||||
Y: 108
|
||||
@@ -0,0 +1,479 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.601881
|
||||
Tree Height: 353
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Info
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.03
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: map
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
camera_depth_frame:
|
||||
Value: true
|
||||
camera_depth_optical_frame:
|
||||
Value: true
|
||||
camera_link:
|
||||
Value: true
|
||||
camera_rgb_frame:
|
||||
Value: true
|
||||
camera_rgb_optical_frame:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Value: true
|
||||
wheelRF_wheel_link:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
map:
|
||||
odom:
|
||||
base_footprint:
|
||||
base_link:
|
||||
base_laser_link:
|
||||
{}
|
||||
camera_link:
|
||||
camera_depth_frame:
|
||||
camera_depth_optical_frame:
|
||||
{}
|
||||
camera_rgb_frame:
|
||||
camera_rgb_optical_frame:
|
||||
{}
|
||||
wheelLB_linkWheel_link:
|
||||
wheelLB_wheel_link:
|
||||
{}
|
||||
wheelLF_linkWheel_link:
|
||||
wheelLF_wheel_link:
|
||||
{}
|
||||
wheelRB_linkWheel_link:
|
||||
wheelRB_wheel_link:
|
||||
{}
|
||||
wheelRF_linkWheel_link:
|
||||
wheelRF_wheel_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 3.46596
|
||||
Min Value: -5.27159e-08
|
||||
Value: true
|
||||
Axis: X
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
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: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 5
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /base_scan_relay
|
||||
Use Fixed Frame: false
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /map
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Global costmap
|
||||
Topic: /planner/move_base/global_costmap/costmap
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: false
|
||||
Name: Local costmap
|
||||
Topic: /planner/move_base/local_costmap/costmap
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRF_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /camera/throttled_image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: theora
|
||||
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: rtabmap_ros/MapCloud
|
||||
Cloud decimation: 1
|
||||
Cloud max depth (m): 4
|
||||
Cloud voxel size (m): 0
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Download graph: false
|
||||
Download map: false
|
||||
Enabled: true
|
||||
Filter floor (m): 0.07
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: MapCloud
|
||||
Node filtering angle (degrees): 0
|
||||
Node filtering radius (m): 0
|
||||
Position Transformer: XYZ
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /rtabmap/mapData
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Class: rtabmap_ros/Info
|
||||
Enabled: true
|
||||
Name: Info
|
||||
Topic: /rtabmap/info
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 255; 149; 57
|
||||
Enabled: true
|
||||
Line Style: Lines
|
||||
Line Width: 0.03
|
||||
Name: move_base global plan
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Topic: /planner/move_base/NavfnROS/plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 2; 14; 255
|
||||
Enabled: true
|
||||
Line Style: Lines
|
||||
Line Width: 0.03
|
||||
Name: move_base local plan
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 4.11231
|
||||
Min Value: -0.00766382
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 85; 255; 0
|
||||
Color Transformer: RGB8
|
||||
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: Voxel_cloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /voxel_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rtabmap_ros/MapGraph
|
||||
Enabled: true
|
||||
Global loop closure: 255; 0; 0
|
||||
Local loop closure: 255; 255; 0
|
||||
Merged neighbor: 255; 170; 0
|
||||
Name: MapGraph
|
||||
Neighbor: 0; 0; 255
|
||||
Topic: /rtabmap/mapGraph
|
||||
User: 255; 0; 0
|
||||
Value: true
|
||||
Virtual: 255; 0; 255
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 255; 0; 255
|
||||
Enabled: true
|
||||
Line Style: Lines
|
||||
Line Width: 0.03
|
||||
Name: Rtabmap global path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Topic: /rtabmap/global_path
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 85; 255; 255
|
||||
Enabled: true
|
||||
Line Style: Lines
|
||||
Line Width: 0.03
|
||||
Name: Rtabmap local path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Topic: /rtabmap/local_path
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.1
|
||||
Class: rviz/Pose
|
||||
Color: 0; 255; 0
|
||||
Enabled: true
|
||||
Head Length: 0.3
|
||||
Head Radius: 0.1
|
||||
Name: Current goal
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05
|
||||
Shape: Arrow
|
||||
Topic: /rtabmap/current_goal
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.1
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.3
|
||||
Head Radius: 0.1
|
||||
Name: Global goal
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05
|
||||
Shape: Arrow
|
||||
Topic: /rtabmap/goal
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /rtabmap/goal
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 13.3786
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -1.4347
|
||||
Y: 1.38269
|
||||
Z: -0.0856586
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 1.2448
|
||||
Target Frame: base_footprint
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.76044
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 800
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a2000000dd00fffffffb0000000a0049006d00610067006501000001d0000000870000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1228
|
||||
X: 396
|
||||
Y: 144
|
||||
@@ -0,0 +1,444 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.601881
|
||||
Tree Height: 304
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
- /Current View1/Focal Point1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: PointCloud2
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.03
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: map
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.02684
|
||||
Min Value: -1.12646
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 2.34177e-38
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 9.21942e-41
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /odom_last_frame
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
az3_base_link:
|
||||
Value: true
|
||||
az3_odom:
|
||||
Value: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
stereo_camera:
|
||||
Value: true
|
||||
stereo_camera_base:
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Value: true
|
||||
wheelRF_wheel_link:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
map:
|
||||
odom:
|
||||
base_footprint:
|
||||
base_link:
|
||||
base_laser_link:
|
||||
{}
|
||||
stereo_camera_base:
|
||||
stereo_camera:
|
||||
{}
|
||||
wheelLB_linkWheel_link:
|
||||
wheelLB_wheel_link:
|
||||
{}
|
||||
wheelLF_linkWheel_link:
|
||||
wheelLF_wheel_link:
|
||||
{}
|
||||
wheelRB_linkWheel_link:
|
||||
wheelRB_wheel_link:
|
||||
{}
|
||||
wheelRF_linkWheel_link:
|
||||
wheelRF_wheel_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: -0.851967
|
||||
Min Value: -2.15198
|
||||
Value: true
|
||||
Axis: X
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
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: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 5
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /base_scan
|
||||
Use Fixed Frame: false
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /map
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /planner/move_base/local_costmap/costmap
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /planner/move_base/global_costmap/costmap
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheelRF_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /stereo_camera/left/image_rect_color_relay
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
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: rtabmap_ros/MapCloud
|
||||
Cloud decimation: 8
|
||||
Cloud max depth (m): 4
|
||||
Cloud voxel size (m): 0
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Download graph: false
|
||||
Download map: false
|
||||
Enabled: true
|
||||
Filter floor (m): 0.1
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: MapCloud
|
||||
Node filtering angle (degrees): 0
|
||||
Node filtering radius (m): 0
|
||||
Position Transformer: XYZ
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /rtabmap/mapData_relay
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- class: rtabmap_ros/Info
|
||||
Enabled: true
|
||||
Name: Info
|
||||
Topic: /rtabmap/info
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.1
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.3
|
||||
Head Radius: 0.1
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05
|
||||
Shape: Arrow
|
||||
Topic: /planner_goal
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Name: Path
|
||||
Topic: /planner/move_base/TrajectoryPlannerROS/global_plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 2; 14; 255
|
||||
Enabled: true
|
||||
Name: Path
|
||||
Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
class: rtabmap_ros/MapGraph
|
||||
Color: 0; 0; 255
|
||||
Enabled: true
|
||||
Name: MapGraph
|
||||
Topic: /rtabmap/mapData_relay
|
||||
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: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /odom_local_map
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /planner_goal
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
class: rtabmap_ros/OrbitOriented
|
||||
Distance: 6.3581
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.0235024
|
||||
Y: -0.0419514
|
||||
Z: 0.22792
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 0.799797
|
||||
Target Frame: base_footprint
|
||||
Value: OrbitOriented (rtabmap)
|
||||
Yaw: 3.11245
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 765
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000151000002b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000171000000dd00fffffffb0000000a0049006d006100670065010000019f000001400000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000001d1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001d1000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000042f000002b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1414
|
||||
X: 159
|
||||
Y: 54
|
||||
@@ -0,0 +1,41 @@
|
||||
TrajectoryPlannerROS:
|
||||
|
||||
# Current limits based on AZ3 standalone configuration.
|
||||
acc_lim_x: 0.75
|
||||
acc_lim_y: 0.75
|
||||
acc_lim_theta: 4
|
||||
# min_vel_x and max_rotational_vel were set to keep the ICR at
|
||||
# minimal distance of 0.48 m.
|
||||
# Basically, max_rotational_vel * rho_min <= min_vel_x
|
||||
max_vel_x: 0.5
|
||||
min_vel_x: 0.24
|
||||
max_vel_theta: 0.5
|
||||
min_vel_theta: -0.5
|
||||
min_in_place_vel_theta: 0.25
|
||||
holonomic_robot: true
|
||||
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
latch_xy_goal_tolerance: true
|
||||
|
||||
# make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
|
||||
sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
|
||||
sim_granularity: 0.025
|
||||
angular_sim_granularity: 0.05
|
||||
vx_samples: 12
|
||||
vtheta_samples: 20
|
||||
|
||||
meter_scoring: true
|
||||
|
||||
pdist_scale: 0.7 # The higher will follow more the global path.
|
||||
gdist_scale: 0.8
|
||||
occdist_scale: 0.01
|
||||
publish_cost_grid_pc: false
|
||||
|
||||
#move_base
|
||||
controller_frequency: 10.0 #The robot can move faster when higher.
|
||||
|
||||
#global planner
|
||||
NavfnROS:
|
||||
allow_unknown: true
|
||||
visualize_potential: false
|
||||
@@ -0,0 +1,10 @@
|
||||
footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]]
|
||||
footprint_padding: 0.02
|
||||
#robot_radius: 0.38
|
||||
#robot_radius: ir_of_robot
|
||||
inflation_layer:
|
||||
inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
|
||||
transform_tolerance: 2
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]]
|
||||
footprint_padding: 0.04
|
||||
inflation_layer:
|
||||
inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
|
||||
transform_tolerance: 2
|
||||
|
||||
obstacle_layer:
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3
|
||||
max_obstacle_height: 0.4
|
||||
track_unknown_space: true
|
||||
|
||||
observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
|
||||
|
||||
laser_scan_sensor: {
|
||||
data_type: LaserScan,
|
||||
topic: scan,
|
||||
expected_update_rate: 0.1,
|
||||
marking: true,
|
||||
clearing: true
|
||||
}
|
||||
|
||||
point_cloud_sensorA: {
|
||||
sensor_frame: base_footprint,
|
||||
data_type: PointCloud2,
|
||||
topic: obstacles_cloud,
|
||||
expected_update_rate: 0.5,
|
||||
marking: true,
|
||||
clearing: true,
|
||||
min_obstacle_height: 0.04
|
||||
}
|
||||
|
||||
point_cloud_sensorB: {
|
||||
sensor_frame: base_footprint,
|
||||
data_type: PointCloud2,
|
||||
topic: ground_cloud,
|
||||
expected_update_rate: 0.5,
|
||||
marking: false,
|
||||
clearing: true,
|
||||
min_obstacle_height: -1.0 # make sure the ground is not filtered
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
#global_costmap
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 1
|
||||
publish_frequency: 1
|
||||
always_send_full_costmap: false
|
||||
plugins:
|
||||
- {name: static_layer, type: "rtabmap_ros::StaticLayer"}
|
||||
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
|
||||
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 2
|
||||
publish_frequency: 1
|
||||
rolling_window: true
|
||||
width: 3.0
|
||||
height: 3.0
|
||||
resolution: 0.025
|
||||
origin_x: 0
|
||||
origin_y: 0
|
||||
|
||||
plugins:
|
||||
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
|
||||
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
|
||||
@@ -0,0 +1,51 @@
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 2.0
|
||||
publish_frequency: 2.0
|
||||
rolling_window: true
|
||||
width: 4.0
|
||||
height: 4.0
|
||||
resolution: 0.025
|
||||
origin_x: -2.0
|
||||
origin_y: -2.0
|
||||
|
||||
plugins:
|
||||
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
|
||||
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
|
||||
|
||||
obstacle_layer:
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
max_obstacle_height: 0.4
|
||||
track_unknown_space: true
|
||||
|
||||
observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
|
||||
|
||||
laser_scan_sensor: {
|
||||
data_type: LaserScan,
|
||||
topic: base_scan,
|
||||
expected_update_rate: 0.2,
|
||||
marking: true,
|
||||
clearing: true
|
||||
}
|
||||
|
||||
point_cloud_sensorA: {
|
||||
sensor_frame: base_footprint,
|
||||
data_type: PointCloud2,
|
||||
topic: obstacles_cloud,
|
||||
expected_update_rate: 0.5,
|
||||
marking: true,
|
||||
clearing: true,
|
||||
min_obstacle_height: 0.04
|
||||
}
|
||||
|
||||
point_cloud_sensorB: {
|
||||
sensor_frame: base_footprint,
|
||||
data_type: PointCloud2,
|
||||
topic: ground_cloud,
|
||||
expected_update_rate: 0.5,
|
||||
marking: false,
|
||||
clearing: true,
|
||||
min_obstacle_height: -1.0 # make usre the ground is not filtered
|
||||
}
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
local_costmap:
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 2.0
|
||||
publish_frequency: 2.0
|
||||
static_map: false
|
||||
rolling_window: true
|
||||
width: 4.0
|
||||
height: 4.0
|
||||
resolution: 0.025
|
||||
origin_x: -2.0
|
||||
origin_y: -2.0
|
||||
|
||||
#observation_sources: laser_scan_sensor point_cloud_sensor
|
||||
observation_sources: point_cloud_sensor
|
||||
|
||||
laser_scan_sensor: {
|
||||
data_type: LaserScan,
|
||||
topic: base_scan,
|
||||
expected_update_rate: 0.2,
|
||||
marking: true,
|
||||
clearing: true}
|
||||
|
||||
# assuming receiving a cloud from rtabmap/obstacles_detection node
|
||||
point_cloud_sensor: {
|
||||
sensor_frame: base_footprint,
|
||||
data_type: PointCloud2,
|
||||
topic: openni_points,
|
||||
expected_update_rate: 0.5,
|
||||
marking: true,
|
||||
clearing: true,
|
||||
min_obstacle_height: -99999.0,
|
||||
max_obstacle_height: 0.5}
|
||||
@@ -0,0 +1,20 @@
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: 00b09d010081e00e_left
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [520.84106910681, 0, 320.207922533597, 0, 520.652683004955, 251.69140630101, 0, 0, 1]
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [-0.344858300062205, 0.131731614744127, -0.00032220157418798, -0.000178643627395838, 0]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [0.999977409119708, -0.00542294021453702, -0.00397151981806852, 0.00542696944573559, 0.999984769417176, 0.00100445821860056, 0.00396601221263954, -0.00102598884371095, 0.999991609011807]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [487.608731712861, 0, 318.1162109375, 0, 0, 487.608731712861, 249.44425201416, 0, 0, 0, 1, 0]
|
||||
@@ -0,0 +1,20 @@
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: 00b09d010081e00e_right
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [525.042672813, 0, 315.778978739153, 0, 524.605377865008, 246.116481979902, 0, 0, 1]
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [-0.350198880846778, 0.143262162037345, -0.000540958577710845, -0.000386869942974346, 0]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [0.999991166299663, -0.00384047779504094, 0.00170823094047773, 0.00384221006573169, 0.999992106658553, -0.00101194980141688, -0.0017043310860856, 0.0010185042442697, 0.999998028950384]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [487.608731712861, 0, 318.1162109375, -58.3626989865376, 0, 487.608731712861, 249.44425201416, 0, 0, 0, 1, 0]
|
||||
@@ -0,0 +1,20 @@
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: depth_PS1080_PrimeSense
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [546.0500834384593, 0.0, 318.26543549764034, 0.0, 545.7495208147751, 235.530988776277, 0.0, 0.0, 1.0]
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [-0.02445400737474688, 0.015558351939314555, 0.0010451121492915797, -0.004270544037870965, 0.0]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [579.1331176757812, 0.0, 314.36071062675546, 0.0, 0.0, 580.1824340820312, 251.67534864987465, 0.0, 0.0, 0.0, 1.0, 0.0]
|
||||
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 551 KiB |
@@ -0,0 +1,30 @@
|
||||
#%YAML:1.0
|
||||
---
|
||||
camera_name: MH_01_easy_left
|
||||
image_width: 752
|
||||
image_height: 480
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [ 4.5865400000000000e+02, 0., 3.6721499999999997e+02, 0.,
|
||||
4.5729599999999999e+02, 2.4837500000000000e+02, 0., 0., 1. ]
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 4
|
||||
data: [ -2.8340810999999999e-01, 7.3959070000000002e-02,
|
||||
1.9358999999999999e-04, 1.7618711400000001e-05 ]
|
||||
distortion_model: plumb_bob
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [ 9.9996634750298619e-01, -1.4227432298321767e-03,
|
||||
8.0795831104762770e-03, 1.3657459036274155e-03,
|
||||
9.9997417608074302e-01, 7.0556296505566432e-03,
|
||||
-8.0894128132919258e-03, -7.0443575534647465e-03,
|
||||
9.9994246755850658e-01 ]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [ 4.3520469597145990e+02, 0., 3.6745172119140625e+02, 0., 0.,
|
||||
4.3520469597145990e+02, 2.5220085144042969e+02, 0., 0., 0., 1.,
|
||||
0. ]
|
||||
@@ -0,0 +1,30 @@
|
||||
#%YAML:1.0
|
||||
---
|
||||
camera_name: MH_01_easy_right
|
||||
image_width: 752
|
||||
image_height: 480
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [ 4.5758699999999999e+02, 0., 3.7999900000000002e+02, 0.,
|
||||
4.5613400000000001e+02, 2.5523800000000000e+02, 0., 0., 1. ]
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 4
|
||||
data: [ -2.8368365000000001e-01, 7.4512839999999997e-02,
|
||||
-1.0473000000000000e-04, -3.5559070000000001e-05 ]
|
||||
distortion_model: plumb_bob
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [ 9.9996335257946345e-01, -3.6258159819210472e-03,
|
||||
7.7554468926514068e-03, 3.6804026836554193e-03,
|
||||
9.9996847525904631e-01, -7.0358456623254633e-03,
|
||||
-7.7296917225483383e-03, 7.0641309842873097e-03,
|
||||
9.9994517345668066e-01 ]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [ 4.3520469597145990e+02, 0., 3.6745172119140625e+02,
|
||||
-4.7906395664848930e+01, 0., 4.3520469597145990e+02,
|
||||
2.5220085144042969e+02, 0., 0., 0., 1., 0. ]
|
||||
@@ -0,0 +1,21 @@
|
||||
%YAML:1.0
|
||||
cameraMatrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 1035.409658, 0., 953.629091, 0., 1036.265896, 550.972045, 0., 0., 1. ]
|
||||
distortionCoefficients: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 5
|
||||
dt: d
|
||||
data: [ 0.046857, -0.054802, 0.002446, -0.001013, 0. ]
|
||||
rotation: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
projection: !!opencv-matrix
|
||||
rows: 4
|
||||
cols: 4
|
||||
dt: d
|
||||
data: [ 1034.458862, 0., 949.608678, 0., 0., 1046.304565, 553.032416, 0., 0., 0., 1., 0., 0., 0., 0., 1. ]
|
||||
@@ -0,0 +1,26 @@
|
||||
%YAML:1.0
|
||||
cameraMatrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 3.6979200177991152e+02, 0., 2.5040545556144602e+02, 0.,
|
||||
3.6934874236487565e+02, 2.0812949708935565e+02, 0., 0., 1. ]
|
||||
distortionCoefficients: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 5
|
||||
dt: d
|
||||
data: [ 1.0599430434497996e-01, -2.7091691685678082e-01,
|
||||
4.8231954365675019e-04, -3.1761820716379199e-05,
|
||||
7.7492684133604814e-02 ]
|
||||
rotation: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
projection: !!opencv-matrix
|
||||
rows: 4
|
||||
cols: 4
|
||||
dt: d
|
||||
data: [ 3.6979200177991152e+02, 0., 2.5040545556144602e+02, 0., 0.,
|
||||
3.6934874236487565e+02, 2.0812949708935565e+02, 0., 0., 0., 1.,
|
||||
0., 0., 0., 0., 1. ]
|
||||
@@ -0,0 +1,33 @@
|
||||
%YAML:1.0
|
||||
rotation: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 9.9998557008106792e-01, 1.2305561691822327e-03,
|
||||
-5.2292792195576393e-03, -1.3388135514396061e-03,
|
||||
9.9978381286062756e-01, -2.0749340233854399e-02,
|
||||
5.2026154880109553e-03, 2.0756041852440336e-02,
|
||||
9.9977103354653341e-01 ]
|
||||
translation: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 1
|
||||
dt: d
|
||||
data: [ -5.1351463516082281e-02, 2.3299760970435590e-03,
|
||||
-1.6640325723173980e-02 ]
|
||||
essential: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ -1.0156323849380249e-05, 1.6685089380143084e-02,
|
||||
1.9841668306476608e-03, -1.6372923685201993e-02,
|
||||
1.0453762704480134e-03, 5.1426722663111546e-02,
|
||||
-2.2611924404357772e-03, -5.1343229156542415e-02,
|
||||
1.0776930835878881e-03 ]
|
||||
fundamental: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ -4.8742459614193642e-09, 8.0171558549327292e-06,
|
||||
-1.3152531820940182e-03, -7.8512380180953911e-06,
|
||||
5.0188640603529770e-07, 1.0980767538413172e-02,
|
||||
3.2068378477732255e-03, -3.3465816946061419e-02, 1. ]
|
||||
@@ -0,0 +1,12 @@
|
||||
Kinect v2 calibration:
|
||||
Copy "506816242542" folder in "~/catkin_ws/src/iai_kinect2/kinect2_bridge/data"
|
||||
|
||||
The number is the Kinect serial shown when launching kinect2_brige:
|
||||
...
|
||||
[Freenect2Impl] enumerating devices...
|
||||
[Freenect2Impl] 12 usb devices connected
|
||||
[Freenect2Impl] found valid Kinect v2 @4:3 with serial 506816242542
|
||||
[Freenect2Impl] found 1 devices
|
||||
Kinect2 devices found:
|
||||
0: 506816242542 (selected)
|
||||
...
|
||||
@@ -0,0 +1,20 @@
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: rgb_PS1080_PrimeSense
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [546.0500834384593, 0.0, 318.26543549764034, 0.0, 545.7495208147751, 235.530988776277, 0.0, 0.0, 1.0]
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [0.07270573116068803, -0.18543929362134534, -0.0011832045804703304, -0.0034080201993029564, 0.0]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [546.8461303710938, 0.0, 315.7269048503658, 0.0, 0.0, 549.0361938476562, 234.61483550908815, 0.0, 0.0, 0.0, 1.0, 0.0]
|
||||
@@ -0,0 +1,363 @@
|
||||
[Gui]
|
||||
AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\0\0\0\0\x1b\0\0\x2\xf7\0\0\x2\xfc\0\0\0\0\0\0\0\x1b\0\0\x2\xf7\0\0\x2\xfc\0\0\0\0\0\0\0\0\a\x80)
|
||||
DepthCalibrationDialog\bin_depth=2
|
||||
DepthCalibrationDialog\bin_height=6
|
||||
DepthCalibrationDialog\bin_width=8
|
||||
DepthCalibrationDialog\cone_radius=0.02
|
||||
DepthCalibrationDialog\cone_stddev_thresh=0.1
|
||||
DepthCalibrationDialog\decimation=1
|
||||
DepthCalibrationDialog\laser_scan=false
|
||||
DepthCalibrationDialog\max_depth=3.5
|
||||
DepthCalibrationDialog\max_model_depth=10
|
||||
DepthCalibrationDialog\min_depth=0
|
||||
DepthCalibrationDialog\smoothing=1
|
||||
DepthCalibrationDialog\voxel=0.01
|
||||
ExportBundlerDialog\exportPoints=false
|
||||
ExportBundlerDialog\laplacianThr=0
|
||||
ExportBundlerDialog\maxAngularSpeed=0
|
||||
ExportBundlerDialog\maxLinearSpeed=0
|
||||
ExportBundlerDialog\sba_iterations=20
|
||||
ExportBundlerDialog\sba_rematch_features=true
|
||||
ExportBundlerDialog\sba_type=0
|
||||
ExportBundlerDialog\sba_variance=1
|
||||
ExportCloudsDialog\assemble=true
|
||||
ExportCloudsDialog\assemble_voxel=0.005
|
||||
ExportCloudsDialog\bilateral=false
|
||||
ExportCloudsDialog\bilateral_sigma_r=0.1
|
||||
ExportCloudsDialog\bilateral_sigma_s=10
|
||||
ExportCloudsDialog\binary=true
|
||||
ExportCloudsDialog\cputsdf_flattenRadius=0.005
|
||||
ExportCloudsDialog\cputsdf_minWeight=0
|
||||
ExportCloudsDialog\cputsdf_randomSplit=1
|
||||
ExportCloudsDialog\cputsdf_resolution=0.01
|
||||
ExportCloudsDialog\cputsdf_size=12
|
||||
ExportCloudsDialog\cputsdf_truncNeg=0.03
|
||||
ExportCloudsDialog\cputsdf_truncPos=0.03
|
||||
ExportCloudsDialog\filtering=false
|
||||
ExportCloudsDialog\filtering_min_neighbors=2
|
||||
ExportCloudsDialog\filtering_radius=0.02
|
||||
ExportCloudsDialog\frame=0
|
||||
ExportCloudsDialog\from_depth=true
|
||||
ExportCloudsDialog\gain=false
|
||||
ExportCloudsDialog\gain_beta=10
|
||||
ExportCloudsDialog\gain_full=false
|
||||
ExportCloudsDialog\gain_overlap=0
|
||||
ExportCloudsDialog\gain_radius=0.02
|
||||
ExportCloudsDialog\gain_rgb=true
|
||||
ExportCloudsDialog\mesh=false
|
||||
ExportCloudsDialog\mesh_angle_tolerance=15
|
||||
ExportCloudsDialog\mesh_clean=true
|
||||
ExportCloudsDialog\mesh_color_radius=0.03
|
||||
ExportCloudsDialog\mesh_decimation_factor=0
|
||||
ExportCloudsDialog\mesh_dense_strategy=1
|
||||
ExportCloudsDialog\mesh_k=20
|
||||
ExportCloudsDialog\mesh_max_polygons=0
|
||||
ExportCloudsDialog\mesh_min_cluster_size=0
|
||||
ExportCloudsDialog\mesh_mu=2.5
|
||||
ExportCloudsDialog\mesh_quad=false
|
||||
ExportCloudsDialog\mesh_radius=0.04
|
||||
ExportCloudsDialog\mesh_texture=false
|
||||
ExportCloudsDialog\mesh_textureBlending=true
|
||||
ExportCloudsDialog\mesh_textureBlendingDecimation=0
|
||||
ExportCloudsDialog\mesh_textureBrightnessConstrastRatioHigh=0
|
||||
ExportCloudsDialog\mesh_textureBrightnessConstrastRatioLow=0
|
||||
ExportCloudsDialog\mesh_textureCameraFiltering=false
|
||||
ExportCloudsDialog\mesh_textureCameraFilteringAngle=30
|
||||
ExportCloudsDialog\mesh_textureCameraFilteringLaplacian=0
|
||||
ExportCloudsDialog\mesh_textureCameraFilteringRadius=0
|
||||
ExportCloudsDialog\mesh_textureCameraFilteringVel=0
|
||||
ExportCloudsDialog\mesh_textureCameraFilteringVelRad=0
|
||||
ExportCloudsDialog\mesh_textureExposureFusion=false
|
||||
ExportCloudsDialog\mesh_textureFormat=0
|
||||
ExportCloudsDialog\mesh_textureMaxAngle=0
|
||||
ExportCloudsDialog\mesh_textureMaxCount=1
|
||||
ExportCloudsDialog\mesh_textureMaxDepthError=0
|
||||
ExportCloudsDialog\mesh_textureMaxDistance=3
|
||||
ExportCloudsDialog\mesh_textureMinCluster=50
|
||||
ExportCloudsDialog\mesh_textureMultiband=false
|
||||
ExportCloudsDialog\mesh_textureRoiRatios=0.0 0.0 0.0 0.0
|
||||
ExportCloudsDialog\mesh_textureSize=5
|
||||
ExportCloudsDialog\mesh_triangle_size=2
|
||||
ExportCloudsDialog\mls=false
|
||||
ExportCloudsDialog\mls_dilation_iterations=0
|
||||
ExportCloudsDialog\mls_dilation_voxel_size=0.01
|
||||
ExportCloudsDialog\mls_output_voxel_size=0
|
||||
ExportCloudsDialog\mls_point_density=0
|
||||
ExportCloudsDialog\mls_polygonial_order=2
|
||||
ExportCloudsDialog\mls_radius=0.04
|
||||
ExportCloudsDialog\mls_upsampling_method=0
|
||||
ExportCloudsDialog\mls_upsampling_radius=0.01
|
||||
ExportCloudsDialog\mls_upsampling_step=0
|
||||
ExportCloudsDialog\normals_k=10
|
||||
ExportCloudsDialog\normals_radius=0
|
||||
ExportCloudsDialog\openchisel_carving_dist_m=0.05
|
||||
ExportCloudsDialog\openchisel_chunk_size_x=16
|
||||
ExportCloudsDialog\openchisel_chunk_size_y=16
|
||||
ExportCloudsDialog\openchisel_chunk_size_z=16
|
||||
ExportCloudsDialog\openchisel_far_plane_dist=1.1
|
||||
ExportCloudsDialog\openchisel_integration_weight=1
|
||||
ExportCloudsDialog\openchisel_merge_vertices=true
|
||||
ExportCloudsDialog\openchisel_near_plane_dist=0.05
|
||||
ExportCloudsDialog\openchisel_truncation_constant=0.001504
|
||||
ExportCloudsDialog\openchisel_truncation_linear=0.00152
|
||||
ExportCloudsDialog\openchisel_truncation_quadratic=0.0019
|
||||
ExportCloudsDialog\openchisel_truncation_scale=10
|
||||
ExportCloudsDialog\openchisel_use_voxel_carving=false
|
||||
ExportCloudsDialog\pipeline=0
|
||||
ExportCloudsDialog\poisson_depth=0
|
||||
ExportCloudsDialog\poisson_iso=8
|
||||
ExportCloudsDialog\poisson_manifold=true
|
||||
ExportCloudsDialog\poisson_minDepth=5
|
||||
ExportCloudsDialog\poisson_outputPolygons=false
|
||||
ExportCloudsDialog\poisson_pointWeight=4
|
||||
ExportCloudsDialog\poisson_samples=1
|
||||
ExportCloudsDialog\poisson_scale=1.1
|
||||
ExportCloudsDialog\poisson_solver=8
|
||||
ExportCloudsDialog\regenerate=true
|
||||
ExportCloudsDialog\regenerate_decimation=1
|
||||
ExportCloudsDialog\regenerate_distortion_model=
|
||||
ExportCloudsDialog\regenerate_fill_error=2
|
||||
ExportCloudsDialog\regenerate_fill_size=0
|
||||
ExportCloudsDialog\regenerate_max_depth=4
|
||||
ExportCloudsDialog\regenerate_min_depth=0
|
||||
ExportCloudsDialog\regenerate_roi=0.0 0.0 0.0 0.0
|
||||
ExportCloudsDialog\regenerate_scan_decimation=1
|
||||
ExportCloudsDialog\regenerate_scan_max_range=0
|
||||
ExportCloudsDialog\regenerate_scan_min_range=0
|
||||
ExportCloudsDialog\regenerate_voxel=0.005
|
||||
ExportCloudsDialog\subtract=false
|
||||
ExportCloudsDialog\subtract_min_neighbors=5
|
||||
ExportCloudsDialog\subtract_point_angle=0
|
||||
ExportCloudsDialog\subtract_point_radius=0.02
|
||||
ExportScansDialog\assemble=true
|
||||
ExportScansDialog\assemble_voxel=0.01
|
||||
ExportScansDialog\binary=true
|
||||
ExportScansDialog\filtering=false
|
||||
ExportScansDialog\filtering_min_neighbors=2
|
||||
ExportScansDialog\filtering_radius=0.02
|
||||
ExportScansDialog\normals_k=20
|
||||
ExportScansDialog\regenerate=false
|
||||
ExportScansDialog\regenerate_decimation=1
|
||||
Figures\counts=
|
||||
Figures\curves=
|
||||
General\beep=false
|
||||
General\cloudCeilingHeight=0
|
||||
General\cloudFiltering=false
|
||||
General\cloudFilteringAngle=20
|
||||
General\cloudFilteringRadius=0.2
|
||||
General\cloudFloorHeight=0
|
||||
General\cloudNoiseMinNeighbors=5
|
||||
General\cloudNoiseRadius=0
|
||||
General\cloudVoxel=0
|
||||
General\cloudsKept=true
|
||||
General\colorScheme0=0
|
||||
General\colorScheme1=0
|
||||
General\colorSchemeScan0=0
|
||||
General\colorSchemeScan1=0
|
||||
General\decimation0=4
|
||||
General\decimation1=4
|
||||
General\downsamplingScan0=1
|
||||
General\downsamplingScan1=1
|
||||
General\figure_cache=true
|
||||
General\figure_time=true
|
||||
General\gravityLength0=1
|
||||
General\gravityLength1=1
|
||||
General\gravityShown0=false
|
||||
General\gravityShown1=false
|
||||
General\gridMapOpacity=0.75
|
||||
General\gridMapShown=false
|
||||
General\gtAlign=true
|
||||
General\imageHighestHypShown=false
|
||||
General\imageRejectedShown=true
|
||||
General\imagesKept=true
|
||||
General\landmarkSize=0
|
||||
General\localizationsGraphView=false
|
||||
General\loggerEventLevel=3
|
||||
General\loggerLevel=2
|
||||
General\loggerPauseLevel=3
|
||||
General\loggerPrintThreadId=false
|
||||
General\loggerPrintTime=true
|
||||
General\loggerType=1
|
||||
General\maxDepth0=4
|
||||
General\maxDepth1=0
|
||||
General\maxRange0=0
|
||||
General\maxRange1=0
|
||||
General\meshing=false
|
||||
General\meshing_angle=15
|
||||
General\meshing_quad=false
|
||||
General\meshing_texture=false
|
||||
General\meshing_triangle_size=2
|
||||
General\minDepth0=0
|
||||
General\minDepth1=0
|
||||
General\minRange0=0
|
||||
General\minRange1=0
|
||||
General\noFiltering=true
|
||||
General\nochangeGraphView=false
|
||||
General\normalKSearch=10
|
||||
General\normalRadiusSearch=0
|
||||
General\notifyNewGlobalPath=false
|
||||
General\octomap=false
|
||||
General\octomap_2dgrid=true
|
||||
General\octomap_3dmap=true
|
||||
General\octomap_depth=16
|
||||
General\octomap_point_size=5
|
||||
General\octomap_rendering_type=0
|
||||
General\odomDisabled=false
|
||||
General\odomOnlyInliersShown=false
|
||||
General\odomQualityThr=50
|
||||
General\odomRegistration=3
|
||||
General\opacity0=1
|
||||
General\opacity1=1
|
||||
General\opacityScan0=1
|
||||
General\opacityScan1=1
|
||||
General\posteriorGraphView=true
|
||||
General\ptSize0=1
|
||||
General\ptSize1=2
|
||||
General\ptSizeFeatures0=3
|
||||
General\ptSizeFeatures1=3
|
||||
General\ptSizeScan0=1
|
||||
General\ptSizeScan1=1
|
||||
General\roiRatios0=0.0 0.0 0.0 0.0
|
||||
General\roiRatios1=0.0 0.0 0.0 0.0
|
||||
General\scanCeilingHeight=0
|
||||
General\scanFloorHeight=0
|
||||
General\scanNormalKSearch=0
|
||||
General\scanNormalRadiusSearch=0
|
||||
General\showClouds0=true
|
||||
General\showClouds1=true
|
||||
General\showFeatures0=false
|
||||
General\showFeatures1=true
|
||||
General\showFrames=false
|
||||
General\showFrustums0=false
|
||||
General\showFrustums1=false
|
||||
General\showGraphs=true
|
||||
General\showIMUAcc=false
|
||||
General\showIMUGravity=false
|
||||
General\showLabels=false
|
||||
General\showLandmarks=true
|
||||
General\showScans0=true
|
||||
General\showScans1=true
|
||||
General\subtractFiltering=false
|
||||
General\subtractFilteringAngle=0
|
||||
General\subtractFilteringMinPts=5
|
||||
General\subtractFilteringRadius=0.02
|
||||
General\verticalLayoutUsed=true
|
||||
General\voxelSizeScan0=0
|
||||
General\voxelSizeScan1=0
|
||||
General\wordsGraphView=false
|
||||
MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\x43\0\0\0\x1b\0\0\x5\xa5\0\0\x3\xf\0\0\0\x43\0\0\0\x39\0\0\x5\xa5\0\0\x3\xf\0\0\0\0\0\0\0\0\a\x80)
|
||||
MainWindow\maximized=false
|
||||
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1N\0\0\x2\x9a\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\x1\0\0\0=\0\0\x2\x9a\0\0\x2)\0\xff\xff\xff\0\0\0\x1\0\0\x4\xf\0\0\x2\x9a\xfc\x2\0\0\0\x2\xfc\0\0\0=\0\0\x2\x9a\0\0\0\xdb\0\xff\xff\xff\xfc\x1\0\0\0\x3\xfc\0\0\x1T\0\0\x1\x6\0\0\0h\0\xff\xff\xff\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_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1%\0\0\0\x13\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\x9a\0\0\0\x37\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\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\x2`\0\0\x1\xde\0\0\0\xc8\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\x1\0\0\x4\x44\0\0\x1\x1f\0\0\0\x46\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\x1I\0\0\0\xf7\0\0\0\xf2\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\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\0\0\0\0\0\0\0\x5\0\0\0\0\xcb\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\xcb\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\0\0\x5\0\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\xcb\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\0i\0\xff\xff\xff\0\0\0\0\0\0\x2\x9a\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
|
||||
MainWindow\status_bar=false
|
||||
PostProcessingDialog\cluster_angle=30
|
||||
PostProcessingDialog\cluster_radius=0.3
|
||||
PostProcessingDialog\detect_more_lc=true
|
||||
PostProcessingDialog\inter_session=true
|
||||
PostProcessingDialog\intra_session=true
|
||||
PostProcessingDialog\iterations=1
|
||||
PostProcessingDialog\reextract_features=false
|
||||
PostProcessingDialog\refine_lc=false
|
||||
PostProcessingDialog\refine_neigbors=false
|
||||
PostProcessingDialog\sba=false
|
||||
PostProcessingDialog\sba_epsilon=0
|
||||
PostProcessingDialog\sba_iterations=20
|
||||
PostProcessingDialog\sba_rematch_features=true
|
||||
PostProcessingDialog\sba_type=1
|
||||
PostProcessingDialog\sba_variance=1
|
||||
PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\x1\f\0\0\0\x1b\0\0\x4\xdb\0\0\x3\x9a\0\0\x1\f\0\0\0\x1b\0\0\x4\xdb\0\0\x3\x9a\0\0\0\0\0\0\0\0\a\x80)
|
||||
graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
|
||||
graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\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\global_path_visible=true
|
||||
graphicsView_graphView\gps_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\x80\x80\0\0)
|
||||
graphicsView_graphView\gps_graph_visible=true
|
||||
graphicsView_graphView\graph_visible=true
|
||||
graphicsView_graphView\grid_visible=true
|
||||
graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
|
||||
graphicsView_graphView\gt_graph_visible=true
|
||||
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\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
|
||||
graphicsView_graphView\link_width=0
|
||||
graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\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\local_path_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)
|
||||
graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\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\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
|
||||
graphicsView_graphView\node_radius=0.009999999776482582
|
||||
graphicsView_graphView\orientation_ENU=false
|
||||
graphicsView_graphView\origin_visible=true
|
||||
graphicsView_graphView\referential_visible=true
|
||||
graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\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)
|
||||
imageView_loopClosure\alpha=150
|
||||
imageView_loopClosure\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
|
||||
imageView_loopClosure\colormap=0
|
||||
imageView_loopClosure\depth_shown=false
|
||||
imageView_loopClosure\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
|
||||
imageView_loopClosure\features_shown=true
|
||||
imageView_loopClosure\features_size=0
|
||||
imageView_loopClosure\graphics_view=false
|
||||
imageView_loopClosure\graphics_view_scale=true
|
||||
imageView_loopClosure\graphics_view_scale_to_height=false
|
||||
imageView_loopClosure\image_shown=true
|
||||
imageView_loopClosure\lines_shown=true
|
||||
imageView_loopClosure\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
|
||||
imageView_loopClosure\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
|
||||
imageView_odometry\alpha=200
|
||||
imageView_odometry\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
|
||||
imageView_odometry\colormap=0
|
||||
imageView_odometry\depth_shown=false
|
||||
imageView_odometry\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
|
||||
imageView_odometry\features_shown=true
|
||||
imageView_odometry\features_size=0
|
||||
imageView_odometry\graphics_view=false
|
||||
imageView_odometry\graphics_view_scale=true
|
||||
imageView_odometry\graphics_view_scale_to_height=false
|
||||
imageView_odometry\image_shown=true
|
||||
imageView_odometry\lines_shown=true
|
||||
imageView_odometry\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
|
||||
imageView_odometry\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
|
||||
imageView_source\alpha=150
|
||||
imageView_source\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
|
||||
imageView_source\colormap=0
|
||||
imageView_source\depth_shown=false
|
||||
imageView_source\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
|
||||
imageView_source\features_shown=true
|
||||
imageView_source\features_size=0
|
||||
imageView_source\graphics_view=false
|
||||
imageView_source\graphics_view_scale=true
|
||||
imageView_source\graphics_view_scale_to_height=false
|
||||
imageView_source\image_shown=true
|
||||
imageView_source\lines_shown=true
|
||||
imageView_source\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
|
||||
imageView_source\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
|
||||
widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
|
||||
widget_cloudViewer\camera_axis_shown=true
|
||||
widget_cloudViewer\camera_focal="@Variant(\0\0\0T?>\"\xb0=!\xa5\0?>!\xe6)"
|
||||
widget_cloudViewer\camera_free=false
|
||||
widget_cloudViewer\camera_lockZ=true
|
||||
widget_cloudViewer\camera_ortho=false
|
||||
widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0G\xad|>\r\xa4\x10@h\xe3\x46)
|
||||
widget_cloudViewer\camera_target_follow=true
|
||||
widget_cloudViewer\camera_target_locked=false
|
||||
widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0?\x80\0\0)
|
||||
widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
|
||||
widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0)
|
||||
widget_cloudViewer\frustum_shown=true
|
||||
widget_cloudViewer\grid=false
|
||||
widget_cloudViewer\grid_cell_count=50
|
||||
widget_cloudViewer\grid_cell_size=1
|
||||
widget_cloudViewer\intensity_max=0
|
||||
widget_cloudViewer\intensity_red_colormap=false
|
||||
widget_cloudViewer\normals=false
|
||||
widget_cloudViewer\normals_scale=0.20000000298023224
|
||||
widget_cloudViewer\normals_step=1
|
||||
widget_cloudViewer\rendering_rate=5
|
||||
widget_cloudViewer\trajectory_shown=true
|
||||
widget_cloudViewer\trajectory_size=100
|
||||
+22295
File diff suppressed because it is too large
Load Diff
+15365
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,7 @@
|
||||
|
||||
**TODO: currently committed here for backup, though a comprehensive step-by-step procedure would be required so that anyone could reproduce the results**
|
||||
|
||||
This is the raw files used to generate results for MIT Stata Center dataset of this paper (for KITTI, EuRoC and TUM datasets, see this [page](https://github.com/introlab/rtabmap/blob/docker/jfr2018)):
|
||||
* M. Labbé and F. Michaud, “RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, accepted, 2018. ([pdf](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/7/7a/Labbe18JFR_preprint.pdf)) ([Wiley](https://doi.org/10.1002/rob.21831))
|
||||
|
||||
Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper.
|
||||
Executable
+128
@@ -0,0 +1,128 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (c) 2013, Juergen Sturm, TUM
|
||||
# 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 TUM 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 OWNER 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.
|
||||
#
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
"""
|
||||
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.
|
||||
|
||||
For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import os
|
||||
import numpy
|
||||
|
||||
|
||||
def read_file_list(filename):
|
||||
"""
|
||||
Reads a trajectory from a text file.
|
||||
|
||||
File format:
|
||||
The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
|
||||
and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
|
||||
|
||||
Input:
|
||||
filename -- File name
|
||||
|
||||
Output:
|
||||
dict -- dictionary of (stamp,data) tuples
|
||||
|
||||
"""
|
||||
file = open(filename)
|
||||
data = file.read()
|
||||
lines = data.replace(","," ").replace("\t"," ").split("\n")
|
||||
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
|
||||
list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
|
||||
return dict(list)
|
||||
|
||||
def associate(first_list, second_list,offset,max_difference):
|
||||
"""
|
||||
Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
|
||||
to find the closest match for every input tuple.
|
||||
|
||||
Input:
|
||||
first_list -- first dictionary of (stamp,data) tuples
|
||||
second_list -- second dictionary of (stamp,data) tuples
|
||||
offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
|
||||
max_difference -- search radius for candidate generation
|
||||
|
||||
Output:
|
||||
matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
|
||||
|
||||
"""
|
||||
first_keys = first_list.keys()
|
||||
second_keys = second_list.keys()
|
||||
potential_matches = [(abs(a - (b + offset)), a, b)
|
||||
for a in first_keys
|
||||
for b in second_keys
|
||||
if abs(a - (b + offset)) < max_difference]
|
||||
potential_matches.sort()
|
||||
matches = []
|
||||
for diff, a, b in potential_matches:
|
||||
if a in first_keys and b in second_keys:
|
||||
first_keys.remove(a)
|
||||
second_keys.remove(b)
|
||||
matches.append((a, b))
|
||||
|
||||
matches.sort()
|
||||
return matches
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# parse command line
|
||||
parser = argparse.ArgumentParser(description='''
|
||||
This script takes two data files with timestamps and associates them
|
||||
''')
|
||||
parser.add_argument('first_file', help='first text file (format: timestamp data)')
|
||||
parser.add_argument('second_file', help='second text file (format: timestamp data)')
|
||||
parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
|
||||
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
|
||||
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
|
||||
args = parser.parse_args()
|
||||
|
||||
first_list = read_file_list(args.first_file)
|
||||
second_list = read_file_list(args.second_file)
|
||||
|
||||
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
|
||||
|
||||
if args.first_only:
|
||||
for a,b in matches:
|
||||
print("%f %s"%(a," ".join(first_list[a])))
|
||||
else:
|
||||
for a,b in matches:
|
||||
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
|
||||
|
||||
|
||||
+50
@@ -0,0 +1,50 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true" />
|
||||
|
||||
<!-- copy provided pr2.lua in $(find cartographer_ros)/configuration_files to use odometry as guess -->
|
||||
<node name="cartographer_node" pkg="cartographer_ros"
|
||||
type="cartographer_node" args="
|
||||
-configuration_directory
|
||||
$(find cartographer_ros)/configuration_files
|
||||
-configuration_basename pr2.lua"
|
||||
output="screen">
|
||||
<remap from="scan" to="/base_scan_t_filtered" /> <!-- /base_scan_t /base_scan_t_filtered /camera_scan -->
|
||||
<remap from="odom" to="/odom_combined" />
|
||||
</node>
|
||||
|
||||
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
|
||||
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
|
||||
|
||||
<node name="tf_remove_frames" pkg="cartographer_ros"
|
||||
type="tf_remove_frames.py">
|
||||
<remap from="tf_out" to="/tf" />
|
||||
<rosparam param="remove_frames">
|
||||
- map
|
||||
- odom_combined
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="rviz" pkg="rviz" type="rviz" required="true"
|
||||
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
|
||||
<node name="playbag" pkg="rosbag" type="play"
|
||||
args="--clock $(arg bag_filename)">
|
||||
<remap from="tf" to="tf_in" />
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,77 @@
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
import tf
|
||||
import numpy
|
||||
import evaluate_ate
|
||||
from visualization_msgs.msg import MarkerArray
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
def callback(data):
|
||||
global slamPoses
|
||||
global gtPoses
|
||||
global stamps
|
||||
global listener
|
||||
global lastSize
|
||||
global slamPosesInd
|
||||
global rmse
|
||||
if len(data.markers) > 0 and lastSize != len(data.markers[0].points):
|
||||
t = rospy.Time(data.markers[0].header.stamp.secs, data.markers[0].header.stamp.nsecs)
|
||||
try:
|
||||
listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
|
||||
(trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
|
||||
gtPoses.append(Point(trans[0], trans[1], 0))
|
||||
stamps.append(t.to_sec())
|
||||
slamPoses.append(data.markers[0].points[len(data.markers[0].points)-1])
|
||||
slamPosesInd.append(len(data.markers[0].points)-1)
|
||||
first_xyz = numpy.empty([0,3])
|
||||
second_xyz = numpy.empty([0,3])
|
||||
for g in gtPoses:
|
||||
newrow = [g.x,g.y,g.z]
|
||||
first_xyz = numpy.vstack([first_xyz, newrow])
|
||||
for p in slamPoses:
|
||||
newrow = [p.x,p.y,p.z]
|
||||
second_xyz = numpy.vstack([second_xyz, newrow])
|
||||
first_xyz = numpy.matrix(first_xyz).transpose()
|
||||
second_xyz = numpy.matrix(second_xyz).transpose()
|
||||
rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
|
||||
rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
rmse.append(rmse_v)
|
||||
print "points= " + str(len(data.markers[0].points)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
|
||||
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
|
||||
print str(e)
|
||||
if len(data.markers) > 0 and len(slamPosesInd) > 0:
|
||||
j=0
|
||||
for i in slamPosesInd:
|
||||
slamPoses[j] = data.markers[0].points[i]
|
||||
j+=1
|
||||
if len(data.markers) > 0:
|
||||
lastSize = len(data.markers[0].points)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('sync_markers_gt', anonymous=True)
|
||||
listener = tf.TransformListener()
|
||||
fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
|
||||
baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
|
||||
rospy.Subscriber("trajectory_node_list", MarkerArray, callback, queue_size=1)
|
||||
slamPoses = []
|
||||
gtPoses = []
|
||||
stamps = []
|
||||
slamPosesInd = []
|
||||
rmse = []
|
||||
lastSize = 0
|
||||
rospy.spin()
|
||||
fileSlam = open('slam_poses.txt','w')
|
||||
fileGt = open('gt_poses.txt','w')
|
||||
fileRMSE = open('rmse.txt','w')
|
||||
print "slam= " + str(len(slamPoses))
|
||||
print "gt= " + str(len(gtPoses))
|
||||
print "stamps= " + str(len(stamps))
|
||||
for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
|
||||
fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
|
||||
fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
|
||||
fileRMSE.write('%f %f\n' % (t, r))
|
||||
fileSlam.close()
|
||||
fileGt.close()
|
||||
fileRMSE.close()
|
||||
@@ -0,0 +1,71 @@
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
import tf
|
||||
import numpy
|
||||
import evaluate_ate
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
def callback(data):
|
||||
global slamPoses
|
||||
global gtPoses
|
||||
global stamps
|
||||
global listener
|
||||
global rmse
|
||||
global lastTime
|
||||
|
||||
if rospy.get_time() - lastTime < 1:
|
||||
return
|
||||
lastTime = rospy.get_time()
|
||||
|
||||
t = rospy.Time(data.header.stamp.secs, data.header.stamp.nsecs)
|
||||
try:
|
||||
listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
|
||||
(trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
|
||||
gtPoses.append(Point(trans[0], trans[1], 0))
|
||||
stamps.append(t.to_sec())
|
||||
slamPoses.append(data.pose.pose.position)
|
||||
first_xyz = numpy.empty([0,3])
|
||||
second_xyz = numpy.empty([0,3])
|
||||
for g in gtPoses:
|
||||
newrow = [g.x,g.y,g.z]
|
||||
first_xyz = numpy.vstack([first_xyz, newrow])
|
||||
for p in slamPoses:
|
||||
newrow = [p.x,p.y,p.z]
|
||||
second_xyz = numpy.vstack([second_xyz, newrow])
|
||||
first_xyz = numpy.matrix(first_xyz).transpose()
|
||||
second_xyz = numpy.matrix(second_xyz).transpose()
|
||||
rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
|
||||
rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
rmse.append(rmse_v)
|
||||
print " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
|
||||
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
|
||||
print str(e)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('sync_odom_gt', anonymous=True)
|
||||
listener = tf.TransformListener()
|
||||
fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
|
||||
baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
|
||||
rospy.Subscriber("icp_odom", Odometry, callback, queue_size=1)
|
||||
slamPoses = []
|
||||
gtPoses = []
|
||||
stamps = []
|
||||
rmse = []
|
||||
lastTime = rospy.get_time()
|
||||
rospy.spin()
|
||||
fileSlam = open('slam_poses.txt','w')
|
||||
fileGt = open('gt_poses.txt','w')
|
||||
fileRMSE = open('rmse.txt','w')
|
||||
print "slam= " + str(len(slamPoses))
|
||||
print "gt= " + str(len(gtPoses))
|
||||
print "stamps= " + str(len(stamps))
|
||||
for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
|
||||
fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
|
||||
fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
|
||||
fileRMSE.write('%f %f\n' % (t, r))
|
||||
fileSlam.close()
|
||||
fileGt.close()
|
||||
fileRMSE.close()
|
||||
@@ -0,0 +1,54 @@
|
||||
diff --git a/ethzasl_icp_mapper/launch/2D_scans/icp.yaml b/ethzasl_icp_mapper/launch/2D_scans/icp.yaml
|
||||
index 67820e7..aec1839 100644
|
||||
--- a/ethzasl_icp_mapper/launch/2D_scans/icp.yaml
|
||||
+++ b/ethzasl_icp_mapper/launch/2D_scans/icp.yaml
|
||||
@@ -1,12 +1,12 @@
|
||||
matcher:
|
||||
KDTreeMatcher:
|
||||
- maxDist: 1.0
|
||||
+ maxDist: 1
|
||||
knn: 5
|
||||
epsilon: 3.16
|
||||
|
||||
outlierFilters:
|
||||
- TrimmedDistOutlierFilter:
|
||||
- ratio: 0.85
|
||||
+ ratio: 0.95
|
||||
- SurfaceNormalOutlierFilter:
|
||||
maxAngle: 0.42
|
||||
|
||||
@@ -25,7 +25,11 @@ transformationCheckers:
|
||||
maxTranslationNorm: 5.00
|
||||
|
||||
inspector:
|
||||
-# VTKFileInspector
|
||||
+# VTKFileInspector:
|
||||
+# baseFileName : debug--
|
||||
+# dumpDataLinks : 1
|
||||
+# dumpReading : 1
|
||||
+# dumpReference : 1
|
||||
NullInspector
|
||||
|
||||
logger:
|
||||
diff --git a/libpointmatcher_ros/src/point_cloud.cpp b/libpointmatcher_ros/src/point_cloud.cpp
|
||||
index b77651d..8eb1f6c 100644
|
||||
--- a/libpointmatcher_ros/src/point_cloud.cpp
|
||||
+++ b/libpointmatcher_ros/src/point_cloud.cpp
|
||||
@@ -449,7 +449,7 @@ namespace PointMatcher_ros
|
||||
try
|
||||
{
|
||||
listener->transformPoint(
|
||||
- fixedFrame,
|
||||
+ pin.header.frame_id,
|
||||
rosMsg.header.stamp,
|
||||
pin,
|
||||
fixedFrame,
|
||||
@@ -459,7 +459,7 @@ namespace PointMatcher_ros
|
||||
if(addObservationDirection)
|
||||
{
|
||||
listener->transformPoint(
|
||||
- fixedFrame,
|
||||
+ s_in.header.frame_id,
|
||||
curTime,
|
||||
s_in,
|
||||
fixedFrame,
|
||||
+195
@@ -0,0 +1,195 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (c) 2013, Juergen Sturm, TUM
|
||||
# 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 TUM 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 OWNER 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.
|
||||
#
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
"""
|
||||
This script computes the absolute trajectory error from the ground truth
|
||||
trajectory and the estimated trajectory.
|
||||
"""
|
||||
|
||||
import sys
|
||||
import numpy
|
||||
import argparse
|
||||
import associate
|
||||
|
||||
def align(model,data):
|
||||
"""Align two trajectories using the method of Horn (closed-form).
|
||||
|
||||
Input:
|
||||
model -- first trajectory (3xn)
|
||||
data -- second trajectory (3xn)
|
||||
|
||||
Output:
|
||||
rot -- rotation matrix (3x3)
|
||||
trans -- translation vector (3x1)
|
||||
trans_error -- translational error per point (1xn)
|
||||
|
||||
"""
|
||||
numpy.set_printoptions(precision=3,suppress=True)
|
||||
model_zerocentered = model - model.mean(1)
|
||||
data_zerocentered = data - data.mean(1)
|
||||
|
||||
W = numpy.zeros( (3,3) )
|
||||
for column in range(model.shape[1]):
|
||||
W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
|
||||
U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
|
||||
S = numpy.matrix(numpy.identity( 3 ))
|
||||
if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
|
||||
S[2,2] = -1
|
||||
rot = U*S*Vh
|
||||
trans = data.mean(1) - rot * model.mean(1)
|
||||
|
||||
model_aligned = rot * model + trans
|
||||
alignment_error = model_aligned - data
|
||||
|
||||
trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
|
||||
|
||||
return rot,trans,trans_error
|
||||
|
||||
def plot_traj(ax,stamps,traj,style,color,label):
|
||||
"""
|
||||
Plot a trajectory using matplotlib.
|
||||
|
||||
Input:
|
||||
ax -- the plot
|
||||
stamps -- time stamps (1xn)
|
||||
traj -- trajectory (3xn)
|
||||
style -- line style
|
||||
color -- line color
|
||||
label -- plot legend
|
||||
|
||||
"""
|
||||
stamps.sort()
|
||||
interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
|
||||
x = []
|
||||
y = []
|
||||
last = stamps[0]
|
||||
for i in range(len(stamps)):
|
||||
if stamps[i]-last < 2*interval:
|
||||
x.append(traj[i][0])
|
||||
y.append(traj[i][1])
|
||||
elif len(x)>0:
|
||||
ax.plot(x,y,style,color=color,label=label)
|
||||
label=""
|
||||
x=[]
|
||||
y=[]
|
||||
last= stamps[i]
|
||||
if len(x)>0:
|
||||
ax.plot(x,y,style,color=color,label=label)
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
# parse command line
|
||||
parser = argparse.ArgumentParser(description='''
|
||||
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
|
||||
''')
|
||||
parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
|
||||
parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
|
||||
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
|
||||
parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
|
||||
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
|
||||
parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
|
||||
parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
|
||||
parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
|
||||
parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
|
||||
args = parser.parse_args()
|
||||
|
||||
first_list = associate.read_file_list(args.first_file)
|
||||
second_list = associate.read_file_list(args.second_file)
|
||||
|
||||
matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))
|
||||
if len(matches)<2:
|
||||
sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
|
||||
|
||||
|
||||
first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
|
||||
second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
|
||||
rot,trans,trans_error = align(second_xyz,first_xyz)
|
||||
|
||||
second_xyz_aligned = rot * second_xyz + trans
|
||||
|
||||
first_stamps = first_list.keys()
|
||||
first_stamps.sort()
|
||||
first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
|
||||
|
||||
second_stamps = second_list.keys()
|
||||
second_stamps.sort()
|
||||
second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
|
||||
second_xyz_full_aligned = rot * second_xyz_full + trans
|
||||
|
||||
if args.verbose:
|
||||
print "compared_pose_pairs %d pairs"%(len(trans_error))
|
||||
|
||||
print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
|
||||
print "absolute_translational_error.median %f m"%numpy.median(trans_error)
|
||||
print "absolute_translational_error.std %f m"%numpy.std(trans_error)
|
||||
print "absolute_translational_error.min %f m"%numpy.min(trans_error)
|
||||
print "absolute_translational_error.max %f m"%numpy.max(trans_error)
|
||||
else:
|
||||
print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
|
||||
if args.save_associations:
|
||||
file = open(args.save_associations,"w")
|
||||
file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
|
||||
file.close()
|
||||
|
||||
if args.save:
|
||||
file = open(args.save,"w")
|
||||
file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
|
||||
file.close()
|
||||
|
||||
if args.plot:
|
||||
import matplotlib
|
||||
matplotlib.use('Agg')
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.pylab as pylab
|
||||
from matplotlib.patches import Ellipse
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111)
|
||||
plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
|
||||
plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")
|
||||
|
||||
#label="difference"
|
||||
#for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
|
||||
# ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
|
||||
# label=""
|
||||
|
||||
ax.legend()
|
||||
|
||||
ax.set_xlabel('x [m]')
|
||||
ax.set_ylabel('y [m]')
|
||||
plt.savefig(args.plot,dpi=300, format='pdf')
|
||||
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
import rosbag
|
||||
import tf
|
||||
import sys
|
||||
from tf.msg import tfMessage
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
print 'Usage: $ python extract_rgbd.py "2012-01-25-12-14-25"'
|
||||
sys.exit(0)
|
||||
|
||||
bagName = sys.argv[1]
|
||||
with rosbag.Bag(bagName + '_rgbd.bag', 'w') as outbag:
|
||||
print 'Processing ' + bagName + '.bag...'
|
||||
for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
|
||||
if topic == "/tf" or topic == "/camera/depth/image_raw" or topic == "/camera/rgb/camera_info" or topic == "/camera/rgb/image_raw":
|
||||
outbag.write(topic, msg, t)
|
||||
print 'Output: ' + bagName + '_out.bag'
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
import rosbag
|
||||
import tf
|
||||
import sys
|
||||
from tf.msg import tfMessage
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
print 'Usage: $ python extract_scans.py "2012-01-25-12-14-25"'
|
||||
sys.exit(0)
|
||||
|
||||
bagName = sys.argv[1]
|
||||
with rosbag.Bag(bagName + '_scans.bag', 'w') as outbag:
|
||||
print 'Processing ' + bagName + '.bag...'
|
||||
for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
|
||||
if topic == "/tf":
|
||||
outbag.write(topic, msg, t)
|
||||
elif topic == "/base_scan":
|
||||
outbag.write(topic, msg, t)
|
||||
elif topic == "/robot_pose_ekf/odom_combined":
|
||||
outbag.write(topic, msg, t)
|
||||
|
||||
print 'Output: ' + bagName + '_out.bag'
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
import rosbag
|
||||
import tf
|
||||
import sys
|
||||
from tf.msg import tfMessage
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
print 'Usage: $ python extract_stereo.py "2012-01-25-12-14-25"'
|
||||
sys.exit(0)
|
||||
|
||||
bagName = sys.argv[1]
|
||||
with rosbag.Bag(bagName + '_stereo.bag', 'w') as outbag:
|
||||
print 'Processing ' + bagName + '.bag...'
|
||||
leftCamInfoStatus = True
|
||||
leftImageStatus = True
|
||||
rightCamInfoStatus = True
|
||||
rightImageStatus = True
|
||||
for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
|
||||
if topic == "/tf":
|
||||
outbag.write(topic, msg, t)
|
||||
elif topic == "/wide_stereo/left/camera_info":
|
||||
if leftCamInfoStatus:
|
||||
outbag.write(topic, msg, t)
|
||||
leftCamInfoStatus = not leftCamInfoStatus
|
||||
elif topic == "/wide_stereo/right/camera_info":
|
||||
if rightCamInfoStatus:
|
||||
outbag.write(topic, msg, t)
|
||||
rightCamInfoStatus = not rightCamInfoStatus
|
||||
elif topic == "/wide_stereo/left/image_raw":
|
||||
if leftImageStatus:
|
||||
outbag.write(topic, msg, t)
|
||||
leftImageStatus = not leftImageStatus
|
||||
elif topic == "/wide_stereo/right/image_raw":
|
||||
if rightImageStatus:
|
||||
outbag.write(topic, msg, t)
|
||||
rightImageStatus = not rightImageStatus
|
||||
|
||||
print 'Output: ' + bagName + '_out.bag'
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
import rosbag
|
||||
from tf.msg import tfMessage
|
||||
with rosbag.Bag('2012-01-25-12-33-29_scans-noodom.bag', 'w') as outbag:
|
||||
for topic, msg, t in rosbag.Bag('2012-01-25-12-33-29_scans.bag').read_messages():
|
||||
if topic == "/tf" and msg.transforms:
|
||||
newList = [];
|
||||
for m in msg.transforms:
|
||||
if m.header.frame_id != "/odom_combined":
|
||||
newList.append(m)
|
||||
else:
|
||||
print 'odom frame removed!'
|
||||
if len(newList)>0:
|
||||
msg.transforms = newList
|
||||
outbag.write(topic, msg, t)
|
||||
else:
|
||||
outbag.write(topic, msg, t)
|
||||
Executable
+16
@@ -0,0 +1,16 @@
|
||||
import rosbag
|
||||
from tf.msg import tfMessage
|
||||
with rosbag.Bag('2012-01-25-12-14-25-noOdomCombined.bag', 'w') as outbag:
|
||||
for topic, msg, t in rosbag.Bag('2012-01-25-12-14-25.bag').read_messages():
|
||||
if topic == "/tf" and msg.transforms:
|
||||
newList = [];
|
||||
for m in msg.transforms:
|
||||
if m.header.frame_id != "odom_combined":
|
||||
newList.append(m)
|
||||
else:
|
||||
print 'map frame removed!'
|
||||
if len(newList)>0:
|
||||
msg.transforms = newList
|
||||
outbag.write(topic, msg, t)
|
||||
else:
|
||||
outbag.write(topic, msg, t)
|
||||
+71
@@ -0,0 +1,71 @@
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
import tf
|
||||
import numpy
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('groundtruth_tf_broadcaster')
|
||||
fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
|
||||
baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
|
||||
offset_time = rospy.get_param('~offset_time', 0.0)
|
||||
offset_x = rospy.get_param('~offset_x', 0.0)
|
||||
offset_y = rospy.get_param('~offset_y', 0.0)
|
||||
offset_theta = rospy.get_param('~offset_theta', 0.0)
|
||||
gtFile = rospy.get_param('~file', 'groundtruth.txt')
|
||||
gtFile = os.path.expanduser(gtFile)
|
||||
br = tf.TransformBroadcaster()
|
||||
|
||||
init_x = 0
|
||||
init_y = 0
|
||||
init = False
|
||||
|
||||
# assuming format "timestamp,x,y,theta"
|
||||
for line in open(gtFile,'r'):
|
||||
mylist = line.split(',')
|
||||
if len(mylist) == 4 and not rospy.is_shutdown():
|
||||
stamp = float(int(mylist[0]))/1000000.0 + offset_time
|
||||
x = float(mylist[1])
|
||||
y = float(mylist[2])
|
||||
theta = float(mylist[3])
|
||||
|
||||
if not init:
|
||||
init_x = x
|
||||
init_y = y
|
||||
init = True
|
||||
|
||||
x -= init_x
|
||||
y -= init_y
|
||||
|
||||
trans1_mat = tf.transformations.translation_matrix((x, y, 0))
|
||||
rot1_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, theta))
|
||||
mat1 = numpy.dot(trans1_mat, rot1_mat)
|
||||
|
||||
trans2_mat = tf.transformations.translation_matrix((offset_x, offset_y, 0))
|
||||
rot2_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, offset_theta))
|
||||
mat2 = numpy.dot(trans2_mat, rot2_mat)
|
||||
|
||||
mat3 = numpy.dot(mat1, mat2)
|
||||
trans3 = tf.transformations.translation_from_matrix(mat3)
|
||||
rot3 = tf.transformations.quaternion_from_matrix(mat3)
|
||||
|
||||
#print(mat1)
|
||||
#print(mat2)
|
||||
#print(mat3)
|
||||
|
||||
now = rospy.get_time()
|
||||
while not rospy.is_shutdown() and now < stamp:
|
||||
delay = stamp - now
|
||||
if delay > 0.05:
|
||||
delay = 0.05
|
||||
rospy.sleep(delay)
|
||||
now = rospy.get_time()
|
||||
if not rospy.is_shutdown():
|
||||
br.sendTransform(trans3,
|
||||
rot3,
|
||||
rospy.Time.from_sec(stamp),
|
||||
baseFrame,
|
||||
fixedFrame)
|
||||
else:
|
||||
break
|
||||
Executable
+103
@@ -0,0 +1,103 @@
|
||||
// icp_odometry with guess from odom_combined frame
|
||||
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Icp/VoxelSize 0.0 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0" rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db
|
||||
// proximity space short range
|
||||
scan_topic:=/base_scan_t_filtered
|
||||
// proximity space longer range
|
||||
scan_topic:=/base_scan_t
|
||||
// RGB-D back-end
|
||||
rgb_topic:=/camera/rgb/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/rgb/camera_info
|
||||
// Stereo back-end
|
||||
stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info right_camera_info_topic:=/wide_stereo/right/camera_info_scaled approx_rgbd_sync:=false approx_sync:=true
|
||||
// odometry
|
||||
odom_frame_id:="odom_combined" odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 icp_odometry:=false
|
||||
// disable loop closure detection
|
||||
--Kp/MaxFeatures -1
|
||||
//neighbor link refine
|
||||
--RGBD/NeighborLinkRefining true --RGBD/OptimizeMaxError 1.5
|
||||
// odom frame to frame
|
||||
--Odom/Strategy 1
|
||||
|
||||
$ rosparam load scan_filter.yaml scan_to_scan_filter_chain
|
||||
$ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
|
||||
|
||||
$ ./republish_scan.py _offset:=82.2
|
||||
|
||||
//stereo
|
||||
$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled
|
||||
$ export ROS_NAMESPACE=wide_stereo
|
||||
$ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw right/image_raw:=right/image_raw left/camera_info:=left/camera_info right/camera_info:=right/camera_info_scaled
|
||||
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
// or
|
||||
// To make ground truth matching dataset 2012-01-25-12-14-25.bag (transform is found by exporting scans according to ground truth for each sequence, then use pcl_icp)
|
||||
// xyz=0.006236,-0.351500,0.000000 rpy=0.000000,0.000000,-0.017832
|
||||
// old -0.00273873 -0.38818 0 0 0 0
|
||||
$ rosrun tf static_transform_publisher 0.006236 -0.351500 0 -0.017832 0 0 world world_offset 100
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275
|
||||
// otherwise
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
|
||||
// rtabmap
|
||||
$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
|
||||
|
||||
$ rostopic hz /rtabmap/grid_map
|
||||
|
||||
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
|
||||
// or
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag
|
||||
|
||||
//copy results (assuming we are in bags directory)
|
||||
$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap
|
||||
//or
|
||||
$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/// OTHER SCAN LIDAR SLAM
|
||||
$ roscore
|
||||
$ rosparam set use_sim_time true
|
||||
$ ./republish_scan.py _offset:=82.2
|
||||
//2012-01-25-12-14-25
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
|
||||
./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
//2012-01-25-12-33-29
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
// for short-lidar
|
||||
$ rosparam load scan_filter.yaml scan_to_scan_filter_chain
|
||||
$ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
|
||||
// for fake lidar kinect
|
||||
$ rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw camera_info:=/camera/rgb/camera_info scan:=/camera_scan _output_frame_id:=openni_rgb_frame _range_max:=6
|
||||
|
||||
// gmapping
|
||||
$ rosrun gmapping slam_gmapping scan:=base_scan_t _odom_frame:=odom_combined _particles:=100 _base_frame:=base_footprint
|
||||
$ ./sync_path_gt.py _frame_id:=scan_gt
|
||||
|
||||
// hector_slam
|
||||
$ rosrun hector_mapping hector_mapping _pub_map_odom_transform:=true _map_frame:=map _base_frame:=base_footprint _odom_frame:=odom_combined scan:=/base_scan_t _map_size:=4096
|
||||
$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=trajectory
|
||||
$ roslaunch hector_geotiff geotiff_mapper.launch trajectory_source_frame_name:=base_footprint
|
||||
|
||||
// etchzasl_icp_mapper (in Mapper::gotScan(), change odomFrame to scanMsgIn.header.frame_id... OR in libpointmatcher_ros/pointcloud.cpp, change in rosMsgToPointMatcherCloud() the target_frame of transformPoint() to point's frame_id)
|
||||
// Remove _offset_x:=-0.275 from gt_tf_broadcaster.py above
|
||||
$ rosrun ethzasl_icp_mapper mapper scan:=/base_scan_t _odom_frame:=/odom_combined _map_frame:=map _subscribe_scan:=true _subscribe_cloud:=false _minMapPointCount:=1000 _minReadingPointCount:=150 _icpConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/icp.yaml" _inputFiltersConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/input_filters.yaml" _mapPostFiltersConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/map_post_filters.yaml" _minOverlap:=0.5
|
||||
$ rosrun ethzasl_icp_mapper occupancy_grid_builder scan:=/base_scan_t
|
||||
$ ./ethzasl_icp_mapper_sync_gt.py _frame_id:=scan_gt
|
||||
|
||||
// cartographer (markers time may be slightly off as they are not updated at the sime time than scan)
|
||||
$ ./pose_to_odom.py
|
||||
$ roslaunch cartographer.launch bag_filename:=/home/mathieu/bags/2012-01-25-12-33-29.bag
|
||||
$ ./cartographer_sync_markers_gt.py _frame_id:=scan_gt
|
||||
|
||||
// karto (don't use graph as markers, but GetAllProcessedScans with GetCorrectedPose for each scan pose, then set time of marker to last scan)
|
||||
$ rosrun slam_karto slam_karto _base_frame:=base_footprint _odom_frame:=odom_combined scan:=/base_scan_t _map_update_interval:=1
|
||||
$ ./slam_karto_sync_markers_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
|
||||
|
||||
// mrpt_graphslam_2d (add scan_topic and odom_topic arguments to launch, don't play with odom_ombined tf)
|
||||
$ roslaunch mrpt_graphslam_2d graphslam.launch scan_topic:=base_scan_t odom_topic:=odom_combined base_link_frame_ID:=base_footprint odometry_frame_ID:=/odom_combined start_rviz:=true
|
||||
$ ./pose_to_odom.py
|
||||
$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/feedback/robot_trajectory
|
||||
|
||||
Executable
+31
@@ -0,0 +1,31 @@
|
||||
// rgbd_odometry
|
||||
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --RGBD/OptimizeMaxError 0.5 --FAST/Threshold 7" odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15" rgbd_sync:=true depth_scale:=1.043 frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true odom_topic:=odom rgb_topic:=/camera/rgb/image_raw_throttle depth_topic:=/camera/depth/image_raw_throttle camera_info_topic:=/camera/rgb/camera_info_throttle approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
|
||||
// use wheel odom
|
||||
odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
|
||||
// flow
|
||||
--Vis/CorType 1 --Odom/KeyFrameThr 0.6 --Vis/BundleAdjustment 0
|
||||
// robot_localization
|
||||
odom_topic:=/odometry/filtered visual_odometry:=false approx_sync:=true
|
||||
$ roslaunch sensor_fusion.launch odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" rgbd_image_topic:=/rtabmap/rgbd_image frame_id:=base_footprint imu_topic:=/torso_lift_imu/data wheelodom_topic:=/base_odometry/odom
|
||||
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
// or
|
||||
// To make ground truth matching dataset 2012-01-25-12-14-25.bag (offset found with pcl_icp2d between end of first bag and begin of second bag)
|
||||
$ rosrun tf static_transform_publisher -0.00273873 -0.38818 0 0 0 0 world world_offset 100
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275
|
||||
// otherwise
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
|
||||
$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
|
||||
|
||||
$ rostopic hz /rtabmap/grid_map
|
||||
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25_rgbd.bag
|
||||
// or
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29_rgbd.bag
|
||||
|
||||
//copy results (assuming we are in bags directory)
|
||||
$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap/rgbd
|
||||
//or
|
||||
$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap/rgbd
|
||||
|
||||
Executable
+34
@@ -0,0 +1,34 @@
|
||||
// stereo_odometry
|
||||
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --Odom/KeyFrameThr 0.3 --Odom/Strategy 0 --OdomF2M/MaxSize 2000 --RGBD/OptimizeMaxError 0.5 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" odom_args:="--Vis/CorType 0" frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info_throttle right_camera_info_topic:=/wide_stereo/right/camera_info_scaled odom_topic:=odom approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
|
||||
// with wheel odom
|
||||
odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
|
||||
// flow
|
||||
--Vis/CorType 1 --Odom/KeyFrameThr 0.6
|
||||
|
||||
$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info_throttle camera_info_out:=/wide_stereo/right/camera_info_scaled
|
||||
|
||||
$ export ROS_NAMESPACE=wide_stereo
|
||||
$ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw_throttle right/image_raw:=right/image_raw_throttle left/camera_info:=left/camera_info_throttle right/camera_info:=right/camera_info_scaled
|
||||
<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
|
||||
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
// or
|
||||
// To make ground truth matching dataset 2012-01-25-12-14-25.bag (offset found with pcl_icp2d between end of first bag and begin of second bag)
|
||||
$ rosrun tf static_transform_publisher -0.00273873 -0.38818 0 0 0 0 world world_offset 100
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275
|
||||
// otherwise
|
||||
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
|
||||
|
||||
$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
|
||||
|
||||
$ rostopic hz /rtabmap/grid_map
|
||||
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25_stereo.bag
|
||||
// or
|
||||
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29_stereo.bag
|
||||
|
||||
//copy results (assuming we are in bags directory)
|
||||
$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap/stereo
|
||||
//or
|
||||
$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap/stereo
|
||||
|
||||
+18
@@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
def callback(data):
|
||||
odom = Odometry()
|
||||
odom.header = data.header
|
||||
odom.child_frame_id = child_frame_id
|
||||
odom.pose = data.pose
|
||||
pub.publish(odom)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('pose_to_odom', anonymous=True)
|
||||
pub = rospy.Publisher('odom_combined', Odometry, queue_size=1)
|
||||
child_frame_id = rospy.get_param('~child_frame_id', "base_footprint")
|
||||
rospy.Subscriber("/robot_pose_ekf/odom_combined", PoseWithCovarianceStamped, callback)
|
||||
rospy.spin()
|
||||
@@ -0,0 +1,46 @@
|
||||
-- Copyright 2016 The Cartographer Authors
|
||||
--
|
||||
-- Licensed under the Apache License, Version 2.0 (the "License");
|
||||
-- you may not use this file except in compliance with the License.
|
||||
-- You may obtain a copy of the License at
|
||||
--
|
||||
-- http://www.apache.org/licenses/LICENSE-2.0
|
||||
--
|
||||
-- Unless required by applicable law or agreed to in writing, software
|
||||
-- distributed under the License is distributed on an "AS IS" BASIS,
|
||||
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
-- See the License for the specific language governing permissions and
|
||||
-- limitations under the License.
|
||||
|
||||
include "map_builder.lua"
|
||||
include "trajectory_builder.lua"
|
||||
|
||||
options = {
|
||||
map_builder = MAP_BUILDER,
|
||||
trajectory_builder = TRAJECTORY_BUILDER,
|
||||
map_frame = "map",
|
||||
tracking_frame = "base_footprint",
|
||||
published_frame = "base_footprint",
|
||||
odom_frame = "odom",
|
||||
provide_odom_frame = false,
|
||||
use_odometry = true,
|
||||
num_laser_scans = 1,
|
||||
num_multi_echo_laser_scans = 0,
|
||||
num_subdivisions_per_laser_scan = 1,
|
||||
num_point_clouds = 0,
|
||||
lookup_transform_timeout_sec = 0.2,
|
||||
submap_publish_period_sec = 0.3,
|
||||
pose_publish_period_sec = 5e-3,
|
||||
trajectory_publish_period_sec = 30e-3,
|
||||
}
|
||||
|
||||
MAP_BUILDER.use_trajectory_builder_2d = true
|
||||
|
||||
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
|
||||
TRAJECTORY_BUILDER_2D.use_imu_data = false
|
||||
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
|
||||
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
|
||||
|
||||
SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2
|
||||
|
||||
return options
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
from sensor_msgs.msg import CameraInfo
|
||||
|
||||
def callback(data):
|
||||
P = list(data.P);
|
||||
P[3] = P[3] * scale
|
||||
data.P = tuple(P);
|
||||
pub.publish(data)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('republish_camera_info', anonymous=True)
|
||||
pub = rospy.Publisher('camera_info_out', CameraInfo, queue_size=1)
|
||||
scale = rospy.get_param('~scale', 1.091664)
|
||||
rospy.Subscriber("camera_info_in", CameraInfo, callback)
|
||||
rospy.spin()
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
from sensor_msgs.msg import LaserScan
|
||||
|
||||
def callback(data):
|
||||
t = rospy.Time(data.header.stamp.secs, data.header.stamp.nsecs)
|
||||
t+=rospy.Duration.from_sec(offset)
|
||||
data.header.stamp = t
|
||||
pub.publish(data)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('republish_scan', anonymous=True)
|
||||
pub = rospy.Publisher('base_scan_t', LaserScan, queue_size=1)
|
||||
offset = rospy.get_param('~offset', 0.0)
|
||||
rospy.Subscriber("base_scan", LaserScan, callback)
|
||||
rospy.spin()
|
||||
+6
@@ -0,0 +1,6 @@
|
||||
scan_filter_chain:
|
||||
- name: range
|
||||
type: LaserScanRangeFilter
|
||||
params:
|
||||
lower_threshold: 0
|
||||
upper_threshold: 5.6
|
||||
@@ -0,0 +1,127 @@
|
||||
diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp
|
||||
index bd9977c..b4f9382 100644
|
||||
--- a/gmapping/src/slam_gmapping.cpp
|
||||
+++ b/gmapping/src/slam_gmapping.cpp
|
||||
@@ -114,6 +114,7 @@ Initial map dimensions and resolution:
|
||||
#include "ros/ros.h"
|
||||
#include "ros/console.h"
|
||||
#include "nav_msgs/MapMetaData.h"
|
||||
+#include <nav_msgs/Path.h>
|
||||
|
||||
#include "gmapping/sensor/sensor_range/rangesensor.h"
|
||||
#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
|
||||
@@ -258,6 +259,7 @@ void SlamGMapping::startLiveSlam()
|
||||
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
|
||||
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
|
||||
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
|
||||
+ pathPub_ = node_.advertise<nav_msgs::Path>("mapPath", 1, true);
|
||||
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
|
||||
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
|
||||
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
|
||||
@@ -273,6 +275,7 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
|
||||
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
|
||||
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
|
||||
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
|
||||
+ pathPub_ = node_.advertise<nav_msgs::Path>("map_path", 1, true);
|
||||
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
|
||||
|
||||
rosbag::Bag bag;
|
||||
@@ -410,6 +413,18 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
|
||||
return false;
|
||||
}
|
||||
|
||||
+ try
|
||||
+ {
|
||||
+ tf_.lookupTransform(laser_frame_, base_frame_, scan.header.stamp, scan_to_base_);
|
||||
+ scan_to_base_.getOrigin().m_floats[2] = 0;
|
||||
+ }
|
||||
+ catch(tf::TransformException e)
|
||||
+ {
|
||||
+ ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
|
||||
+ e.what());
|
||||
+ return false;
|
||||
+ }
|
||||
+
|
||||
// create a point 1m above the laser position and transform it into the laser-frame
|
||||
tf::Vector3 v;
|
||||
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
|
||||
@@ -617,6 +632,8 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
|
||||
ROS_DEBUG("scan processed");
|
||||
|
||||
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
|
||||
+ GMapping::GridSlamProcessor::TNode * node = gsp_->getParticles()[gsp_->getBestParticleIndex()].node;
|
||||
+
|
||||
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
|
||||
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
|
||||
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
|
||||
@@ -699,6 +716,23 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
|
||||
delta_);
|
||||
|
||||
ROS_DEBUG("Trajectory tree:");
|
||||
+ nav_msgs::Path path;
|
||||
+ int count = 0;
|
||||
+ for(GMapping::GridSlamProcessor::TNode* n = best.node;
|
||||
+ n;
|
||||
+ n = n->parent)
|
||||
+ {
|
||||
+ if(!n->reading)
|
||||
+ {
|
||||
+ ROS_DEBUG("Reading is NULL");
|
||||
+ continue;
|
||||
+ }
|
||||
+ ++count;
|
||||
+ }
|
||||
+ path.poses.resize(count);
|
||||
+ int oi = path.poses.size()-1;
|
||||
+
|
||||
+
|
||||
for(GMapping::GridSlamProcessor::TNode* n = best.node;
|
||||
n;
|
||||
n = n->parent)
|
||||
@@ -712,6 +746,13 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
|
||||
ROS_DEBUG("Reading is NULL");
|
||||
continue;
|
||||
}
|
||||
+ path.poses[oi].header.frame_id = map_frame_;
|
||||
+ path.poses[oi].header.stamp = ros::Time(n->reading->getTime());
|
||||
+
|
||||
+ tf::Transform tmp = tf::Transform(tf::createQuaternionFromRPY(0, 0, n->pose.theta), tf::Vector3(n->pose.x, n->pose.y, 0))*scan_to_base_;
|
||||
+ tf::poseTFToMsg(tmp, path.poses[oi].pose);
|
||||
+ --oi;
|
||||
+
|
||||
matcher.invalidateActiveArea();
|
||||
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
|
||||
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
|
||||
@@ -764,8 +805,11 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
|
||||
map_.map.header.stamp = ros::Time::now();
|
||||
map_.map.header.frame_id = tf_.resolve( map_frame_ );
|
||||
|
||||
+ path.header = map_.map.header;
|
||||
+
|
||||
sst_.publish(map_.map);
|
||||
sstm_.publish(map_.map.info);
|
||||
+ pathPub_.publish(path);
|
||||
}
|
||||
|
||||
bool
|
||||
diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h
|
||||
index ae622b9..8d84645 100644
|
||||
--- a/gmapping/src/slam_gmapping.h
|
||||
+++ b/gmapping/src/slam_gmapping.h
|
||||
@@ -53,6 +53,7 @@ class SlamGMapping
|
||||
ros::Publisher entropy_publisher_;
|
||||
ros::Publisher sst_;
|
||||
ros::Publisher sstm_;
|
||||
+ ros::Publisher pathPub_;
|
||||
ros::ServiceServer ss_;
|
||||
tf::TransformListener tf_;
|
||||
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
|
||||
@@ -92,6 +93,8 @@ class SlamGMapping
|
||||
std::string map_frame_;
|
||||
std::string odom_frame_;
|
||||
|
||||
+ tf::StampedTransform scan_to_base_;
|
||||
+
|
||||
void updateMap(const sensor_msgs::LaserScan& scan);
|
||||
bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
|
||||
bool initMapper(const sensor_msgs::LaserScan& scan);
|
||||
@@ -0,0 +1,118 @@
|
||||
diff --git a/src/slam_karto.cpp b/src/slam_karto.cpp
|
||||
index 712a9ca..0c0d885 100644
|
||||
--- a/src/slam_karto.cpp
|
||||
+++ b/src/slam_karto.cpp
|
||||
@@ -68,7 +68,7 @@ class SlamKarto
|
||||
bool updateMap();
|
||||
void publishTransform();
|
||||
void publishLoop(double transform_publish_period);
|
||||
- void publishGraphVisualization();
|
||||
+ void publishGraphVisualization(const ros::Time & stamp);
|
||||
|
||||
// ROS handles
|
||||
ros::NodeHandle node_;
|
||||
@@ -435,16 +435,22 @@ SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
|
||||
}
|
||||
|
||||
void
|
||||
-SlamKarto::publishGraphVisualization()
|
||||
+SlamKarto::publishGraphVisualization(const ros::Time & stamp)
|
||||
{
|
||||
std::vector<float> graph;
|
||||
solver_->getGraph(graph);
|
||||
|
||||
+ std::vector<karto::LocalizedRangeScan*> scans = mapper_->GetAllProcessedScans();
|
||||
+
|
||||
+ if(scans.empty())
|
||||
+ {
|
||||
+ return;
|
||||
+ }
|
||||
visualization_msgs::MarkerArray marray;
|
||||
|
||||
visualization_msgs::Marker m;
|
||||
m.header.frame_id = "map";
|
||||
- m.header.stamp = ros::Time::now();
|
||||
+ m.header.stamp = stamp;
|
||||
m.id = 0;
|
||||
m.ns = "karto";
|
||||
m.type = visualization_msgs::Marker::SPHERE;
|
||||
@@ -462,7 +468,7 @@ SlamKarto::publishGraphVisualization()
|
||||
|
||||
visualization_msgs::Marker edge;
|
||||
edge.header.frame_id = "map";
|
||||
- edge.header.stamp = ros::Time::now();
|
||||
+ edge.header.stamp = ros::Time(scans.back()->GetTime());
|
||||
edge.action = visualization_msgs::Marker::ADD;
|
||||
edge.ns = "karto";
|
||||
edge.id = 0;
|
||||
@@ -477,14 +483,14 @@ SlamKarto::publishGraphVisualization()
|
||||
|
||||
m.action = visualization_msgs::Marker::ADD;
|
||||
uint id = 0;
|
||||
- for (uint i=0; i<graph.size()/2; i++)
|
||||
+ for (uint i=0; i<scans.size(); i++)
|
||||
{
|
||||
m.id = id;
|
||||
- m.pose.position.x = graph[2*i];
|
||||
- m.pose.position.y = graph[2*i+1];
|
||||
+ m.pose.position.x = scans[i]->GetCorrectedPose().GetX();
|
||||
+ m.pose.position.y = scans[i]->GetCorrectedPose().GetY();
|
||||
marray.markers.push_back(visualization_msgs::Marker(m));
|
||||
id++;
|
||||
-
|
||||
+/*
|
||||
if(i>0)
|
||||
{
|
||||
edge.points.clear();
|
||||
@@ -500,15 +506,15 @@ SlamKarto::publishGraphVisualization()
|
||||
|
||||
marray.markers.push_back(visualization_msgs::Marker(edge));
|
||||
id++;
|
||||
- }
|
||||
+ }*/
|
||||
}
|
||||
-
|
||||
+/*
|
||||
m.action = visualization_msgs::Marker::DELETE;
|
||||
for (; id < marker_count_; id++)
|
||||
{
|
||||
m.id = id;
|
||||
marray.markers.push_back(visualization_msgs::Marker(m));
|
||||
- }
|
||||
+ }*/
|
||||
|
||||
marker_count_ = marray.markers.size();
|
||||
|
||||
@@ -537,12 +543,14 @@ SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
|
||||
karto::Pose2 odom_pose;
|
||||
if(addScan(laser, scan, odom_pose))
|
||||
{
|
||||
- ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
|
||||
+ ROS_INFO("added scan at pose: %.3f %.3f %.3f",
|
||||
odom_pose.GetX(),
|
||||
odom_pose.GetY(),
|
||||
odom_pose.GetHeading());
|
||||
|
||||
- publishGraphVisualization();
|
||||
+ publishGraphVisualization(scan->header.stamp);
|
||||
+
|
||||
+ ROS_INFO("published markers");
|
||||
|
||||
if(!got_map_ ||
|
||||
(scan->header.stamp - last_map_update) > map_update_interval_)
|
||||
diff --git a/src/spa_solver.cpp b/src/spa_solver.cpp
|
||||
index 5d9a962..6a65211 100644
|
||||
--- a/src/spa_solver.cpp
|
||||
+++ b/src/spa_solver.cpp
|
||||
@@ -46,9 +46,9 @@ void SpaSolver::Compute()
|
||||
|
||||
typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
|
||||
|
||||
- ROS_INFO("Calling doSPA for loop closure");
|
||||
+ //ROS_INFO("Calling doSPA for loop closure");
|
||||
m_Spa.doSPA(40);
|
||||
- ROS_INFO("Finished doSPA for loop closure");
|
||||
+ //ROS_INFO("Finished doSPA for loop closure");
|
||||
NodeVector nodes = m_Spa.getNodes();
|
||||
forEach(NodeVector, &nodes)
|
||||
{
|
||||
@@ -0,0 +1,83 @@
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
import tf
|
||||
import numpy
|
||||
import evaluate_ate
|
||||
from visualization_msgs.msg import MarkerArray
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
def callback(data):
|
||||
global slamPoses
|
||||
global gtPoses
|
||||
global stamps
|
||||
global listener
|
||||
global lastSize
|
||||
global slamPosesInd
|
||||
global rmse
|
||||
|
||||
point_markers = []
|
||||
for m in data.markers:
|
||||
if m.type==2:
|
||||
point_markers.append(m)
|
||||
|
||||
if len(point_markers) > 0 and lastSize != len(point_markers):
|
||||
t = rospy.Time(point_markers[0].header.stamp.secs, point_markers[0].header.stamp.nsecs)
|
||||
try:
|
||||
listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
|
||||
(trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
|
||||
gtPoses.append(Point(trans[0], trans[1], 0))
|
||||
stamps.append(t.to_sec())
|
||||
slamPoses.append(point_markers[len(point_markers)-1].pose.position)
|
||||
slamPosesInd.append(len(point_markers)-1)
|
||||
first_xyz = numpy.empty([0,3])
|
||||
second_xyz = numpy.empty([0,3])
|
||||
for g in gtPoses:
|
||||
newrow = [g.x,g.y,g.z]
|
||||
first_xyz = numpy.vstack([first_xyz, newrow])
|
||||
for p in slamPoses:
|
||||
newrow = [p.x,p.y,p.z]
|
||||
second_xyz = numpy.vstack([second_xyz, newrow])
|
||||
first_xyz = numpy.matrix(first_xyz).transpose()
|
||||
second_xyz = numpy.matrix(second_xyz).transpose()
|
||||
rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
|
||||
rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
rmse.append(rmse_v)
|
||||
print "points= " + str(len(point_markers)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
|
||||
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
|
||||
print str(e)
|
||||
if len(data.markers) > 0 and len(slamPosesInd) > 0:
|
||||
j=0
|
||||
for i in slamPosesInd:
|
||||
slamPoses[j] = point_markers[i].pose.position
|
||||
j+=1
|
||||
if len(data.markers) > 0:
|
||||
lastSize = len(point_markers)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('sync_markers_gt', anonymous=True)
|
||||
listener = tf.TransformListener()
|
||||
fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
|
||||
baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
|
||||
rospy.Subscriber("visualization_marker_array", MarkerArray, callback, queue_size=1)
|
||||
slamPoses = []
|
||||
gtPoses = []
|
||||
stamps = []
|
||||
slamPosesInd = []
|
||||
rmse = []
|
||||
lastSize = 0
|
||||
rospy.spin()
|
||||
fileSlam = open('slam_poses.txt','w')
|
||||
fileGt = open('gt_poses.txt','w')
|
||||
fileRMSE = open('rmse.txt','w')
|
||||
print "slam= " + str(len(slamPoses))
|
||||
print "gt= " + str(len(gtPoses))
|
||||
print "stamps= " + str(len(stamps))
|
||||
for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
|
||||
fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
|
||||
fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
|
||||
fileRMSE.write('%f %f\n' % (t, r))
|
||||
fileSlam.close()
|
||||
fileGt.close()
|
||||
fileRMSE.close()
|
||||
+77
@@ -0,0 +1,77 @@
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
import tf
|
||||
import numpy
|
||||
import evaluate_ate
|
||||
from nav_msgs.msg import Path
|
||||
from geometry_msgs.msg import Pose
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
def callback(data):
|
||||
global slamPoses
|
||||
global gtPoses
|
||||
global stamps
|
||||
global listener
|
||||
global lastSize
|
||||
global slamPosesInd
|
||||
global rmse
|
||||
if lastSize != len(data.poses):
|
||||
t = rospy.Time(data.poses[len(data.poses)-1].header.stamp.secs, data.poses[len(data.poses)-1].header.stamp.nsecs)
|
||||
try:
|
||||
listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
|
||||
(trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
|
||||
gtPoses.append(Pose(trans, rot))
|
||||
stamps.append(t.to_sec())
|
||||
slamPoses.append(data.poses[len(data.poses)-1].pose)
|
||||
slamPosesInd.append(len(data.poses)-1)
|
||||
first_xyz = numpy.empty([0,3])
|
||||
second_xyz = numpy.empty([0,3])
|
||||
for g in gtPoses:
|
||||
newrow = [g.position[0],g.position[1],g.position[2]]
|
||||
first_xyz = numpy.vstack([first_xyz, newrow])
|
||||
for p in slamPoses:
|
||||
newrow = [p.position.x,p.position.y,p.position.z]
|
||||
second_xyz = numpy.vstack([second_xyz, newrow])
|
||||
first_xyz = numpy.matrix(first_xyz).transpose()
|
||||
second_xyz = numpy.matrix(second_xyz).transpose()
|
||||
rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
|
||||
rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
rmse.append(rmse_v)
|
||||
print "points= " + str(len(data.poses)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
|
||||
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
|
||||
print str(e)
|
||||
if len(data.poses) > 0 and len(slamPosesInd) > 0:
|
||||
j=0
|
||||
for i in slamPosesInd:
|
||||
slamPoses[j] = data.poses[i].pose
|
||||
j+=1
|
||||
lastSize = len(data.poses)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('sync_path_gt', anonymous=True)
|
||||
listener = tf.TransformListener()
|
||||
fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
|
||||
baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
|
||||
rospy.Subscriber("mapPath", Path, callback, queue_size=1)
|
||||
slamPoses = []
|
||||
gtPoses = []
|
||||
stamps = []
|
||||
slamPosesInd = []
|
||||
rmse = []
|
||||
lastSize = 0
|
||||
rospy.spin()
|
||||
fileSlam = open('slam_poses.txt','w')
|
||||
fileGt = open('gt_poses.txt','w')
|
||||
fileRMSE = open('rmse.txt','w')
|
||||
print "slam= " + str(len(slamPoses))
|
||||
print "gt= " + str(len(gtPoses))
|
||||
print "stamps= " + str(len(stamps))
|
||||
for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
|
||||
fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.position.x, c.position.y))
|
||||
fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.position[0], g.position[1]))
|
||||
fileRMSE.write('%f %f\n' % (t, r))
|
||||
fileSlam.close()
|
||||
fileGt.close()
|
||||
fileRMSE.close()
|
||||
+30
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<param name="use_sim_time" value="true"/>
|
||||
<group ns="/wide_stereo" >
|
||||
<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="15"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle">
|
||||
<param name="rate" type="double" value="15.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_raw"/>
|
||||
<remap from="depth/image_in" to="depth/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="rgb/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="rgb/image_raw_throttle"/>
|
||||
<remap from="depth/image_out" to="depth/image_raw_throttle"/>
|
||||
<remap from="rgb/camera_info_out" to="rgb/camera_info_throttle"/>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
@@ -0,0 +1,78 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- Backward compatibility launch file, use rtabmap.launch instead -->
|
||||
|
||||
<!-- 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 -->
|
||||
|
||||
<!-- Choose visualization -->
|
||||
<arg name="rviz" default="false" />
|
||||
<arg name="rtabmapviz" default="true" />
|
||||
|
||||
<!-- Localization-only mode -->
|
||||
<arg name="localization" default="false"/>
|
||||
|
||||
<!-- Corresponding config files -->
|
||||
<arg name="rtabmapviz_cfg" default="~/.ros/rtabmap_gui.ini" />
|
||||
<arg name="rviz_cfg" default="$(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="database_path" default="~/.ros/rtabmap.db"/>
|
||||
<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 -->
|
||||
<arg name="approx_sync" default="true"/> <!-- if timestamps of the input topics are not synchronized -->
|
||||
|
||||
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
|
||||
<arg name="depth_registered_topic" default="/camera/depth_registered/image_raw" />
|
||||
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
|
||||
<arg name="compressed" default="false"/>
|
||||
|
||||
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
|
||||
<arg name="scan_topic" default="/scan"/>
|
||||
|
||||
<arg name="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set -->
|
||||
<arg name="scan_cloud_topic" default="/scan_cloud"/>
|
||||
|
||||
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
|
||||
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
|
||||
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
|
||||
|
||||
<arg name="namespace" default="rtabmap"/>
|
||||
<arg name="wait_for_transform" default="0.2"/>
|
||||
|
||||
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
|
||||
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="localization" value="$(arg localization)"/>
|
||||
<arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" />
|
||||
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
|
||||
|
||||
<arg name="frame_id" value="$(arg frame_id)"/>
|
||||
<arg name="namespace" value="$(arg namespace)"/>
|
||||
<arg name="database_path" value="$(arg database_path)"/>
|
||||
<arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
|
||||
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
|
||||
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
|
||||
<arg name="approx_sync" value="$(arg approx_sync)"/>
|
||||
|
||||
<arg name="rgb_topic" value="$(arg rgb_topic)" />
|
||||
<arg name="depth_topic" value="$(arg depth_registered_topic)" />
|
||||
<arg name="camera_info_topic" value="$(arg camera_info_topic)" />
|
||||
<arg name="compressed" value="$(arg compressed)"/>
|
||||
|
||||
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
|
||||
<arg name="scan_topic" value="$(arg scan_topic)"/>
|
||||
|
||||
<arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
|
||||
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
|
||||
|
||||
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
|
||||
<arg name="odom_topic" value="$(arg odom_topic)"/>
|
||||
<arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
|
||||
<arg name="odom_args" value="$(arg rtabmap_args)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,109 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Kinect 2
|
||||
Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2
|
||||
Make sure it is calibrated!
|
||||
Run:
|
||||
$ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
|
||||
$ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch
|
||||
-->
|
||||
|
||||
<!-- Which image resolution to process in rtabmap: sd, qhd, hd -->
|
||||
<arg name="resolution" default="qhd" />
|
||||
|
||||
<!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
|
||||
<arg name="frame_id" default="kinect2_base_link"/>
|
||||
|
||||
<!-- Rotate the camera -->
|
||||
<arg name="pi/2" value="1.5707963267948966"/>
|
||||
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
|
||||
args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" />
|
||||
|
||||
<!-- Choose visualization -->
|
||||
<arg name="rviz" default="false" />
|
||||
<arg name="rtabmapviz" default="true" />
|
||||
|
||||
<!-- Corresponding config files -->
|
||||
<arg name="rtabmapviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
|
||||
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
|
||||
|
||||
<!-- slightly increase default parameters for larger images (qhd=720p) -->
|
||||
<arg name="gftt_block_size" default="5" />
|
||||
<arg name="gftt_min_distance" default="5" />
|
||||
|
||||
<group ns="rtabmap">
|
||||
|
||||
<!-- Odometry -->
|
||||
<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
|
||||
<remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
|
||||
<remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
|
||||
<remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
|
||||
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
|
||||
<param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/>
|
||||
<param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
|
||||
</node>
|
||||
|
||||
<!-- Visual SLAM (robot side) -->
|
||||
<!-- args: "delete_db_on_start" and "udebug" -->
|
||||
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
|
||||
<param name="subscribe_depth" type="bool" value="true"/>
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
|
||||
<remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
|
||||
<remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
|
||||
<remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
|
||||
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
|
||||
<param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/>
|
||||
<param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
|
||||
</node>
|
||||
|
||||
<!-- Visualisation RTAB-Map -->
|
||||
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen">
|
||||
<param name="subscribe_depth" type="bool" value="true"/>
|
||||
<param name="subscribe_odom_info" type="bool" value="true"/>
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
|
||||
<remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
|
||||
<remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
|
||||
<remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
|
||||
</node>
|
||||
|
||||
</group>
|
||||
|
||||
<!-- Visualization RVIZ -->
|
||||
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
|
||||
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
|
||||
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
|
||||
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
|
||||
<remap from="rgb/image_in" to="/kinect2/$(arg resolution)/image_color_rect"/>
|
||||
<remap from="depth/image_in" to="/kinect2/$(arg resolution)/image_depth_rect"/>
|
||||
<remap from="rgb/camera_info_in" to="/kinect2/$(arg resolution)/camera_info"/>
|
||||
|
||||
<remap from="odom_in" to="rtabmap/odom"/>
|
||||
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_odom_sync/image"/>
|
||||
<remap from="depth/image_out" to="data_odom_sync/depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
|
||||
<remap from="odom_out" to="odom_sync"/>
|
||||
</node>
|
||||
<node if="$(arg rviz)" 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="voxel_size" type="double" value="0.01"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,84 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->
|
||||
|
||||
<!-- 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 visualization -->
|
||||
<arg name="rtabmapviz" default="true" />
|
||||
<arg name="rviz" default="false" />
|
||||
|
||||
<!-- Localization-only mode -->
|
||||
<arg name="localization" default="false"/>
|
||||
|
||||
<!-- Corresponding config files -->
|
||||
<arg name="rtabmapviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
|
||||
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
|
||||
|
||||
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
|
||||
<arg name="database_path" default="~/.ros/rtabmap.db"/>
|
||||
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
|
||||
<arg name="launch_prefix" default=""/>
|
||||
<arg name="approx_sync" default="false"/> <!-- if timestamps of the input topics are not synchronized -->
|
||||
|
||||
<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="compressed" default="false"/>
|
||||
|
||||
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
|
||||
<arg name="scan_topic" default="/scan"/>
|
||||
|
||||
<arg name="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set -->
|
||||
<arg name="scan_cloud_topic" default="/scan_cloud"/>
|
||||
|
||||
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
|
||||
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
|
||||
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
|
||||
|
||||
<arg name="namespace" default="rtabmap"/>
|
||||
<arg name="wait_for_transform" default="0.2"/>
|
||||
|
||||
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
|
||||
<arg name="stereo" value="true"/>
|
||||
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="localization" value="$(arg localization)"/>
|
||||
<arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" />
|
||||
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
|
||||
|
||||
<arg name="frame_id" value="$(arg frame_id)"/>
|
||||
<arg name="namespace" value="$(arg namespace)"/>
|
||||
<arg name="database_path" value="$(arg database_path)"/>
|
||||
<arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
|
||||
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
|
||||
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
|
||||
<arg name="approx_sync" value="$(arg approx_sync)"/>
|
||||
|
||||
<arg name="stereo_namespace" value="$(arg stereo_namespace)"/>
|
||||
<arg name="left_image_topic" value="$(arg left_image_topic)" />
|
||||
<arg name="right_image_topic" value="$(arg right_image_topic)" />
|
||||
<arg name="left_camera_info_topic" value="$(arg left_camera_info_topic)" />
|
||||
<arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
|
||||
|
||||
<arg name="compressed" value="$(arg compressed)"/>
|
||||
|
||||
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
|
||||
<arg name="scan_topic" value="$(arg scan_topic)"/>
|
||||
|
||||
<arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
|
||||
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
|
||||
|
||||
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
|
||||
<arg name="odom_topic" value="$(arg odom_topic)"/>
|
||||
<arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
|
||||
<arg name="odom_args" value="$(arg rtabmap_args)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,42 @@
|
||||
<library path="lib/librtabmap_legacy_plugins">
|
||||
<class name="rtabmap_legacy/data_throttle"
|
||||
type="rtabmap_legacy::DataThrottleNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class name="rtabmap_legacy/stereo_throttle"
|
||||
type="rtabmap_legacy::StereoThrottleNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class name="rtabmap_legacy/data_odom_sync"
|
||||
type="rtabmap_legacy::DataOdomSyncNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class name="rtabmap_legacy/obstacles_detection_old"
|
||||
type="rtabmap_legacy::ObstaclesDetectionOld"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class name="rtabmap_legacy/undistort_depth"
|
||||
type="rtabmap_legacy::UndistortDepth"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>rtabmap_legacy</name>
|
||||
<version>0.1.0</version>
|
||||
<description>RTAB-Map's legacy launch files.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
<license>BSD</license>
|
||||
<url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
|
||||
<url type="repository">https://github.com/introlab/rtabmap_ros</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>rtabmap_conversions</depend>
|
||||
<depend>rtabmap_msgs</depend>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,293 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <std_srvs/Empty.h>
|
||||
|
||||
#include <rtabmap/core/CameraRGB.h>
|
||||
#include <rtabmap/core/CameraThread.h>
|
||||
#include <rtabmap/core/CameraEvent.h>
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UEventsHandler.h>
|
||||
#include <rtabmap/utilite/UEventsManager.h>
|
||||
#include <rtabmap/utilite/UDirectory.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rtabmap_legacy/CameraConfig.h>
|
||||
|
||||
class CameraWrapper : public UEventsHandler
|
||||
{
|
||||
public:
|
||||
// Usb device like a Webcam
|
||||
CameraWrapper(int usbDevice = 0,
|
||||
float imageRate = 0,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0) :
|
||||
cameraThread_(0),
|
||||
camera_(0),
|
||||
frameId_("camera")
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
pnh.param("frame_id", frameId_, frameId_);
|
||||
image_transport::ImageTransport it(nh);
|
||||
rosPublisher_ = it.advertise("image", 1);
|
||||
startSrv_ = nh.advertiseService("start_camera", &CameraWrapper::startSrv, this);
|
||||
stopSrv_ = nh.advertiseService("stop_camera", &CameraWrapper::stopSrv, this);
|
||||
UEventsManager::addHandler(this);
|
||||
}
|
||||
|
||||
virtual ~CameraWrapper()
|
||||
{
|
||||
if(cameraThread_)
|
||||
{
|
||||
cameraThread_->join(true);
|
||||
delete cameraThread_;
|
||||
}
|
||||
}
|
||||
|
||||
bool init()
|
||||
{
|
||||
if(cameraThread_)
|
||||
{
|
||||
return cameraThread_->camera()->init();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void start()
|
||||
{
|
||||
if(cameraThread_)
|
||||
{
|
||||
cameraThread_->start();
|
||||
}
|
||||
}
|
||||
|
||||
bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("Camera started...");
|
||||
if(cameraThread_)
|
||||
{
|
||||
cameraThread_->start();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("Camera stopped...");
|
||||
if(cameraThread_)
|
||||
{
|
||||
cameraThread_->kill();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void setParameters(int deviceId, double frameRate, const std::string & path, bool pause)
|
||||
{
|
||||
ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f pause=%s",
|
||||
deviceId, path.c_str(), frameRate, pause?"true":"false");
|
||||
if(cameraThread_)
|
||||
{
|
||||
rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_);
|
||||
rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(camera_);
|
||||
|
||||
if(imagesCam)
|
||||
{
|
||||
// images
|
||||
if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
|
||||
{
|
||||
imagesCam->setImageRate(frameRate);
|
||||
if(pause && !cameraThread_->isPaused())
|
||||
{
|
||||
cameraThread_->join(true);
|
||||
}
|
||||
else if(!pause && cameraThread_->isPaused())
|
||||
{
|
||||
cameraThread_->start();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
delete cameraThread_;
|
||||
cameraThread_ = 0;
|
||||
}
|
||||
}
|
||||
else if(videoCam)
|
||||
{
|
||||
if(!path.empty() && path.compare(videoCam->getFilePath()) == 0)
|
||||
{
|
||||
// video
|
||||
videoCam->setImageRate(frameRate);
|
||||
if(pause && !cameraThread_->isPaused())
|
||||
{
|
||||
cameraThread_->join(true);
|
||||
}
|
||||
else if(!pause && cameraThread_->isPaused())
|
||||
{
|
||||
cameraThread_->start();
|
||||
}
|
||||
}
|
||||
else if(path.empty() &&
|
||||
videoCam->getFilePath().empty() &&
|
||||
videoCam->getUsbDevice() == deviceId)
|
||||
{
|
||||
// usb device
|
||||
videoCam->setImageRate(frameRate);
|
||||
if(pause && !cameraThread_->isPaused())
|
||||
{
|
||||
cameraThread_->join(true);
|
||||
}
|
||||
else if(!pause && cameraThread_->isPaused())
|
||||
{
|
||||
cameraThread_->start();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
delete cameraThread_;
|
||||
cameraThread_ = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Wrong camera type ?!?");
|
||||
delete cameraThread_;
|
||||
cameraThread_ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if(!cameraThread_)
|
||||
{
|
||||
if(!path.empty() && UDirectory::exists(path))
|
||||
{
|
||||
//images
|
||||
camera_ = new rtabmap::CameraImages(path, frameRate);
|
||||
}
|
||||
else if(!path.empty() && UFile::exists(path))
|
||||
{
|
||||
//video
|
||||
camera_ = new rtabmap::CameraVideo(path, false, frameRate);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path))
|
||||
{
|
||||
ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
|
||||
}
|
||||
//usb device
|
||||
camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate);
|
||||
}
|
||||
cameraThread_ = new rtabmap::CameraThread(camera_);
|
||||
init();
|
||||
if(!pause)
|
||||
{
|
||||
start();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual bool handleEvent(UEvent * event)
|
||||
{
|
||||
if(event->getClassName().compare("CameraEvent") == 0)
|
||||
{
|
||||
rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
|
||||
const cv::Mat & image = e->data().imageRaw();
|
||||
if(!image.empty() && image.depth() == CV_8U)
|
||||
{
|
||||
cv_bridge::CvImage img;
|
||||
if(image.channels() == 1)
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::MONO8;
|
||||
}
|
||||
else
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||
}
|
||||
img.image = image;
|
||||
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
|
||||
rosMsg->header.frame_id = frameId_;
|
||||
rosMsg->header.stamp = ros::Time::now();
|
||||
rosPublisher_.publish(rosMsg);
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private:
|
||||
image_transport::Publisher rosPublisher_;
|
||||
rtabmap::CameraThread * cameraThread_;
|
||||
rtabmap::Camera * camera_;
|
||||
ros::ServiceServer startSrv_;
|
||||
ros::ServiceServer stopSrv_;
|
||||
std::string frameId_;
|
||||
};
|
||||
|
||||
CameraWrapper * camera = 0;
|
||||
void callback(rtabmap_legacy::CameraConfig &config, uint32_t level)
|
||||
{
|
||||
if(camera)
|
||||
{
|
||||
camera->setParameters(config.device_id, config.frame_rate, config.video_or_images_path, config.pause);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
//ULogger::setLevel(ULogger::kDebug);
|
||||
ULogger::setEventLevel(ULogger::kWarning);
|
||||
|
||||
ros::init(argc, argv, "camera");
|
||||
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
camera = new CameraWrapper(); // webcam device 0
|
||||
|
||||
dynamic_reconfigure::Server<rtabmap_legacy::CameraConfig> server;
|
||||
dynamic_reconfigure::Server<rtabmap_legacy::CameraConfig>::CallbackType f;
|
||||
f = boost::bind(&callback, boost::placeholders::_1, boost::placeholders::_2);
|
||||
server.setCallback(f);
|
||||
|
||||
ros::spin();
|
||||
|
||||
//cleanup
|
||||
if(camera)
|
||||
{
|
||||
delete camera;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,113 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UDirectory.h>
|
||||
#include <rtabmap/core/CameraStereo.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
|
||||
ros::init(argc, argv, "uvc_stereo_camera");
|
||||
|
||||
ros::NodeHandle pnh("~");
|
||||
double rate = 0.0;
|
||||
std::string id = "camera";
|
||||
std::string frameId = "camera_link";
|
||||
double scale = 1.0;
|
||||
pnh.param("rate", rate, rate);
|
||||
pnh.param("camera_id", id, id);
|
||||
pnh.param("frame_id", frameId, frameId);
|
||||
pnh.param("scale", scale, scale);
|
||||
|
||||
rtabmap::CameraStereoVideo camera(0, false, rate);
|
||||
|
||||
if(camera.init(UDirectory::homeDir() + "/.ros/camera_info", id))
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle left_nh(nh, "left");
|
||||
ros::NodeHandle right_nh(nh, "right");
|
||||
image_transport::ImageTransport left_it(left_nh);
|
||||
image_transport::ImageTransport right_it(right_nh);
|
||||
|
||||
image_transport::Publisher imageLeftPub = left_it.advertise(left_nh.resolveName("image_raw"), 1);
|
||||
image_transport::Publisher imageRightPub = right_it.advertise(right_nh.resolveName("image_raw"), 1);
|
||||
ros::Publisher infoLeftPub = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info"), 1);
|
||||
ros::Publisher infoRightPub = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info"), 1);
|
||||
|
||||
while(ros::ok())
|
||||
{
|
||||
rtabmap::SensorData data = camera.takeImage();
|
||||
|
||||
ros::Time currentTime = ros::Time::now();
|
||||
|
||||
cv_bridge::CvImage imageLeft;
|
||||
imageLeft.header.frame_id = frameId;
|
||||
imageLeft.header.stamp = currentTime;
|
||||
imageLeft.encoding = "bgr8";
|
||||
cv::resize(data.imageRaw(), imageLeft.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
|
||||
imageLeftPub.publish(imageLeft.toImageMsg());
|
||||
|
||||
cv_bridge::CvImage imageRight;
|
||||
imageRight.header = imageLeft.header;
|
||||
imageRight.encoding = "mono8";
|
||||
cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
|
||||
imageRightPub.publish(imageRight.toImageMsg());
|
||||
|
||||
if(data.stereoCameraModels().size())
|
||||
{
|
||||
sensor_msgs::CameraInfo infoLeft, infoRight;
|
||||
rtabmap_conversions::cameraModelToROS(data.stereoCameraModels()[0].left().scaled(scale), infoLeft);
|
||||
rtabmap_conversions::cameraModelToROS(data.stereoCameraModels()[0].right().scaled(scale), infoRight);
|
||||
infoLeft.header = imageLeft.header;
|
||||
infoRight.header = imageLeft.header;
|
||||
infoLeftPub.publish(infoLeft);
|
||||
infoRightPub.publish(infoRight);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("No calibration loaded!");
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Could not initialize the camera!");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 "pluginlib/class_list_macros.hpp"
|
||||
#include "nodelet/nodelet.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
namespace rtabmap_legacy
|
||||
{
|
||||
|
||||
class DataOdomSyncNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
//Constructor
|
||||
DataOdomSyncNodelet():
|
||||
sync_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~DataOdomSyncNodelet()
|
||||
{
|
||||
delete sync_;
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
ros::NodeHandle& private_nh = getPrivateNodeHandle();
|
||||
|
||||
ros::NodeHandle rgb_nh(nh, "rgb");
|
||||
ros::NodeHandle depth_nh(nh, "depth");
|
||||
ros::NodeHandle rgb_pnh(private_nh, "rgb");
|
||||
ros::NodeHandle depth_pnh(private_nh, "depth");
|
||||
image_transport::ImageTransport rgb_it(rgb_nh);
|
||||
image_transport::ImageTransport depth_it(depth_nh);
|
||||
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
|
||||
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
|
||||
|
||||
int queueSize = 10;
|
||||
private_nh.param("queue_size", queueSize, queueSize);
|
||||
|
||||
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_);
|
||||
sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
|
||||
|
||||
image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
|
||||
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
|
||||
info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
|
||||
odom_sub_.subscribe(nh, "odom_in", 1);
|
||||
|
||||
imagePub_ = rgb_it.advertise("image_out", 1);
|
||||
imageDepthPub_ = depth_it.advertise("image_out", 1);
|
||||
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
|
||||
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 1);
|
||||
};
|
||||
|
||||
void callback(const sensor_msgs::ImageConstPtr& image,
|
||||
const sensor_msgs::ImageConstPtr& imageDepth,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfo,
|
||||
const nav_msgs::OdometryConstPtr & odom)
|
||||
{
|
||||
if(imagePub_.getNumSubscribers())
|
||||
{
|
||||
imagePub_.publish(image);
|
||||
}
|
||||
if(imageDepthPub_.getNumSubscribers())
|
||||
{
|
||||
imageDepthPub_.publish(imageDepth);
|
||||
}
|
||||
if(infoPub_.getNumSubscribers())
|
||||
{
|
||||
infoPub_.publish(camInfo);
|
||||
}
|
||||
if(odomPub_.getNumSubscribers())
|
||||
{
|
||||
odomPub_.publish(odom);
|
||||
}
|
||||
}
|
||||
|
||||
image_transport::Publisher imagePub_;
|
||||
image_transport::Publisher imageDepthPub_;
|
||||
ros::Publisher infoPub_;
|
||||
ros::Publisher odomPub_;
|
||||
|
||||
image_transport::SubscriberFilter image_sub_;
|
||||
image_transport::SubscriberFilter image_depth_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
|
||||
message_filters::Synchronizer<MySyncPolicy> * sync_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::DataOdomSyncNodelet, nodelet::Nodelet);
|
||||
}
|
||||
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 "pluginlib/class_list_macros.hpp"
|
||||
#include "nodelet/nodelet.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <rtabmap/core/util2d.h>
|
||||
|
||||
namespace rtabmap_legacy
|
||||
{
|
||||
|
||||
class DataThrottleNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
//Constructor
|
||||
DataThrottleNodelet():
|
||||
rate_(0),
|
||||
approxSync_(0),
|
||||
exactSync_(0),
|
||||
decimation_(1)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~DataThrottleNodelet()
|
||||
{
|
||||
if(approxSync_)
|
||||
{
|
||||
delete approxSync_;
|
||||
}
|
||||
if(exactSync_)
|
||||
{
|
||||
delete exactSync_;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
ros::Time last_update_;
|
||||
double rate_;
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
ros::NodeHandle& private_nh = getPrivateNodeHandle();
|
||||
|
||||
ros::NodeHandle rgb_nh(nh, "rgb");
|
||||
ros::NodeHandle depth_nh(nh, "depth");
|
||||
ros::NodeHandle rgb_pnh(private_nh, "rgb");
|
||||
ros::NodeHandle depth_pnh(private_nh, "depth");
|
||||
image_transport::ImageTransport rgb_it(rgb_nh);
|
||||
image_transport::ImageTransport depth_it(depth_nh);
|
||||
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
|
||||
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
|
||||
|
||||
int queueSize = 10;
|
||||
bool approxSync = true;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
if(private_nh.getParam("max_rate", rate_))
|
||||
{
|
||||
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
|
||||
}
|
||||
private_nh.param("rate", rate_, rate_);
|
||||
private_nh.param("queue_size", queueSize, queueSize);
|
||||
private_nh.param("approx_sync", approxSync, approxSync);
|
||||
private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
|
||||
private_nh.param("decimation", decimation_, decimation_);
|
||||
ROS_ASSERT(decimation_ >= 1);
|
||||
NODELET_INFO("rate=%f Hz", rate_);
|
||||
NODELET_INFO("decimation=%d", decimation_);
|
||||
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
|
||||
approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
|
||||
exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
|
||||
}
|
||||
|
||||
image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
|
||||
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
|
||||
info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
|
||||
|
||||
imagePub_ = rgb_it.advertise("image_out", 1);
|
||||
imageDepthPub_ = depth_it.advertise("image_out", 1);
|
||||
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
|
||||
};
|
||||
|
||||
void callback(const sensor_msgs::ImageConstPtr& image,
|
||||
const sensor_msgs::ImageConstPtr& imageDepth,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfo)
|
||||
{
|
||||
if (rate_ > 0.0)
|
||||
{
|
||||
NODELET_DEBUG("update set to %f", rate_);
|
||||
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
|
||||
{
|
||||
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
NODELET_DEBUG("rate unset continuing");
|
||||
|
||||
last_update_ = ros::Time::now();
|
||||
|
||||
double rgbStamp = image->header.stamp.toSec();
|
||||
double depthStamp = imageDepth->header.stamp.toSec();
|
||||
double infoStamp = camInfo->header.stamp.toSec();
|
||||
|
||||
if(infoPub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
sensor_msgs::CameraInfo info = *camInfo;
|
||||
info.height /= decimation_;
|
||||
info.width /= decimation_;
|
||||
info.roi.height /= decimation_;
|
||||
info.roi.width /= decimation_;
|
||||
info.K[2]/=float(decimation_); // cx
|
||||
info.K[5]/=float(decimation_); // cy
|
||||
info.K[0]/=float(decimation_); // fx
|
||||
info.K[4]/=float(decimation_); // fy
|
||||
info.P[2]/=float(decimation_); // cx
|
||||
info.P[6]/=float(decimation_); // cy
|
||||
info.P[0]/=float(decimation_); // fx
|
||||
info.P[5]/=float(decimation_); // fy
|
||||
infoPub_.publish(info);
|
||||
}
|
||||
else
|
||||
{
|
||||
infoPub_.publish(camInfo);
|
||||
}
|
||||
}
|
||||
if(imagePub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
|
||||
cv_bridge::CvImage out;
|
||||
out.header = imagePtr->header;
|
||||
out.encoding = imagePtr->encoding;
|
||||
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
|
||||
imagePub_.publish(out.toImageMsg());
|
||||
}
|
||||
else
|
||||
{
|
||||
imagePub_.publish(image);
|
||||
}
|
||||
}
|
||||
|
||||
if(imageDepthPub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
|
||||
cv_bridge::CvImage out;
|
||||
out.header = imagePtr->header;
|
||||
out.encoding = imagePtr->encoding;
|
||||
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
|
||||
imageDepthPub_.publish(out.toImageMsg());
|
||||
}
|
||||
else
|
||||
{
|
||||
imageDepthPub_.publish(imageDepth);
|
||||
}
|
||||
}
|
||||
|
||||
if( rgbStamp != image->header.stamp.toSec() ||
|
||||
depthStamp != imageDepth->header.stamp.toSec())
|
||||
{
|
||||
NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
|
||||
"sure the node publishing the topics doesn't override the same data after publishing them. A "
|
||||
"solution is to use this node within another nodelet manager. Stamps: "
|
||||
"rgb=%f->%f depth=%f->%f",
|
||||
rgbStamp, image->header.stamp.toSec(),
|
||||
depthStamp, imageDepth->header.stamp.toSec());
|
||||
}
|
||||
}
|
||||
|
||||
image_transport::Publisher imagePub_;
|
||||
image_transport::Publisher imageDepthPub_;
|
||||
ros::Publisher infoPub_;
|
||||
|
||||
image_transport::SubscriberFilter image_sub_;
|
||||
image_transport::SubscriberFilter image_depth_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
int decimation_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::DataThrottleNodelet, nodelet::Nodelet);
|
||||
}
|
||||
@@ -0,0 +1,357 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 <pluginlib/class_list_macros.hpp>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include "rtabmap/core/util3d.h"
|
||||
#include "rtabmap/core/util3d_filtering.h"
|
||||
#include "rtabmap/core/util3d_mapping.h"
|
||||
#include "rtabmap/core/util3d_transforms.h"
|
||||
|
||||
namespace rtabmap_legacy
|
||||
{
|
||||
|
||||
class ObstaclesDetectionOld : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
ObstaclesDetectionOld() :
|
||||
frameId_("base_link"),
|
||||
normalKSearch_(20),
|
||||
groundNormalAngle_(M_PI_4),
|
||||
clusterRadius_(0.05),
|
||||
minClusterSize_(20),
|
||||
maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
|
||||
maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
|
||||
segmentFlatObstacles_(false),
|
||||
waitForTransform_(false),
|
||||
optimizeForCloseObjects_(false),
|
||||
projVoxelSize_(0.01)
|
||||
{}
|
||||
|
||||
virtual ~ObstaclesDetectionOld()
|
||||
{}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
|
||||
int queueSize = 10;
|
||||
pnh.param("queue_size", queueSize, queueSize);
|
||||
pnh.param("frame_id", frameId_, frameId_);
|
||||
pnh.param("normal_k", normalKSearch_, normalKSearch_);
|
||||
pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
|
||||
if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
|
||||
{
|
||||
NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
|
||||
"to \"cluster_radius\"! Your value is still copied to "
|
||||
"corresponding parameter. Instead of normal radius, nearest neighbors count "
|
||||
"\"normal_k\" is used instead (default 20).");
|
||||
pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
|
||||
}
|
||||
else
|
||||
{
|
||||
pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
|
||||
}
|
||||
pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
|
||||
pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
|
||||
pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
|
||||
pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
|
||||
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
|
||||
pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
|
||||
pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_);
|
||||
|
||||
cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this);
|
||||
|
||||
groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
|
||||
obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
|
||||
projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
|
||||
{
|
||||
ros::WallTime time = ros::WallTime::now();
|
||||
|
||||
if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
// no one wants the results
|
||||
return;
|
||||
}
|
||||
|
||||
rtabmap::Transform localTransform;
|
||||
try
|
||||
{
|
||||
if(waitForTransform_)
|
||||
{
|
||||
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
|
||||
{
|
||||
NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
tf::StampedTransform tmp;
|
||||
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
|
||||
localTransform = rtabmap_conversions::transformFromTF(tmp);
|
||||
}
|
||||
catch(tf::TransformException & ex)
|
||||
{
|
||||
NODELET_ERROR("%s",ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::fromROSMsg(*cloudMsg, *originalCloud);
|
||||
|
||||
//Common variables for all strategies
|
||||
pcl::IndicesPtr ground, obstacles;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
if(originalCloud->size())
|
||||
{
|
||||
originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
|
||||
if(maxObstaclesHeight_ > 0)
|
||||
{
|
||||
// std::numeric_limits<float>::lowest() exists only for c++11
|
||||
originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
|
||||
}
|
||||
|
||||
if(originalCloud->size())
|
||||
{
|
||||
if(!optimizeForCloseObjects_)
|
||||
{
|
||||
// This is the default strategy
|
||||
pcl::IndicesPtr flatObstacles(new std::vector<int>);
|
||||
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
|
||||
originalCloud,
|
||||
ground,
|
||||
obstacles,
|
||||
normalKSearch_,
|
||||
groundNormalAngle_,
|
||||
clusterRadius_,
|
||||
minClusterSize_,
|
||||
segmentFlatObstacles_,
|
||||
maxGroundHeight_,
|
||||
&flatObstacles);
|
||||
|
||||
if(groundPub_.getNumSubscribers() &&
|
||||
ground.get() && ground->size())
|
||||
{
|
||||
pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
|
||||
}
|
||||
|
||||
if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
|
||||
obstacles.get() && obstacles->size())
|
||||
{
|
||||
// remove flat obstacles from obstacles
|
||||
std::set<int> flatObstaclesSet;
|
||||
if(projObstaclesPub_.getNumSubscribers())
|
||||
{
|
||||
flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
|
||||
}
|
||||
|
||||
obstaclesCloud->resize(obstacles->size());
|
||||
obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
|
||||
|
||||
int oi=0;
|
||||
for(unsigned int i=0; i<obstacles->size(); ++i)
|
||||
{
|
||||
obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
|
||||
if(flatObstaclesSet.size() == 0 ||
|
||||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
|
||||
{
|
||||
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
|
||||
obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
|
||||
++oi;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
obstaclesCloudWithoutFlatSurfaces->resize(oi);
|
||||
if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
|
||||
{
|
||||
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// in this case optimizeForCloseObject_ is true:
|
||||
// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
|
||||
// one for potential floor points further away than 1m.
|
||||
// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
|
||||
// which allows to detect smaller objects, without increasing the number of false positive.
|
||||
// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
|
||||
// grond normal angle (* 2.).
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
|
||||
|
||||
// Part 1: segment floor and obstacles near the robot
|
||||
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
|
||||
originalCloud_near,
|
||||
ground,
|
||||
obstacles,
|
||||
normalKSearch_,
|
||||
groundNormalAngle_,
|
||||
clusterRadius_,
|
||||
minClusterSize_,
|
||||
segmentFlatObstacles_,
|
||||
maxGroundHeight_);
|
||||
|
||||
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
|
||||
{
|
||||
pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
|
||||
ground->clear();
|
||||
}
|
||||
|
||||
if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
|
||||
{
|
||||
pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
|
||||
obstacles->clear();
|
||||
}
|
||||
|
||||
// Part 2: segment floor and obstacles far from the robot
|
||||
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
|
||||
originalCloud_far,
|
||||
ground,
|
||||
obstacles,
|
||||
normalKSearch_,
|
||||
2.*groundNormalAngle_,
|
||||
3.*clusterRadius_,
|
||||
minClusterSize_,
|
||||
segmentFlatObstacles_,
|
||||
maxGroundHeight_);
|
||||
|
||||
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
|
||||
*groundCloud += *groundCloud2;
|
||||
}
|
||||
|
||||
|
||||
if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
|
||||
*obstaclesCloud += *obstacles2;
|
||||
}
|
||||
}
|
||||
|
||||
if(!localTransform.isIdentity())
|
||||
{
|
||||
//transform back in topic frame
|
||||
rtabmap::Transform localTransformInv = localTransform.inverse();
|
||||
if(groundCloud->size())
|
||||
{
|
||||
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
|
||||
}
|
||||
if(obstaclesCloud->size())
|
||||
{
|
||||
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(groundPub_.getNumSubscribers())
|
||||
{
|
||||
sensor_msgs::PointCloud2 rosCloud;
|
||||
pcl::toROSMsg(*groundCloud, rosCloud);
|
||||
rosCloud.header = cloudMsg->header;
|
||||
|
||||
//publish the message
|
||||
groundPub_.publish(rosCloud);
|
||||
}
|
||||
|
||||
if(obstaclesPub_.getNumSubscribers())
|
||||
{
|
||||
sensor_msgs::PointCloud2 rosCloud;
|
||||
pcl::toROSMsg(*obstaclesCloud, rosCloud);
|
||||
rosCloud.header = cloudMsg->header;
|
||||
|
||||
//publish the message
|
||||
obstaclesPub_.publish(rosCloud);
|
||||
}
|
||||
|
||||
if(projObstaclesPub_.getNumSubscribers())
|
||||
{
|
||||
sensor_msgs::PointCloud2 rosCloud;
|
||||
pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
|
||||
rosCloud.header.stamp = cloudMsg->header.stamp;
|
||||
rosCloud.header.frame_id = frameId_;
|
||||
|
||||
//publish the message
|
||||
projObstaclesPub_.publish(rosCloud);
|
||||
}
|
||||
|
||||
NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
|
||||
}
|
||||
|
||||
private:
|
||||
std::string frameId_;
|
||||
int normalKSearch_;
|
||||
double groundNormalAngle_;
|
||||
double clusterRadius_;
|
||||
int minClusterSize_;
|
||||
double maxObstaclesHeight_;
|
||||
double maxGroundHeight_;
|
||||
bool segmentFlatObstacles_;
|
||||
bool waitForTransform_;
|
||||
bool optimizeForCloseObjects_;
|
||||
double projVoxelSize_;
|
||||
|
||||
tf::TransformListener tfListener_;
|
||||
|
||||
ros::Publisher groundPub_;
|
||||
ros::Publisher obstaclesPub_;
|
||||
ros::Publisher projObstaclesPub_;
|
||||
|
||||
ros::Subscriber cloudSub_;
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::ObstaclesDetectionOld, nodelet::Nodelet);
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,270 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 "pluginlib/class_list_macros.hpp"
|
||||
#include "nodelet/nodelet.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <rtabmap/core/util2d.h>
|
||||
|
||||
namespace rtabmap_legacy
|
||||
{
|
||||
|
||||
class StereoThrottleNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
//Constructor
|
||||
StereoThrottleNodelet():
|
||||
rate_(0),
|
||||
approxSync_(0),
|
||||
exactSync_(0),
|
||||
decimation_(1)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~StereoThrottleNodelet()
|
||||
{
|
||||
if(approxSync_)
|
||||
{
|
||||
delete approxSync_;
|
||||
}
|
||||
if(exactSync_)
|
||||
{
|
||||
delete exactSync_;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
ros::Time last_update_;
|
||||
double rate_;
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
ros::NodeHandle& pnh = getPrivateNodeHandle();
|
||||
|
||||
ros::NodeHandle left_nh(nh, "left");
|
||||
ros::NodeHandle right_nh(nh, "right");
|
||||
ros::NodeHandle left_pnh(pnh, "left");
|
||||
ros::NodeHandle right_pnh(pnh, "right");
|
||||
image_transport::ImageTransport left_it(left_nh);
|
||||
image_transport::ImageTransport right_it(right_nh);
|
||||
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
|
||||
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
|
||||
|
||||
int queueSize = 5;
|
||||
bool approxSync = false;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
pnh.param("approx_sync", approxSync, approxSync);
|
||||
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
|
||||
pnh.param("rate", rate_, rate_);
|
||||
pnh.param("queue_size", queueSize, queueSize);
|
||||
pnh.param("decimation", decimation_, decimation_);
|
||||
ROS_ASSERT(decimation_ >= 1);
|
||||
NODELET_INFO("rate=%f Hz", rate_);
|
||||
NODELET_INFO("decimation=%d", decimation_);
|
||||
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
if(approxSyncMaxInterval>0.0)
|
||||
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
|
||||
approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
|
||||
}
|
||||
|
||||
imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
|
||||
imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
|
||||
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
|
||||
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
|
||||
|
||||
imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
|
||||
imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
|
||||
infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
|
||||
infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
|
||||
};
|
||||
|
||||
void callback(const sensor_msgs::ImageConstPtr& imageLeft,
|
||||
const sensor_msgs::ImageConstPtr& imageRight,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoRight)
|
||||
{
|
||||
if (rate_ > 0.0)
|
||||
{
|
||||
NODELET_DEBUG("update set to %f", rate_);
|
||||
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
|
||||
{
|
||||
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
NODELET_DEBUG("rate unset continuing");
|
||||
|
||||
last_update_ = ros::Time::now();
|
||||
|
||||
double leftStamp = imageLeft->header.stamp.toSec();
|
||||
double rightStamp = imageRight->header.stamp.toSec();
|
||||
double leftInfoStamp = camInfoLeft->header.stamp.toSec();
|
||||
double rightInfoStamp = camInfoRight->header.stamp.toSec();
|
||||
|
||||
if(infoLeftPub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
sensor_msgs::CameraInfo info = *camInfoLeft;
|
||||
info.height /= decimation_;
|
||||
info.width /= decimation_;
|
||||
info.roi.height /= decimation_;
|
||||
info.roi.width /= decimation_;
|
||||
info.K[2]/=float(decimation_); // cx
|
||||
info.K[5]/=float(decimation_); // cy
|
||||
info.K[0]/=float(decimation_); // fx
|
||||
info.K[4]/=float(decimation_); // fy
|
||||
info.P[2]/=float(decimation_); // cx
|
||||
info.P[6]/=float(decimation_); // cy
|
||||
info.P[0]/=float(decimation_); // fx
|
||||
info.P[5]/=float(decimation_); // fy
|
||||
info.P[3]/=float(decimation_); // Tx
|
||||
infoLeftPub_.publish(info);
|
||||
}
|
||||
else
|
||||
{
|
||||
infoLeftPub_.publish(camInfoLeft);
|
||||
}
|
||||
}
|
||||
if(infoRightPub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
sensor_msgs::CameraInfo info = *camInfoRight;
|
||||
info.height /= decimation_;
|
||||
info.width /= decimation_;
|
||||
info.roi.height /= decimation_;
|
||||
info.roi.width /= decimation_;
|
||||
info.K[2]/=float(decimation_); // cx
|
||||
info.K[5]/=float(decimation_); // cy
|
||||
info.K[0]/=float(decimation_); // fx
|
||||
info.K[4]/=float(decimation_); // fy
|
||||
info.P[2]/=float(decimation_); // cx
|
||||
info.P[6]/=float(decimation_); // cy
|
||||
info.P[0]/=float(decimation_); // fx
|
||||
info.P[5]/=float(decimation_); // fy
|
||||
info.P[3]/=float(decimation_); // Tx
|
||||
infoRightPub_.publish(info);
|
||||
}
|
||||
else
|
||||
{
|
||||
infoRightPub_.publish(camInfoRight);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if(imageLeftPub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
|
||||
cv_bridge::CvImage out;
|
||||
out.header = imagePtr->header;
|
||||
out.encoding = imagePtr->encoding;
|
||||
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
|
||||
imageLeftPub_.publish(out.toImageMsg());
|
||||
}
|
||||
else
|
||||
{
|
||||
imageLeftPub_.publish(imageLeft);
|
||||
}
|
||||
}
|
||||
if(imageRightPub_.getNumSubscribers())
|
||||
{
|
||||
if(decimation_ > 1)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
|
||||
cv_bridge::CvImage out;
|
||||
out.header = imagePtr->header;
|
||||
out.encoding = imagePtr->encoding;
|
||||
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
|
||||
imageRightPub_.publish(out.toImageMsg());
|
||||
}
|
||||
else
|
||||
{
|
||||
imageRightPub_.publish(imageRight);
|
||||
}
|
||||
}
|
||||
|
||||
if( leftStamp != imageLeft->header.stamp.toSec() ||
|
||||
rightStamp != imageRight->header.stamp.toSec())
|
||||
{
|
||||
NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
|
||||
"sure the node publishing the topics doesn't override the same data after publishing them. A "
|
||||
"solution is to use this node within another nodelet manager. Stamps: "
|
||||
"left%f->%f right=%f->%f",
|
||||
leftStamp, imageLeft->header.stamp.toSec(),
|
||||
rightStamp, imageRight->header.stamp.toSec());
|
||||
}
|
||||
}
|
||||
|
||||
image_transport::Publisher imageLeftPub_;
|
||||
image_transport::Publisher imageRightPub_;
|
||||
ros::Publisher infoLeftPub_;
|
||||
ros::Publisher infoRightPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageLeft_;
|
||||
image_transport::SubscriberFilter imageRight_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
int decimation_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::StereoThrottleNodelet, nodelet::Nodelet);
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 <pluginlib/class_list_macros.hpp>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber.h>
|
||||
#include <image_transport/publisher.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
|
||||
namespace rtabmap_legacy
|
||||
{
|
||||
|
||||
class UndistortDepth : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
UndistortDepth()
|
||||
{}
|
||||
|
||||
virtual ~UndistortDepth()
|
||||
{
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
|
||||
std::string modelPath;
|
||||
pnh.param("model", modelPath, modelPath);
|
||||
|
||||
if(modelPath.empty())
|
||||
{
|
||||
NODELET_ERROR("undistort_depth: \"model\" parameter should be set!");
|
||||
}
|
||||
|
||||
model_.load(modelPath);
|
||||
if(!model_.isValid())
|
||||
{
|
||||
NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
image_transport::ImageTransport it(nh);
|
||||
sub_ = it.subscribe("depth", 1, &UndistortDepth::callback, this);
|
||||
pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1);
|
||||
}
|
||||
}
|
||||
|
||||
void callback(const sensor_msgs::ImageConstPtr& depth)
|
||||
{
|
||||
if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
|
||||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
|
||||
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
|
||||
{
|
||||
NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
|
||||
return;
|
||||
}
|
||||
|
||||
if(pub_.getNumSubscribers())
|
||||
{
|
||||
if(depth->width == model_.getWidth() && depth->width == model_.getWidth())
|
||||
{
|
||||
cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
|
||||
model_.undistort(imageDepthPtr->image);
|
||||
pub_.publish(imageDepthPtr->toImageMsg());
|
||||
}
|
||||
else
|
||||
{
|
||||
NODELET_ERROR("Input depth image size (%dx%d) and distortion model "
|
||||
"size (%dx%d) don't match! Cannot undistort image.",
|
||||
depth->width, depth->height,
|
||||
model_.getWidth(), model_.getHeight());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
clams::DiscreteDepthDistortionModel model_;
|
||||
image_transport::Publisher pub_;
|
||||
image_transport::Subscriber sub_;
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::UndistortDepth, nodelet::Nodelet);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user