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

Added test_two_kinects_one_map.launch. Removed tf_prefix parameter (frame_id, odom_frame_id and map_frame_id should be set directly)

Browse Source
This commit is contained in:
matlabbe
2017-04-04 12:55:31 -04:00
parent b0ee4b6398
commit 18d09f0405
4 changed files with 82 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/rtabmap.launch
+12 -6
View File
@@ -30,11 +30,14 @@
<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="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="map_frame_id" default="map"/>
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="10"/>
<arg name="wait_for_transform" default="0.2"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
@@ -65,7 +68,7 @@
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="vo_frame_id" default="odom"/>
<arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
@@ -98,10 +101,11 @@
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
@@ -117,10 +121,11 @@
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
@@ -134,6 +139,7 @@
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
@@ -175,8 +181,8 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
launch/tests/test_two_kinects_one_map.launch
+62
View File
@@ -0,0 +1,62 @@
<launch>
<!-- Testing 2 Kinects localizing in the same map at the same time -->
<!-- Prerequisities: the default ~/.ros/rtabmap.db should be
already created with one of the Kinect using mapping tutorial:
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="-.-delete_db_on_start"
-->
<!-- Cameras -->
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
<arg name="camera" value="camera1" />
<arg name="device_id" value="#1" />
</include>
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
<arg name="camera" value="camera2" />
<arg name="device_id" value="#2" />
</include>
<node pkg="tf" type="static_transform_publisher" name="world_to_map1"
args="0 0 0 0 0 0 world map1 100" />
<node pkg="tf" type="static_transform_publisher" name="world_to_map2"
args="0 0 0 0 0 0 world map2 100" />
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="namespace" value="rtabmap1"/>
<arg name="localization" value="true" />
<arg name="frame_id" value="camera1_link" />
<arg name="map_frame_id" value="map1" />
<arg name="vo_frame_id" value="odom1" />
<arg name="rgb_topic" value="/camera1/rgb/image_rect_color" />
<arg name="depth_topic" value="/camera1/depth_registered/image_raw" />
<arg name="camera_info_topic" value="/camera1/rgb/camera_info" />
<arg name="rtabmapviz" value="false"/>
</include>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="namespace" value="rtabmap2"/>
<arg name="localization" value="true" />
<arg name="frame_id" value="camera2_link" />
<arg name="map_frame_id" value="map2" />
<arg name="vo_frame_id" value="odom2" />
<arg name="rgb_topic" value="/camera2/rgb/image_rect_color" />
<arg name="depth_topic" value="/camera2/depth_registered/image_raw" />
<arg name="camera_info_topic" value="/camera2/rgb/camera_info" />
<arg name="rtabmapviz" value="false"/>
</include>
<!-- Visualization RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
</launch>
src/CoreWrapper.cpp
+4 -27
View File
@@ -126,7 +126,6 @@ void CoreWrapper::onInit()
bool publishTf = true;
double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms
std::string tfPrefix = "";
pnh.param("config_path", configPath_, configPath_);
pnh.param("database_path", databasePath_, databasePath_);
@@ -145,7 +144,10 @@ void CoreWrapper::onInit()
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
if(pnh.hasParam("tf_prefix"))
{
ROS_ERROR("tf_prefix parameter has been removed, use directly map_frame_id, odom_frame_id and frame_id parameters.");
}
pnh.param("tf_tolerance", tfTolerance, tfTolerance);
pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
@@ -166,31 +168,6 @@ void CoreWrapper::onInit()
"switches scan values.");
}
if(!tfPrefix.empty())
{
if(!frameId_.empty())
{
frameId_ = tfPrefix+"/"+frameId_;
}
if(!mapFrameId_.empty())
{
mapFrameId_ = tfPrefix+"/"+mapFrameId_;
}
if(!odomFrameId_.empty())
{
odomFrameId_ = tfPrefix+"/"+odomFrameId_;
}
if(!groundTruthFrameId_.empty())
{
groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_;
}
if(!groundTruthBaseFrameId_.empty())
{
groundTruthBaseFrameId_ = tfPrefix+"/"+groundTruthBaseFrameId_;
}
// keep worldFrameId_ without prefix as it should be global
}
NODELET_INFO("rtabmap: frame_id = %s", frameId_.c_str());
if(!odomFrameId_.empty())
{
src/OdometryROS.cpp
+4 -18
View File
@@ -112,12 +112,14 @@ void OdometryROS::onInit()
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
std::string tfPrefix;
std::string configPath;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
if(pnh.hasParam("tf_prefix"))
{
ROS_ERROR("tf_prefix parameter has been removed, use directly odom_frame_id and frame_id parameters.");
}
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
@@ -141,22 +143,6 @@ void OdometryROS::onInit()
configPath = UDirectory::currentDir(true) + configPath;
}
if(!tfPrefix.empty())
{
if(!frameId_.empty())
{
frameId_ = tfPrefix + "/" + frameId_;
}
if(!odomFrameId_.empty())
{
odomFrameId_ = tfPrefix + "/" + odomFrameId_;
}
if(!groundTruthFrameId_.empty())
{
groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
}
}
if(initialPoseStr.size())
{
std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));