introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

MapCloudDisplay: Cannot call "/rtabmap/get_map_data" service #1126

Open BytePaul opened 9 months ago

BytePaul commented 9 months ago

When trying to download the map in Rviz under MapCLoud it geives me the following error : "MapCloudDisplay: Cannot call "/rtabmap/get_map_data" service"

this is my program:

<launch>

<arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>

  <!-- Choose visualization -->
  <arg name="rtabmap_viz"             default="true" />
  <arg name="rviz"                    default="true" />

  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  <arg name="initial_pose"            default=""/>             <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc -->
  <arg name="loc_thr"                 default="0.0"/>          <!-- In localization mode, setting this threshold will make rtabmap publishing diagnostics about if the robot is localized (under that error threshold). -->

  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>

  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_launch)/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="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="100"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="clear_params"            default="true"/>
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>
  <arg name="approx_sync_max_interval"  default="0"/> <!-- (sec) 0 means infinite interval duration (used with approx_sync=true) -->

  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />

  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />

  <!-- Already synchronized RGB-D related topic, with rtabmap_sync/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />

  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->

  <arg name="gen_cloud"               default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
  <arg name="gen_cloud_decimation"    default="4"/>
  <arg name="gen_cloud_voxel"         default="0.05"/>

  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="$(arg gen_cloud)"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_deskewing"          default="false"/>
  <arg name="scan_deskewing_slerp"    default="false"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="$(arg scan_deskewing)"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->

  <arg name="gen_depth"                  default="false" /> <!-- Generate depth image from scan_cloud -->
  <arg name="gen_depth_decimation"       default="1" />
  <arg name="gen_depth_fill_holes_size"  default="0" />
  <arg name="gen_depth_fill_iterations"  default="1" />
  <arg name="gen_depth_fill_holes_error" default="0.1" />

  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="/tb_control/wheel_odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="odom"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="0.001"/>         <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="0.001"/>         <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <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="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>

  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>      <!-- max_clouds and time should not be set at the same time -->
  <arg name="scan_cloud_assembling_max_clouds"   default="0"/>      <!-- max_clouds and time should not be set at the same time -->
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_range_min"    default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_range_max"    default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>

  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->

  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->

  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  <arg name="fiducial_topic"           default="/fiducial_transforms" />  <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->

  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <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="rtabmap">

    <!-- Launch the freenect nodelet manager and other related nodelets for Kinect v1 -->
    <!--<include file="$(find freenect_launch)/launch/freenect.launch"> -->
    <!-- You can include any specific configurations for freenect_launch here -->
    <!--    <arg name="depth_registration" value="true"/> -->
    <!-- </include> -->

    <!-- Load the rgbd_sync nodelet into the /camera_nodelet_manager provided by freenect_launch -->
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_sync/rgbd_sync" 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/rgb/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->

      <!-- Should be true for not synchronized camera topics 
           (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="true"/> 
    </node>

    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
          <param name="frame_id" type="string" value="base_link"/>

          <param name="subscribe_depth" type="bool" value="false"/>
          <param name="subscribe_rgbd" type="bool" value="true"/>
          <param name="subscribe_scan" type="bool" value="true"/>

          <remap from="odom" to="/tb_control/wheel_odom"/>
          <remap from="scan" to="/scan"/>
          <remap from="rgbd_image" to="rgbd_image"/>

          <param name="queue_size" type="int" value="1000"/>

          <!-- 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="Grid/FromDepth"            type="string" value="false"/> <!-- occupancy grid from lidar -->
          <param name="Reg/Force3DoF"             type="string" value="true"/>
          <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
          <param name="Grid/3D"              type="string" value="true"/>
          <param name="Grid/Sensor"              type="string" value="2"/>
          <param name="Grid/CellSize"              type="string" value="0.05"/>
          <param name="Grid/RangeMax"              type="string" value="0"/>
          <param name="Grid/RayTracing"              type="string" value="true"/>

          <!-- ICP parameters -->
          <param name="Icp/VoxelSize"                 type="string" value="0.05"/>
          <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

          <!-- localization mode -->
          <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
          <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
          <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>

    </node>

        <node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_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/rgb/camera_info"/>
          <remap from="rgbd_image"      to="rgbd_image"/>
          <remap from="odom"            to="/tb_control/wheel_odom"/>

          <param name="frame_id"                    type="string" value="footprint"/>
          <param name="publish_tf"                  type="bool"   value="true"/>
          <param name="wait_for_transform_duration" type="double" value="0.2"/>

        </node>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <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="$(eval approx_sync and not use_odom_features)"/>

      <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     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>

      <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 unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>

  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />
    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
    <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
  </node>
  </group>
</launch>

i was able to see it previously when I was using just a kinect camera for Visual SLAM, but when i am trying to get both LiDAR and Camera running it gives me the above error

matlabbe commented 9 months ago

I think it is because rviz is started inside rtabmap namespace. You may have to change the namespace property of MapCloud display to nothing or /rtabmap instead of default rtabmap.

image