Segfault: sim_joint_ out of range

[Briancbn]

Bug Report

I have a multiple position joint interface setup, mainly porting over what ur_description(melodic-staging branch) have in ROS 1. I have encounter the following error. Here is my gdb log. This is using latest on #34

[INFO] [launch]: All log files can be found below /home/cypressiea/.ros/log/2020-10-20-09-52-41-898669-cypressiea-v-306273
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [gzserver-1]: process started with pid [306275]
[gzserver-1] GNU gdb (Ubuntu 9.2-0ubuntu1~20.04) 9.2
[gzserver-1] Copyright (C) 2020 Free Software Foundation, Inc.
[gzserver-1] License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
[gzserver-1] This is free software: you are free to change and redistribute it.
[gzserver-1] There is NO WARRANTY, to the extent permitted by law.
[gzserver-1] Type "show copying" and "show warranty" for details.
[gzserver-1] This GDB was configured as "x86_64-linux-gnu".
[gzserver-1] Type "show configuration" for configuration details.
[gzserver-1] For bug reporting instructions, please see:
[gzserver-1] <http://www.gnu.org/software/gdb/bugs/>.
[gzserver-1] Find the GDB manual and other documentation resources online at:
[gzserver-1]     <http://www.gnu.org/software/gdb/documentation/>.
[gzserver-1] 
[gzserver-1] For help, type "help".
[gzserver-1] Type "apropos word" to search for commands related to "word"...
[gzserver-1] Reading symbols from gzserver...
[gzserver-1] (No debugging symbols found in gzserver)
[gzserver-1] Starting program: /usr/bin/gzserver --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so
[gzserver-1] [Thread debugging using libthread_db enabled]
[gzserver-1] Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[gzserver-1] [New Thread 0x7fffde9a3700 (LWP 306283)]
[gzserver-1] Gazebo multi-robot simulator, version 11.2.0
[gzserver-1] Copyright (C) 2012 Open Source Robotics Foundation.
[gzserver-1] Released under the Apache 2 License.
[gzserver-1] http://gazebosim.org
[gzserver-1] 
[gzserver-1] [New Thread 0x7fffddb75700 (LWP 306284)]
...
...
[gzserver-1] [New Thread 0x7ffe9ffff700 (LWP 306379)]
[gzserver-1] [INFO] [1603158768.493129392] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-1] [INFO] [1603158768.494957603] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-1] [INFO] [1603158768.495054374] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-1] [ERROR] [1603158768.496429109] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-1] [ERROR] [1603158768.496445350] [gazebo_ros2_control]: param_name robot_description
[gzserver-1] [ERROR] [1603158768.497058957] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-1] Parsing robot urdf xml string.
[gzserver-1] [INFO] [1603158768.605069728] [gazebo_ros2_control]: Loading joint 'shoulder_pan_joint' of type 'hardware_interface/PositionJointInterface'
[gzserver-1] 
[gzserver-1] Thread 93 "gzserver" received signal SIGSEGV, Segmentation fault.
[gzserver-1] [Switching to Thread 0x7ffef4ff5700 (LWP 306378)]
[gzserver-1] 0x00007ffff6b26c2e in gazebo::physics::Joint::Position(unsigned int) const ()
[gzserver-1]    from /lib/x86_64-linux-gnu/libgazebo_physics.so.11

Proposed Solution

I managed to resolve the error by changing the following part
https://github.com/ros-simulation/gazebo_ros2_control/blob/a6030db928b6fcecff636769ebe9d9eca1f1fee5/gazebo_ros2_control/src/default_robot_hw_sim.cpp#L302-L304
to

 for (unsigned int j = 0; j < sim_joints_.size(); j++) { 
   // Gazebo has an interesting API... 
   double position = sim_joints_[j]->Position(0); 

It seems that unlike other class variables, sim_joints_ is not resized at the start and is created through push_pack. I am not sure this is the best solution.

[guru-florida]

@Briancbn Just found this one too. There is a readSim() call in initSim() that needs to be moved outside the loop to the bottom. I.e. all joint setup needs to complete before that initial readSim() takes place. Comment in PR #34.

[ahcorde]

@Briancbn fixed in the PR.

Let me know if this fix the problem

[Briancbn]

Thanks for the quick fix, I have verified it works. Really appreciated! I am closing this.