position_control crashes

[Lukaszm94]

Hello,
I want to control three Dynamixel servos (MX-106, MX-64, MX-28) using position_control.
But before controlling 3 servos, I wanted to test the code with one servo (MX-64).
Parameters: baud 115200, ID=4, firmware version 39 (protocol v1.0).
I compiled dynamixel-workbench from source and when I run the following launch file:

<launch>
	<arg name="device_name"                default="/dev/ttyUSB0"/>
	<arg name="baud_rate"                  default="115200"/>
	<arg name="protocol_version"           default="1.0"/>
	
	<arg name="scan_range"                 default="10"/>

	<arg name="profile_velocity"           default="200"/>
	<arg name="profile_acceleration"       default="50"/>

	<param name="device_name"              value="$(arg device_name)"/>
	<param name="baud_rate"                value="$(arg baud_rate)"/>
	<param name="protocol_version"         value="$(arg protocol_version)"/>

	<param name="scan_range"               value="$(arg scan_range)"/>

	<param name="profile_velocity"         value="$(arg profile_velocity)"/>
	<param name="profile_acceleration"     value="$(arg profile_acceleration)"/>

	<node name="position_control" pkg="dynamixel_workbench_controllers" type="position_control" required="true" output="screen" />
</launch>

an error shows up

luke@Ubuntu16:~/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws$ roslaunch my_dynamixel_workbench_tutorial position_control.launch 
... logging to /home/luke/.ros/log/8001fd30-02a5-11e8-896c-c04a0013df1b/roslaunch-Ubuntu16-31362.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Ubuntu16:44787/

SUMMARY
========

PARAMETERS
 * /baud_rate: 115200
 * /device_name: /dev/ttyUSB0
 * /profile_acceleration: 50
 * /profile_velocity: 200
 * /protocol_version: 1.0
 * /rosdistro: kinetic
 * /rosversion: 1.12.12
 * /scan_range: 10

NODES
  /
    position_control (dynamixel_workbench_controllers/position_control)

ROS_MASTER_URI=http://localhost:11311

process[position_control-1]: started with pid [31379]
-----------------------------------------------------------------------
  dynamixel_workbench controller; position control example             
-----------------------------------------------------------------------

MODEL   : MX-64
ID      : 4

-----------------------------------------------------------------------
================================================================================REQUIRED process [position_control-1] has died!
process has died [pid 31379, exit code -11, cmd /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/devel/lib/dynamixel_workbench_controllers/position_control __name:=position_control __log:=/home/luke/.ros/log/8001fd30-02a5-11e8-896c-c04a0013df1b/position_control-1.log].
log file: /home/luke/.ros/log/8001fd30-02a5-11e8-896c-c04a0013df1b/position_control-1*.log
Initiating shutdown!
================================================================================
[position_control-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done`

The console output does not give much info, so I compiled the source code in debug mode
catkin_make -DCMAKE_BUILD_TYPE=Debug
and ran in gdb. Output from gdb:

GNU gdb (Ubuntu 7.11.1-0ubuntu1~16.5) 7.11.1
Copyright (C) 2016 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.  Type "show copying"
and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
<http://www.gnu.org/software/gdb/documentation/>.
For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/devel/lib/dynamixel_workbench_controllers/position_control...done.
(gdb) run
Starting program: /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/devel/lib/dynamixel_workbench_controllers/position_control __name:=position_control __log:=/home/luke/.ros/log/8001fd30-02a5-11e8-896c-c04a0013df1b/position_control-1.log
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7ffff1639700 (LWP 31227)]
[New Thread 0x7ffff0e38700 (LWP 31228)]
[New Thread 0x7fffebfff700 (LWP 31229)]
[New Thread 0x7fffeb7fe700 (LWP 31234)]
-----------------------------------------------------------------------
  dynamixel_workbench controller; position control example             
-----------------------------------------------------------------------

MODEL   : MX-64
ID      : 4

-----------------------------------------------------------------------

Thread 1 "position_contro" received signal SIGSEGV, Segmentation fault.
0x00007ffff7bce8fb in DynamixelDriver::readRegister (this=0x6532f0, 
    id=4 '\004', item_name=0x428adb "Present_Velocity", 
    data=0x7ffff7dd6940 <DynamixelWorkbench::itemRead(unsigned char, char const*)::data>)
    at /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench/dynamixel_driver.cpp:438
438       if (cti->data_length == BYTE)
(gdb) backtrace
#0  0x00007ffff7bce8fb in DynamixelDriver::readRegister (this=0x6532f0, 
    id=4 '\004', item_name=0x428adb "Present_Velocity", 
    data=0x7ffff7dd6940 <DynamixelWorkbench::itemRead(unsigned char, char const*)::data>)
    at /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench/dynamixel_driver.cpp:438
#1  0x00007ffff7bd33ee in DynamixelWorkbench::itemRead (this=0x6532f0, 
    id=4 '\004', item_name=0x428adb "Present_Velocity")
    at /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_workbench/dynamixel_workbench.cpp:326
#2  0x000000000041e6f0 in PositionControl::dynamixelStatePublish (
    this=0x7fffffffd650)
    at /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/src/position_control.cpp:94
#3  0x000000000041e932 in PositionControl::controlLoop (this=0x7fffffffd650)
    at /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/src/position_control.cpp:106
#4  0x000000000041eaef in main (argc=1, argv=0x7fffffffd848)
    at /home/luke/WORK/DIAGSTAR/Software/ros_manipulator_pc/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/src/position_control.cpp:143
(gdb)

Do you know what causes this behaviour? How can I solve this problem? Thanks in advance,
Luke

[routiful]

Hi :)

Sorry for late comment.
I updated protocol combine function in dynamixel-workbench-toolbox.
You can download it in master branch.
After use it please comment how it works.

Thanks
Darby

[Lukaszm94]

Hey,
Thank you very much. Now it runs fine, I'm able to access and control one servo. I'll test on target system (3 servos) and let you know if it works.
By the way, right now in topic /dynamixel_state I'm getting 10.425Hz update rate. If I understand correctly from #53, with 3 servos I'll be getting around 3.3Hz update rate. That's really slow for my application. Is there any chance of adding changes from jysung@26fc2a2 to master branch (not exactly the code, but the idea of reading multiple registers with one transmission)?

Thanks so much for your help,
Luke

[routiful]

Hi :)

Because of compatibility for different protocol, 'Controller' is used 'read4byte' function without 'torque_control' example.
If you change 'read' to sync read', update rate will be better.

'torque_control' example to show how to use 'sync read' function.

Best regards
Darby

[Lukaszm94]

Ok, it works fine :)
Thanks for your time, this issue can be closed.