Is there a way to run FUNMAP on Gazebo simulation

Hi, I included a Stretch Robot model into a Gazebo world that I prepared before. 2D Lidar-based SLAM with GMapping works well. I also would like to test the FUNMAP on this simulation environment but it does not work. Is there a way to run FUNMAP on Gazebo simulation?

Hi @burakaksoy, FUNMAP on the simulated Stretch should work in theory, but hasn’t been tested yet. The Gazebo simulation provides point clouds, 2D lidar scans, and odometry required by FUNMAP, but differs from the real Stretch in ways that could be causing failures. In particular, the simulated robot has five separate action servers for commanding joints, while the real robot has only one. Since FUNMAP expects one action server, this is likely where you’re seeing errors. If you could you provide more information on how you’re using FUNMAP and what errors you are seeing, I can help you make FUNMAP work in the Gazebo simulation.

Hi,

Here is the launch file that I am using

<launch>

  <arg name="debug_directory" value="/home/burak/stretch-gazebo-debug/"/>

  <remap to="/realsense/depth/color/points" from="/camera/depth/color/points" />
  <remap to="/stretch_diff_drive_controller/odom" from="/odom" />
  <remap to="/stretch_diff_drive_controller/cmd_vel" from="/stretch/cmd_vel" />
  <remap to="/joint_states" from="/stretch/joint_states" />

  <!-- REALSENSE D435i -->
  <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
  <!-- STRETCH DRIVER -->
  

  <!-- MAPPING -->
  <!-- example of args for funmap that loads a map on launch (should have double hyphen before load_map) -->
  <!-- load_map FILENAME -->
  <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen" >
    <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
  </node>
  <!-- -->

  <!-- IMU FILTER -->

  <!-- ROBOT LOCALIZATION FILTER -->
  <include file="$(find stretch_core)/launch/stretch_ekf.launch" />
  <!-- -->

  <!-- LASER RANGE FINDER -->
  
  <!-- LASER SCAN MATCHER FOR ODOMETRY -->
  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
  <!-- -->

  <!-- KEYBOARD TELEOP -->
  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on'/>
  <!-- -->

  <!-- VISUALIZE -->
  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_funmap)/rviz/stretch_mapping.rviz" />
  <!-- -->    
</launch>

I removed the launches of some of the packages since their published topics are already provided with the Gazebo server. I also added some re-mappings so that what FUNMAP and Keyboard teleop expect as topic names are provided (They may not be correct though). But I could not figure out how to map for action_topics. With this launch file I get the following output in the terminal while the gazebo server is running.

burak@buraka-ubuntu18:~$ roslaunch stretch_funmap mapping_gazebo_3D_FUNMAP.launch 
... logging to /home/burak/.ros/log/e2937d2e-0b58-11ec-949d-342eb70b42a3/roslaunch-buraka-ubuntu18-9620.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://buraka-ubuntu18:36029/

SUMMARY
========

CLEAR PARAMETERS
 * /ekf_se/

