Re: Problems with launch and mapping
Posted by denzle on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problems-with-launch-and-mapping-tp6808p6829.html
So i appear to have made some progress. Although the odom is out and quite messy.
I made some changes to cpp files in the ouster folder particularly the timing. And use of tf2_ros allowed proper correction.
I'm hoping you may be able to direct me where i'm going wrong
- The use of rviz to view the cloud built in rtabmapviz
- I'm going to read through any other problems with odom within the forum but any advice would be most welcome
- Have i got the imu and lidar correct or should they be directly connected to base_link rather than os1_sensor?
- opencv appears to be missing although reading 'https://github.com/introlab/rtabmap_ros/issues/422' i assume i should follow the information in that thread. Is it even worth adding opencv from source?
- Would the use of a realsense t265 increase odometry or should i be looking at other means ie Hz increase or soem missing node?
Some info from my current setup. My launch file does need cleaning up but for now it's working also the realsense needs to be added once i have the lidar working properly.
Thanks in advance
Dan
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problems-with-launch-and-mapping-tp6808p6829.html
So i appear to have made some progress. Although the odom is out and quite messy.
I made some changes to cpp files in the ouster folder particularly the timing. And use of tf2_ros allowed proper correction.
I'm hoping you may be able to direct me where i'm going wrong
- The use of rviz to view the cloud built in rtabmapviz
- I'm going to read through any other problems with odom within the forum but any advice would be most welcome
- Have i got the imu and lidar correct or should they be directly connected to base_link rather than os1_sensor?
- opencv appears to be missing although reading 'https://github.com/introlab/rtabmap_ros/issues/422' i assume i should follow the information in that thread. Is it even worth adding opencv from source?
- Would the use of a realsense t265 increase odometry or should i be looking at other means ie Hz increase or soem missing node?
Some info from my current setup. My launch file does need cleaning up but for now it's working also the realsense needs to be added once i have the lidar working properly.
Thanks in advance
Dan
<?xml version="1.0"?>
<!--
Copyright (c) 2018, STEREOLABS.
All rights reserved.
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.
-->
<launch>
<arg name="frame_id" default="base_link"/>
<arg name="odom_frame_id" default="/odom"/>
<arg name="map_frame_id" default="map"/>
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="true" />
<arg name="image" default="true" />
<arg name="scan_20_hz" default="true"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="namespace" default="rtabmap"/>
<arg name="rgb_topic" default="/camera/color/image_raw"/>
<arg name="depth_topic" default="/camera/aligned_depth_to_color/image_raw"/>
<arg name="camera_info_topic" default="/camera/color/camera_info"/>
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="output" default="screen"/>
<arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
<arg name="rgbd_sync" default="true"/>
<arg name="approx_rgbd_sync" default="true"/>
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="rgbd_topic" default="rgbd_image" />
<arg name="depth_scale" default="1.0" />
<arg name="approx_sync" default="true"/>
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="true"/>
<arg name="scan_cloud_topic" default="/os1_cloud_node/points"/>
<arg name="scan_cloud_max_points" default="0"/>
<arg name="scan_cloud_filtered" default="false"/>
<arg name="visual_odometry" default="true"/>
<arg name="icp_odometry" default="true"/>
<arg name="args" default=""/>
<arg name="rtabmap_args" default="$(arg args)"/>
<arg name="launch_prefix" default=""/>
<arg name="odom_topic" default="/odom"/>
<arg name="vo_frame_id" default="$(arg odom_topic)"/>
<arg name="publish_tf_odom" default="true"/>
<arg name="odom_tf_angular_variance" default="1"/>
<arg name="odom_tf_linear_variance" default="1"/>
<arg name="odom_args" default=""/>
<arg name="odom_sensor_sync" default="false"/>
<arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="imu_topic" default="/os1_cloud_node/imu/data"/>
<arg name="wait_imu_to_init" default="true"/>
<arg name="publish_tf_map" default="true"/>
<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" />
<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="stereo" default="false"/>
<arg if="$(arg stereo)" name="depth" default="false"/>
<arg unless="$(arg stereo)" name="depth" default="true"/>
<arg name="compressed" default="false"/>
<arg name="rgb_image_transport" default="compressed"/>
<arg name="depth_image_transport" default="compressedDepth"/>
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/>
<arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/>
<arg if="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)"/>
<arg unless="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)_relay"/>
<group ns="$(arg namespace)">
<group if="$(arg depth)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
</group>
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
</node>
</group>
</group>
<group if="$(arg stereo)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
</group>
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
</node>
</group>
</group>
<group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros">
<remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
<remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
</node>
</group>
</group>
<node if="$(arg rviz)" pkg="ouster_ros" name="viz_node" type="viz_node" output="screen" required="true">
<remap from="~/os1_config" to="/os1_node/os1_config"/>
<remap from="~/points" to="/os1_cloud_node/points"/>
</node>
<node if="$(arg image)" pkg="ouster_ros" name="img_node" type="img_node" output="screen" required="true">
<remap from="~/os1_config" to="/os1_node/os1_config"/>
<remap from="~/points" to="/os1_cloud_node/points"/>
</node>
<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu" to="/os1_cloud_node/imu"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="true"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
<param name="use_mag" value="true"/>
<param name="publish_tf" value="false"/>
<param name="world_frame" value="enu"/>
<remap from="/imu/data_raw" to="/os1_cloud_node/imu/data_raw"/>
<remap from="/imu/data" to="/os1_cloud_node/imu/data"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="/os1_cloud_node/imu/data"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<group ns="rtabmap">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="rgbd_image"/> <!-- output -->
<param name="approx_sync" type="bool" value="true"/> <!-- false for realsense -->
</node>
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<remap from="imu" to="/os1_cloud_node/imu/data"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_imu_to_init" type="bool" value="false"/>
<param name="Reg/Strategy" type="string" value="1"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.9"/>
<param name="Odom/Strategy" type="string" value="1"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="subscribe_rgbd" type="bool" value="false"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/>
<param name="scan_cloud_max_points" value="32768"/> <!-- based on os-1 spec: 327,680 pts/second for 10 Hz -->
<remap from="odom" to="/odom"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<remap from="rgbd_image" to="rgbd_image"/>
<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Reg/Strategy" type="string" value="1"/> <!-- 1=ICP -->
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/MinInliers" type="string" value="15"/>
<param name="Rtabmap/TimeThr" type="string" value="0"/> <!-- set to 0 to disable memory management -->
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Grid/CellSize" type="string" value="0.05"/> <!-- 5cm voxel default-->
<param name="Kp/MaxFeatures" type="string" value="400"/>
<param name="Optimizer/Strategy" type="string" value="1"/> <!-- 0 is TORO, Default G20-->
<param name="RGBD/OptimizeMaxError" type="string" value="1"/> <!--When using TORO, seto to 0,otherwise set to 1-->
<param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
<param name="Grid/FromDepth" type="string" value="true"/> <!--suppress warning (subscribe_scan=true)-->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/> <!--suppress warning (subscribe_scan = true and Reg/Strategy=1-->
<param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Grid/FromDepth" type="string" value="true"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
<param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" />
<param name="fixed_frame_id" type="string" value="base_link" />
<remap from="cloud" to="/os1_cloud_node/points"/>
<remap from="odom" to="/odom"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="/odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<remap from="odom" to="odom"/>
<param name="subscribe_depth" value="false"/>
</node>
</group>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" />
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 1 map odom" />
<node pkg="tf2_ros" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 1 odom base_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_os1_sensor" args="0 0 0 0 0 0 1 base_link os1_sensor" />
<node pkg="tf2_ros" type="static_transform_publisher" name="os1_sensor_to_os1_lidar" args="0 0 0 0 0 0 1 os1_sensor os1_lidar" />
<node pkg="tf2_ros" type="static_transform_publisher" name="os1_sensor_to_os1_imu" args="0 0 0 0 0 0 1 os1_sensor os1_imu" />
</group>
</launch> [ INFO] [1596744081.894361327]: rtabmap (1177): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0545s, Maps update=0.0026s pub=0.0000s (local map=88, WM=88) [ INFO] [1596744081.933717591]: Odom: ratio=0.998808, std dev=0.009545m|0.009545rad, update time=0.003495s [ WARN] (2020-08-06 21:01:21.942) util3d.cpp:486::cloudFromDepthRGB() Decimation (4) is not valid for current image size (depth=4x3). Highest compatible decimation used=1. [ WARN] (2020-08-06 21:01:21.942) util3d.cpp:604::cloudFromDepthRGB() Cloud with only NaN values created! [ INFO] [1596744082.035554965]: Odom: ratio=0.998805, std dev=0.009056m|0.009056rad, update time=0.003423s [ INFO] [1596744082.139224506]: Odom: ratio=0.998808, std dev=0.009064m|0.009064rad, update time=0.003504s
| Free forum by Nabble | Edit this page |