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.
Hi,
I’m also interested in testing FUNMAP in the simulation environment. I’ve copied some of the code provided in this thread but I’m receiving an error I’m unsure of. At launch everything seems fine until there is a scan matching error. Here is the output after running the launch file you provided. The terminal then proceeds to print the last warning. I also don’t seem to see anything in Rviz. Is there anything I can do to fix this?
jterrazas@lab-pc-1:~$ roslaunch stretch_funmap mapping_gazebo.launch
... logging to /home/jterrazas/.ros/log/35f46220-088e-11ed-9e28-a4bb6dcdb919/roslaunch-lab-pc-1-5081.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://lab-pc-1:41185/
SUMMARY
========
PARAMETERS
* /funmap/debug_directory: ~/stretch_user//d...
* /gazebo/enable_ros_network: True
* /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: True
* /robot_description: <?xml version="1....
* /robot_state_publisher/publish_frequency: 30.0
* /rosdistro: melodic
* /rosversion: 1.14.13
* /stretch_arm_controller/allow_partial_joints_goal: True
* /stretch_arm_controller/constraints/action_monitor_rate: 10
* /stretch_arm_controller/constraints/goal_time: 0.6
* /stretch_arm_controller/constraints/joint_arm_l0/goal: 0.1
* /stretch_arm_controller/constraints/joint_arm_l0/trajectory: 0.1
* /stretch_arm_controller/constraints/joint_arm_l1/goal: 0.1
* /stretch_arm_controller/constraints/joint_arm_l1/trajectory: 0.1
* /stretch_arm_controller/constraints/joint_arm_l2/goal: 0.1
* /stretch_arm_controller/constraints/joint_arm_l2/trajectory: 0.1
* /stretch_arm_controller/constraints/joint_arm_l3/goal: 0.1
* /stretch_arm_controller/constraints/joint_arm_l3/trajectory: 0.1
* /stretch_arm_controller/constraints/joint_lift/goal: 0.1
* /stretch_arm_controller/constraints/joint_lift/trajectory: 0.1
* /stretch_arm_controller/constraints/joint_wrist_yaw/goal: 0.1
* /stretch_arm_controller/constraints/joint_wrist_yaw/trajectory: 0.1
* /stretch_arm_controller/constraints/state_publish_rate: 25
* /stretch_arm_controller/constraints/stop_trajectory_duration: 0.5
* /stretch_arm_controller/constraints/stopped_velocity_tolerance: 0.05
* /stretch_arm_controller/joints: ['joint_lift', 'j...
* /stretch_arm_controller/type: position_controll...
* /stretch_diff_drive_controller/angular/z/has_acceleration_limits: True
* /stretch_diff_drive_controller/angular/z/has_velocity_limits: True
* /stretch_diff_drive_controller/angular/z/max_acceleration: 6.0
* /stretch_diff_drive_controller/angular/z/max_velocity: 2.0
* /stretch_diff_drive_controller/base_frame_id: base_link
* /stretch_diff_drive_controller/left_wheel: ['joint_left_wheel']
* /stretch_diff_drive_controller/linear/x/has_acceleration_limits: True
* /stretch_diff_drive_controller/linear/x/has_velocity_limits: True
* /stretch_diff_drive_controller/linear/x/max_acceleration: 1.0
* /stretch_diff_drive_controller/linear/x/max_velocity: 0.5
* /stretch_diff_drive_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
* /stretch_diff_drive_controller/publish_rate: 50
* /stretch_diff_drive_controller/right_wheel: ['joint_right_whe...
* /stretch_diff_drive_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
* /stretch_diff_drive_controller/type: diff_drive_contro...
* /stretch_diff_drive_controller/wheel_radius: 0.05
* /stretch_diff_drive_controller/wheel_separation: 0.315
* /stretch_gripper_controller/allow_partial_joints_goal: True
* /stretch_gripper_controller/constraints/action_monitor_rate: 10
* /stretch_gripper_controller/constraints/goal_time: 0.6
* /stretch_gripper_controller/constraints/joint_gripper_finger_left/goal: 0.1
* /stretch_gripper_controller/constraints/joint_gripper_finger_left/trajectory: 0.1
* /stretch_gripper_controller/constraints/joint_gripper_finger_right/goal: 0.1
* /stretch_gripper_controller/constraints/joint_gripper_finger_right/trajectory: 0.1
* /stretch_gripper_controller/constraints/state_publish_rate: 25
* /stretch_gripper_controller/constraints/stop_trajectory_duration: 0.5
* /stretch_gripper_controller/constraints/stopped_velocity_tolerance: 0.05
* /stretch_gripper_controller/joints: ['joint_gripper_f...
* /stretch_gripper_controller/type: position_controll...
* /stretch_head_controller/allow_partial_joints_goal: True
* /stretch_head_controller/constraints/action_monitor_rate: 10
* /stretch_head_controller/constraints/goal_time: 0.6
* /stretch_head_controller/constraints/joint_head_pan/goal: 0.1
* /stretch_head_controller/constraints/joint_head_pan/trajectory: 0.1
* /stretch_head_controller/constraints/joint_head_tilt/goal: 0.1
* /stretch_head_controller/constraints/joint_head_tilt/trajectory: 0.1
* /stretch_head_controller/constraints/state_publish_rate: 25
* /stretch_head_controller/constraints/stop_trajectory_duration: 0.5
* /stretch_head_controller/constraints/stopped_velocity_tolerance: 0.05
* /stretch_head_controller/joints: ['joint_head_pan'...
* /stretch_head_controller/type: position_controll...
* /stretch_joint_state_controller/publish_rate: 50
* /stretch_joint_state_controller/type: joint_state_contr...
* /use_sim_time: True
NODES
/
d435i_frustum_visualizer (stretch_core/d435i_frustum_visualizer)
funmap (stretch_funmap/funmap)
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
keyboard_teleop (stretch_core/keyboard_teleop)
laser_scan_matcher_node (laser_scan_matcher/laser_scan_matcher_node)
publish_ground_truth_odom (stretch_gazebo/publish_ground_truth_odom.py)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz (rviz/rviz)
stretch_controller_spawner (controller_manager/spawner)
urdf_spawner (stretch_gazebo/spawn_model)
auto-starting new master
process[master]: started with pid [5148]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to 35f46220-088e-11ed-9e28-a4bb6dcdb919
process[rosout-1]: started with pid [5186]
started core service [/rosout]
process[gazebo-2]: started with pid [5193]
process[gazebo_gui-3]: started with pid [5198]
process[urdf_spawner-4]: started with pid [5203]
process[robot_state_publisher-5]: started with pid [5204]
process[stretch_controller_spawner-6]: started with pid [5205]
process[publish_ground_truth_odom-7]: started with pid [5206]
process[d435i_frustum_visualizer-8]: started with pid [5212]
[ WARN] [1658364241.471893195]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
process[funmap-9]: started with pid [5213]
process[laser_scan_matcher_node-10]: started with pid [5216]
process[keyboard_teleop-11]: started with pid [5219]
process[rviz-12]: started with pid [5225]
[ INFO] [1658364241.509193129]: Starting LaserScanMatcher
[ INFO] [1658364241.673122607]: rviz version 1.13.24
[ INFO] [1658364241.673175677]: compiled against Qt version 5.9.5
[ INFO] [1658364241.673347610]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1658364241.682050854]: Forcing OpenGl version 0.
Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[Wrn] [GuiIface.cc:199] g/gui-plugin is really loading a SystemPlugin. To load a GUI plugin please use --gui-client-plugin
Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[ INFO] [1658364241.884914742]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1658364241.886923939]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.109.20.120
[ INFO] [1658364241.898091436]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1658364241.899584206]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.109.20.120
[INFO] [1658364241.967511, 0.000000]: /d435i_frustum_visualizer started
[INFO] [1658364242.279989, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1658364242.304550, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[INFO] [1658364242.352597, 0.000000]: /keyboard_teleop started
[INFO] [1658364242.907161, 0.000000]: /funmap started
[ INFO] [1658364243.573734506]: Stereo is NOT SUPPORTED
[ INFO] [1658364243.573867087]: OpenGL device: Quadro P620/PCIe/SSE2
[ INFO] [1658364243.573914580]: OpenGl version: 4.6 (GLSL 4.6).
[ INFO] [1658364244.388997691]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[WARN] [1658364244.403985, 0.000000]: Waiting 4.00 seconds to unpause physics
[INFO] [1658364244.420099, 0.000000]: Calling service /gazebo/spawn_urdf_model
[ INFO] [1658364244.440605387]: Physics dynamic reconfigure ready.
[Err] [ModelDatabase.cc:235] No <database> tag in the model database database.config found here[http://gazebosim.org/models/]
[Err] [ModelDatabase.cc:294] Unable to download model manifests
[ INFO] [1658364244.750799134]: <initialOrientationAsReference> set to: 0
[ INFO] [1658364244.750889335]: <robotNamespace> set to: //
[ INFO] [1658364244.750920383]: <topicName> set to: //imu/data
[ INFO] [1658364244.750946921]: <frameName> set to: base_link
[ INFO] [1658364244.750986255]: <updateRateHZ> set to: 25
[ INFO] [1658364244.751021348]: <gaussianNoise> set to: 0.001
[ INFO] [1658364244.751054161]: <xyzOffset> set to: 0 0 0
[ INFO] [1658364244.751103506]: <rpyOffset> set to: 0 -0 0
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
[ INFO] [1658364245.102942969]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1658364245.102988018]: Starting Laser Plugin (ns = /)
[ INFO] [1658364245.104958843]: Laser Plugin (ns = /) <tf_prefix_>, set to ""
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
[ INFO] [1658364245.270371923]: <initialOrientationAsReference> set to: 0
[ INFO] [1658364245.270453108]: <robotNamespace> set to: //
[ INFO] [1658364245.270481537]: <topicName> set to: //camera/imu/data
[ INFO] [1658364245.270511534]: <frameName> set to: camera_gyro_frame
[ INFO] [1658364245.270547371]: <updateRateHZ> set to: 50
[ INFO] [1658364245.270581558]: <gaussianNoise> set to: 0.001
[ INFO] [1658364245.270620014]: <xyzOffset> set to: 0 0 0
[ INFO] [1658364245.270656019]: <rpyOffset> set to: 0 -0 0
[ INFO] [1658364245.285612974]: <initialOrientationAsReference> set to: 0
[ INFO] [1658364245.285656205]: <robotNamespace> set to: //
[ INFO] [1658364245.285669493]: <topicName> set to: //wrist_imu/data
[ INFO] [1658364245.285681929]: <frameName> set to: link_wrist_yaw
[ INFO] [1658364245.285706603]: <updateRateHZ> set to: 25
[ INFO] [1658364245.285723322]: <gaussianNoise> set to: 0.001
[ INFO] [1658364245.285738522]: <xyzOffset> set to: 0 0 0
[ INFO] [1658364245.285753828]: <rpyOffset> set to: 0 -0 0
[Wrn] [msgs.cc:1852] Conversion of sensor type[depth] not supported.
[INFO] [1658364245.292263, 0.000000]: Spawn status: SpawnModel: Successfully spawned entity
[INFO] [1658364245.294825, 0.000000]: Waiting for service /gazebo/set_model_configuration
[INFO] [1658364245.298848, 0.000000]: temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.
[WARN] [1658364245.407777, 0.000000]: Waiting 3.00 seconds to unpause physics
[ INFO] [1658364246.159357527]: Loading gazebo_ros_control plugin
[ INFO] [1658364246.159442830]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1658364246.160399620]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1658364246.286813897]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_right_wheel
[ERROR] [1658364246.287754425]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_left_wheel
[ERROR] [1658364246.288781136]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_lift
[ERROR] [1658364246.289527295]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_arm_l0
[ERROR] [1658364246.290127404]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_arm_l1
[ERROR] [1658364246.290722391]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_arm_l2
[ERROR] [1658364246.291289803]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_arm_l3
[ERROR] [1658364246.291815204]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_wrist_yaw
[ERROR] [1658364246.292336696]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_head_pan
[ERROR] [1658364246.292881329]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_head_tilt
[ERROR] [1658364246.293423928]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_gripper_finger_left
[ERROR] [1658364246.293962864]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/joint_gripper_finger_right
[ INFO] [1658364246.298696621]: Loaded gazebo_ros_control.
[INFO] [1658364246.301672, 0.000000]: Calling service /gazebo/set_model_configuration
[INFO] [1658364246.307689, 0.000000]: Set model configuration status: SetModelConfiguration: success
[ INFO] [1658364246.323401098]: Realsense Gazebo ROS plugin loading.
RealSensePlugin: The realsense_camera plugin is attach to model robot
[WARN] [1658364246.411030, 0.000000]: Waiting 1.99 seconds to unpause physics
[ INFO] [1658364246.453484327]: Controller state will be published at 50Hz.
[ INFO] [1658364246.457013037]: Wheel separation will be multiplied by 1.
[ INFO] [1658364246.460264629]: Left wheel radius will be multiplied by 1.
[ INFO] [1658364246.460452377]: Right wheel radius will be multiplied by 1.
[ INFO] [1658364246.461622527]: Velocity rolling window size of 10.
[ INFO] [1658364246.462670141]: Velocity commands will be considered old if they are older than 0.5s.
[ INFO] [1658364246.463232986]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1658364246.465218233]: Base frame_id set to base_link
[ INFO] [1658364246.465987094]: Odometry frame_id set to odom
[ INFO] [1658364246.466941206]: Publishing to tf is enabled
[ INFO] [1658364246.491682061]: Odometry params : wheel separation 0.315, left wheel radius 0.05, right wheel radius 0.05
[ INFO] [1658364246.498020376]: Adding left wheel with joint name: joint_left_wheel and right wheel with joint name: joint_right_wheel
[ INFO] [1658364246.520420125]: Dynamic Reconfigure:
DynamicParams:
Odometry parameters:
left wheel radius multiplier: 1
right wheel radius multiplier: 1
wheel separation multiplier: 1
Publication parameters:
Publish executed velocity command: disabled
Publication rate: 50
Publish frame odom on tf: enabled
[urdf_spawner-4] process has finished cleanly
log file: /home/jterrazas/.ros/log/35f46220-088e-11ed-9e28-a4bb6dcdb919/urdf_spawner-4*.log
[Wrn] [msgs.cc:1852] Conversion of sensor type[depth] not supported.
[Wrn] [msgs.cc:1852] Conversion of sensor type[depth] not supported.
[Wrn] [msgs.cc:1852] Conversion of sensor type[depth] not supported.
[WARN] [1658364247.416035, 0.000000]: Waiting 0.99 seconds to unpause physics
[WARN] [1658364248.426666, 0.001000]: Waiting for robot to spawn to publish ground truth odometry
:err: Valid: 0/2000 invalid: 2000.
[ WARN] [1658364248.728191365, 0.184000000]: Error in scan matching
:err: Valid: 0/2000 invalid: 2000.
[ WARN] [1658364249.005607170, 0.365000000]: Error in scan matching
:err: Valid: 0/2000 invalid: 2000.
[ WARN] [1658364249.285916785, 0.547000000]: Error in scan matching
:err: Valid: 0/2000 invalid: 2000.
[ WARN] [1658364249.580339986, 0.729000000]: Error in scan matching
:err: Valid: 0/2000 invalid: 2000.
Hi @jterraz, welcome to the forum! The launch file you’re using is actually from a work-in-progress branch I created to solve some of the initial issues I encountered with using FUNMAP in simulation. It’s not finished which is why you’re seeing these scan matching errors. If you’re interested in getting it working, here’s my suggestion for tackling the scan matching errors.
I would start by bringing up just the Gazebo simulation of the robot using this launch file. Use rostopic to investigate the lidar topic. Then launch the laser_scan_matcher launch file in a separate terminal and tweak the launch file until the scan matching errors disappear. You can then incorporate your findings into the mapping_gazebo.launch file from FUNMAP.
In my post above, I outlined some additional challenges left to tackle in order to get FUNMAP running in simulation. Let me know if you have any questions!