PARAMETERS
 * /ekf_se/acceleration_gains: [0.8, 0.0, 0.0, 0...
 * /ekf_se/acceleration_limits: [1.3, 0.0, 0.0, 0...
 * /ekf_se/base_link_frame: base_link
 * /ekf_se/control_config: [True, False, Fal...
 * /ekf_se/control_timeout: 0.2
 * /ekf_se/debug: False
 * /ekf_se/debug_out_file: /path/to/debug/fi...
 * /ekf_se/deceleration_gains: [1.0, 0.0, 0.0, 0...
 * /ekf_se/deceleration_limits: [1.3, 0.0, 0.0, 0...
 * /ekf_se/frequency: 15
 * /ekf_se/imu0: /imu/data
 * /ekf_se/imu0_config: [False, False, Fa...
 * /ekf_se/imu0_differential: True
 * /ekf_se/imu0_nodelay: False
 * /ekf_se/imu0_queue_size: 1
 * /ekf_se/imu0_relative: True
 * /ekf_se/imu0_remove_gravitational_acceleration: True
 * /ekf_se/initial_estimate_covariance: ['1e-9', 0, 0, 0,...
 * /ekf_se/map_frame: map
 * /ekf_se/odom0: /odom
 * /ekf_se/odom0_config: [True, True, True...
 * /ekf_se/odom0_differential: True
 * /ekf_se/odom0_nodelay: False
 * /ekf_se/odom0_queue_size: 1
 * /ekf_se/odom0_relative: False
 * /ekf_se/odom_frame: odom
 * /ekf_se/print_diagnostics: True
 * /ekf_se/process_noise_covariance: [0.05, 0, 0, 0, 0...
 * /ekf_se/publish_acceleration: False
 * /ekf_se/publish_tf: False
 * /ekf_se/sensor_timeout: 0.2
 * /ekf_se/stamped_control: False
 * /ekf_se/transform_time_offset: 0.0
 * /ekf_se/transform_timeout: 0.0
 * /ekf_se/two_d_mode: True
 * /ekf_se/use_control: False
 * /ekf_se/world_frame: odom
 * /funmap/debug_directory: /home/burak/stret...
 * /laser_scan_matcher_node/fixed_frame: odom
 * /laser_scan_matcher_node/max_angular_correction_deg: 20.0
 * /laser_scan_matcher_node/max_iterations: 10
 * /laser_scan_matcher_node/max_linear_correction: 0.2
 * /laser_scan_matcher_node/use_imu: False
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /
    ekf_se (robot_localization/ekf_localization_node)
    funmap (stretch_funmap/funmap)
    keyboard_teleop (stretch_core/keyboard_teleop)
    laser_scan_matcher_node (laser_scan_matcher/laser_scan_matcher_node)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[funmap-1]: started with pid [9659]
process[ekf_se-2]: started with pid [9660]
process[laser_scan_matcher_node-3]: started with pid [9661]
process[keyboard_teleop-4]: started with pid [9663]
process[rviz-5]: started with pid [9672]
[ INFO] [1630535311.422481104]: Starting LaserScanMatcher
[ WARN] [1630535311.466753294]: Both imu0_differential and imu0_relative were set to true. Using differential mode.
[ INFO] [1630535311.567803499]: rviz version 1.13.18
[ INFO] [1630535311.568057924]: compiled against Qt version 5.9.5
[ INFO] [1630535311.568077710]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1630535311.578442687]: Forcing OpenGl version 0.
[ WARN] [1630535311.653947446, 579.556000000]: Failed to meet update rate! Took 579.55600000000004002
[ WARN] [1630535311.654076949, 579.556000000]: Failed to meet update rate! Took 579.48933333299999049
[ INFO] [1630535312.433883962, 579.773000000]: Stereo is NOT SUPPORTED
[ INFO] [1630535312.434011378, 579.773000000]: OpenGL device: NVIDIA GeForce GTX 1650 Ti with Max-Q Design/PCIe/SSE2
[ INFO] [1630535312.434053340, 579.773000000]: OpenGl version: 4.6 (GLSL 4.6).
[ERROR] [1630535312.554818803, 579.797000000]: PluginlibFactory: The plugin for class 'rviz_imu_plugin/Imu' failed to load.  Error: According to the loaded plugin descriptions the class rviz_imu_plugin/Imu with base class type rviz::Display does not exist. Declared types are  moveit_rviz_plugin/MotionPlanning moveit_rviz_plugin/PlanningScene moveit_rviz_plugin/RobotState moveit_rviz_plugin/Trajectory rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/WrenchStamped rviz_plugin_tutorials/Imu
[ERROR] [1630535312.555236815, 579.797000000]: PluginlibFactory: The plugin for class 'rviz_imu_plugin/Imu' failed to load.  Error: According to the loaded plugin descriptions the class rviz_imu_plugin/Imu with base class type rviz::Display does not exist. Declared types are  moveit_rviz_plugin/MotionPlanning moveit_rviz_plugin/PlanningScene moveit_rviz_plugin/RobotState moveit_rviz_plugin/Trajectory rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/WrenchStamped rviz_plugin_tutorials/Imu
[INFO] [1630535312.855876, 0.000000]: /keyboard_teleop started
[INFO] [1630535313.752910, 0.000000]: /funmap started
Traceback (most recent call last):
  File "/home/burak/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 1304, in <module>
    node.main()
  File "/home/burak/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 1206, in main
    hm.HelloNode.main(self, 'funmap', 'funmap')
  File "/home/burak/catkin_ws/src/stretch_ros/hello_helpers/src/hello_helpers/hello_misc.py", line 147, in main
    sys.exit()
NameError: global name 'sys' is not defined
[funmap-1] process has died [pid 9659, exit code 1, cmd /home/burak/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap /camera/depth/color/points:=/realsense/depth/color/points /odom:=/stretch_diff_drive_controller/odom /stretch/cmd_vel:=/stretch_diff_drive_controller/cmd_vel /stretch/joint_states:=/joint_states __name:=funmap __log:=/home/burak/.ros/log/e2937d2e-0b58-11ec-949d-342eb70b42a3/funmap-1.log].
log file: /home/burak/.ros/log/e2937d2e-0b58-11ec-949d-342eb70b42a3/funmap-1*.log

Hello @burakaksoy, in your output, FUNMAP is silently stalling as it waits for /stretch_controller/follow_joint_trajectory, the real robot’s action server to come online. To make this clearer, I’ve started a development branch called feature/funmap_simulation_updates, which prints out what services/servers a node is waiting for. This branch also includes a launch file that sets up lidar and odometry correctly.

remap-ing the simulated action servers’ topics will cause issues. For example, a call to the action server with all joints wouldn’t get distributed correctly to the five simulated action servers. You could try writing a node that hosts a /stretch_controller/follow_joint_trajectory action server and distributes the commands accordingly. This would require converting mobile base commands into velocity commands for the simulated Stretch’s mobile base controller.

One other hurdle with running FUNMAP on the Gazebo simulated Stretch is that FUNMAP monitors effort to detect contact. If you plan on using FUNMAP to perform manipulation within Gazebo, additional sensors will need to be added to the Gazebo URDF.