Unable to Read position and velocity of Dynamixel Motors to pass via ROS

[amaheshwari2509]

Hello,

I am providing the actuation to three motors to my robotics system (custom changed of Waffle PI) from publisher node (designed in ROS) using /cmd_vel topic over twist message. This part is successfully achieved. Further, I am NOT able to read the "present position and velocity (Control Table Address : 128 & 132)" which is to be used as a sensor feedback for my control algorithm. My goals is to receive these values (from Motor Driver.cpp) and pass onto custom-designed Publisher node (turtlebot3_core.ino). While uploading my code via Arduino IDE, I am not receiving any compiler error and upload is successful. I am writing position to third motor and writing velocity to first and second motor. I want to read position and velocity of all the three motors.

Note : I am able to read the values successfully via Matlab-Dynamixel SDK and Workbench.

Step 1 : I have written read position and velocity method based on control table and on same semantics of read encoder.

Step 2 and 3: Made a topic (with Twist message type) and passing values from read position and velocity (step 1) in this message. Further calling the method in loop. (Upload successful with no compiler errors)

Step 4 (following image): I am receiving zero values of position and velocity ("else condition" defined in step 3) . I believe it means methods defined in motor driver unable to read dynamixel motor (dxl_comm_result_pos and dxl_comm_result_vel are false even after calling read position and read velocity methods). It is given by left image of terminal snapshot displaying all zero values.

I asked this query to know how to received desired output (in the position and velocity ranges defined by control table) shown in right side of following image.

Currently I am doing from MATLAB Dynamixel SDK sucesfully ( receiving position and velocity in desired ranges as per the measured value) but I wish to do it from ROS (C++). Looking forward to your response.

Thanks
Best Regards