Posted by
Chris_fhtw on
URL: http://official-rtab-map-forum.206.s1.nabble.com/rtanmap-without-publishing-map-odom-tp4780.html
Hello
I currently have a problem that in the tf-tree map and odom are already published by other Nodes running on my robot. Now when i start rtabmap to build a 3d-map while moving the robot all my TFs in rviz start jumping because rtabmap tryes to overwrite my map -> odom with its own.
Is there a way i can stop rtabmap publishing its own map --> odom or at least rename them to not get those topics overwritten all the time?
EDIT: I dont use visual odometry because i want to use the TFs and odometry from hector mapping
EDIT2:
Also i currently tried a new Code where i start hector_mapping together with rtabmap.
<?xml version="1.0"?>
<launch>
<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"/>
<arg if="$(arg localization)" name="increment_memory" default="false"/>
<arg unless="$(arg localization)" name="increment_memory" default="true"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatch_odom"/>
<arg name="base_frame" default="base_link"/>
<arg name="odom_frame" default="odom"/>
<arg name="pub_odometry" default="true"/>
<arg name="pub_map_odom_transform" default="false"/>
<arg name="pub_map_scanmatch_transform" default="true"/>
<arg name="scan_subscriber_queue_size" default="5"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_size" default="2048"/>
<arg name="trajectory_source_frame_name" default="/base_link"/>
<arg name="trajectory_update_rate" default="4"/>
<arg name="trajectory_publish_rate" default="0.25"/>
<arg name="map_file_path" default="/home/twmr/elrob2018_ws/maps"/>
<arg name="map_file_base_name" default="hector_slam_map"/>
<arg name="map_frame" default="hector_map"/>
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="false" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<remap from="dynamic_map" to="static_map"/>
</node>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
</node>
<node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
<remap from="map" to="/static_map" />
</node>
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<remap from="odom" to="/scanmatch_odom"/>
<remap from="scan" to="/scan"/>
<remap from="mapData" to="mapData"/>
<remap from="rgb/image" to="/camera/color/image_raw"/>
<remap from="depth/image" to="/camera/depth/image_rect_raw"/>
<remap from="rgb/camera_info" to="/camera/color/camera_info"/>
<remap from="goal_out" to="/move_base_simple/goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="grid_map" to="/rtabmap/grid_map"/>
</node>
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<remap from="rgb/image" to="/camera/color/image_raw"/>
<remap from="depth/image" to="/camera/depth/image_rect_raw"/>
<remap from="rgb/camera_info" to="/camera/color/camera_info"/>
<remap from="scan" to="/scan"/>
<remap from="odom" to="/scanmatch_odom"/>
</node>
</group>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/rgb/image_raw"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
</node>
</launch>
If i start the launchfile i get the 2D-scans from the laser with hector mapping but i get this failure from rtabmap:
... logging to /home/twmr/.ros/log/7e3f50d8-8b5d-11e8-b412-2046a100fb8d/roslaunch-twmr-desktop-27203.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server
http://10.0.0.100:41715/SUMMARY
========
PARAMETERS
* /hector_mapping/base_frame: base_link
* /hector_mapping/map_frame: hector_map
* /hector_mapping/map_multi_res_levels: 2
* /hector_mapping/map_resolution: 0.05
* /hector_mapping/map_size: 2048
* /hector_mapping/map_update_angle_thresh: 0.06
* /hector_mapping/odom_frame: odom
* /hector_mapping/pub_map_odom_transform: False
* /hector_mapping/pub_map_scanmatch_transform: True
* /hector_mapping/pub_odometry: True
* /hector_mapping/scan_topic: /scan
* /points_xyzrgb/depth/image_transport: compressedDepth
* /points_xyzrgb/rgb/image_transport: compressed
* /points_xyzrgb/voxel_size: 0.01
* /rosdistro: kinetic
* /rosversion: 1.12.13
* /rtabmap/rtabmap/Grid/FromDepth: false
* /rtabmap/rtabmap/Icp/CorrespondenceRatio: 0.2
* /rtabmap/rtabmap/Kp/DetectorStrategy: 0
* /rtabmap/rtabmap/Kp/MaxFeatures: 200
* /rtabmap/rtabmap/Mem/IncrementalMemory: true
* /rtabmap/rtabmap/Optimizer/Strategy: 0
* /rtabmap/rtabmap/RGBD/OptimizeFromGraphEnd: true
* /rtabmap/rtabmap/RGBD/ProximityPathMaxNeighbors: 10
* /rtabmap/rtabmap/Reg/Force3DoF: true
* /rtabmap/rtabmap/Reg/Strategy: 1
* /rtabmap/rtabmap/SURF/HessianThreshold: 500
* /rtabmap/rtabmap/Vis/InlierDistance: 0.1
* /rtabmap/rtabmap/Vis/MaxDepth: 10.0
* /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
* /rtabmap/rtabmap/frame_id: base_link
* /rtabmap/rtabmap/subscribe_depth: True
* /rtabmap/rtabmap/subscribe_scan: True
* /use_sim_time: False
NODES
/rtabmap/
rtabmap (rtabmap_ros/rtabmap)
/
hector_mapping (hector_mapping/hector_mapping)
points_xyzrgb (nodelet/nodelet)
ROS_MASTER_URI=
http://10.0.0.100:11311process[hector_mapping-1]: started with pid [27220]
process[rtabmap/rtabmap-2]: started with pid [27221]
HectorSM map lvl 0: cellLength: 0.05 res x:2048 res y: 2048
process[points_xyzrgb-3]: started with pid [27302]
HectorSM map lvl 1: cellLength: 0.1 res x:1024 res y: 1024
[ INFO] [1532012403.488806226]: Starting node...
[ INFO] [1532012403.508983224]: HectorSM p_base_frame_: base_link
[ INFO] [1532012403.509048222]: HectorSM p_map_frame_: hector_map
[ INFO] [1532012403.509075408]: HectorSM p_odom_frame_: odom
[ INFO] [1532012403.509104504]: HectorSM p_scan_topic_: /scan
[ INFO] [1532012403.509131871]: HectorSM p_use_tf_scan_transformation_: true
[ INFO] [1532012403.509158780]: HectorSM p_pub_map_odom_transform_: false
[ INFO] [1532012403.509184205]: HectorSM p_scan_subscriber_queue_size_: 5
[ INFO] [1532012403.509208815]: HectorSM p_map_pub_period_: 2.000000
[ INFO] [1532012403.509236405]: HectorSM p_update_factor_free_: 0.300000
[ INFO] [1532012403.509264892]: HectorSM p_update_factor_occupied_: 0.900000
[ INFO] [1532012403.509294169]: HectorSM p_map_update_distance_threshold_: 0.200000
[ INFO] [1532012403.509322816]: HectorSM p_map_update_angle_threshold_: 0.060000
[ INFO] [1532012403.509350473]: HectorSM p_laser_z_min_value_: -1.000000
[ INFO] [1532012403.509377430]: HectorSM p_laser_z_max_value_: 1.000000
[ INFO] [1532012403.540256961]: Initializing nodelet with 8 worker threads.
[ INFO] [1532012403.919025188]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000
[ INFO] [1532012403.919063945]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000
[ INFO] [1532012403.919083997]: /rtabmap/rtabmap(maps): map_cleanup = true
[ INFO] [1532012403.919099840]: /rtabmap/rtabmap(maps): map_negative_poses_ignored = true
[ INFO] [1532012403.919117610]: /rtabmap/rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1532012403.919132153]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true
[ INFO] [1532012403.919161526]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false
[ INFO] [1532012403.919190674]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1532012403.921649111]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16
[ INFO] [1532012403.944506270]: rtabmap: frame_id = base_link
[ INFO] [1532012403.944545761]: rtabmap: map_frame_id = map
[ INFO] [1532012403.944578001]: rtabmap: tf_delay = 0.050000
[ INFO] [1532012403.944602972]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1532012403.944628525]: rtabmap: odom_sensor_sync = false
[ INFO] [1532012404.040637806]: Setting RTAB-Map parameter "Grid/FromDepth"="false"
[ INFO] [1532012404.087996665]: Setting RTAB-Map parameter "Icp/CorrespondenceRatio"="0.2"
[ INFO] [1532012404.131003767]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="0"
[ INFO] [1532012404.145178863]: Setting RTAB-Map parameter "Kp/MaxFeatures"="200"
[ INFO] [1532012404.179917422]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1532012404.391684535]: Setting RTAB-Map parameter "Optimizer/Strategy"="0"
[ INFO] [1532012404.423410253]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="true"
[ INFO] [1532012404.442696536]: Setting RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="10"
[ INFO] [1532012404.448439530]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1532012404.450744548]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1532012404.507429741]: Setting RTAB-Map parameter "SURF/HessianThreshold"="500"
[ INFO] [1532012404.584709989]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.1"
[ INFO] [1532012404.587137935]: Setting RTAB-Map parameter "Vis/MaxDepth"="10.0"
[ INFO] [1532012404.843969416]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" is true.
[ INFO] [1532012405.058155300]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1532012405.067681522]: rtabmap: Deleted database "/home/twmr/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1532012405.067734244]: rtabmap: Using database from "/home/twmr/.ros/rtabmap.db" (0 MB).
[ WARN] (2018-07-19 17:00:05.069) Memory.cpp:652::parseParameters() Mem/UseOdomFeatures is enabled, but Vis/FeatureType and Kp/DetectorStrategy parameters are not the same! Disabling Mem/UseOdomFeatures...
[ WARN] (2018-07-19 17:00:05.071) Memory.cpp:652::parseParameters() Mem/UseOdomFeatures is enabled, but Vis/FeatureType and Kp/DetectorStrategy parameters are not the same! Disabling Mem/UseOdomFeatures...
[ WARN] (2018-07-19 17:00:06.231) Memory.cpp:652::parseParameters() Mem/UseOdomFeatures is enabled, but Vis/FeatureType and Kp/DetectorStrategy parameters are not the same! Disabling Mem/UseOdomFeatures...
[ INFO] [1532012406.231633785]: rtabmap: Database version = "0.17.3".
[ INFO] [1532012406.259811483]: /rtabmap/rtabmap: queue_size = 10
[ INFO] [1532012406.259843029]: /rtabmap/rtabmap: rgbd_cameras = 1
[ INFO] [1532012406.259863004]: /rtabmap/rtabmap: approx_sync = true
[ INFO] [1532012406.259917706]: Setup depth callback
[ INFO] [1532012406.291203681]:
/rtabmap/rtabmap subscribed to (approx sync):
/scanmatch_odom,
/camera/color/image_raw,
/camera/depth/image_rect_raw,
/camera/color/camera_info,
/scan
[ INFO] [1532012406.298435531]: rtabmap 0.17.3 started...
[ INFO:0] Initialize OpenCL runtime...
beignet-opencl-icd: no supported GPU found, this is probably the wrong opencl-icd package for this hardware
(If you have multiple ICDs installed and OpenCL works, you can ignore this message)
[ WARN] (2018-07-19 17:00:06.645) MapsManager.cpp:900::publishMaps() Graph has changed! The whole cloud is regenerated.
[ INFO] [1532012406.645733610]: Assembled 1 obstacle and 0 ground clouds (176 points, 0.000692s)
[ INFO] [1532012406.647797655]: rtabmap (1): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0792s, Maps update=0.0022s pub=0.0027s (local map=1, WM=1)
[ERROR] (2018-07-19 17:00:07.595) Memory.cpp:818::addSignatureToStm() Failed to invert the covariance matrix! Covariance matrix should be invertible!
Covariance: [10.50832843780518, 0.4830244183540344, 0, 0, 0, -109.3600463867188;
0.4830244183540344, 12.84318923950195, 0, 0, 0, -295.8150329589844;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
-109.3600463867188, -295.8150329589844, 0, 0, 0, 18401.181640625]
[ INFO] [1532012407.598056790]: Assembled 0 obstacle and 0 ground clouds (176 points, 0.000010s)
[ INFO] [1532012407.598381978]: rtabmap (2): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0359s, Maps update=0.0013s pub=0.0003s (local map=1, WM=1)
[ERROR] (2018-07-19 17:00:08.728) Memory.cpp:818::addSignatureToStm() Failed to invert the covariance matrix! Covariance matrix should be invertible!
Covariance: [10.68249607086182, 0.06204519420862198, 0, 0, 0, -104.4377822875977;
0.06204519420862198, 12.5845422744751, 0, 0, 0, -295.0779418945312;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0;
-104.4377822875977, -295.0779418945312, 0, 0, 0, 18898.70703125]
[ INFO] [1532012408.731288089]: Assembled 0 obstacle and 0 ground clouds (176 points, 0.000008s)
[ INFO] [1532012408.731611601]: rtabmap (3): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0360s, Maps update=0.0014s pub=0.0003s (local map=1, WM=1)
WHich means i may have to few samples or not continuous. I just havent found a way currently to fix this error. Is there maybe a problem with the odometry or too similar pictures?