[ dynamixel_workbench_controllers] Controller bugfix

[mmurooka]

Fix two bugs in dynamixel_workbench_controllers.cpp.

• remove the iteration of position_cnt, which is unnecessary: 4c03ddf
• use correct ID in getPresentPosition function: 7ac5dcc

[robograffitti]

@yhna Check this PR, please.

[yhna]

Thank you for your contribution.
We will test this source code.

[JaehyunShim]

@mmurooka @yoshimalucky

As for the first commit, jnt_tra_msgs_ includes multiple points on the trajectory for multiple joints (two joints this case: pan, tilt), which means the current double iteration you mentioned is not redundant.

Also, I do not see any problems with the current code you suggested an alternative for in your second commit. (It can be rewritten as you suggested but what it does is basically the same as line 351-357)

Let us know if you have any other questions,
Ryan

[mmurooka]

Thanks for your comment. I'm still thinking this PR is correct, so could you check again...

As for the first commit, jnt_tra_msgs_ includes multiple points on the trajectory for multiple joints (two joints this case: pan, tilt), which means the current double iteration you mentioned is not redundant.

When trajectoryMsgCallback is called, it stores trajectory into jnt_tra_msg_ and sets is_moving_ true.
A function writeCallback is called every control cycle, and writes each trajectory point while jnt_tra_msg_ is true.
All trajectory points are finished to send and is_moving_ should be set false, when writeCallback is called jnt_tra_msg_->points.size() times. But in the current code, is_moving_ is set false when writeCallback is called jnt_tra_msg_->points.size() * jnt_tra_msg_->points[point_cnt].positions.size() times. This is not correct.

All joints command in single trajectory point are written in one writeCallback call:
https://github.com/ROBOTIS-GIT/dynamixel-workbench/blob/master/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp#L599-L600

Also, I do not see any problems with the current code you suggested an alternative for in your second commit. (It can be rewritten as you suggested but what it does is basically the same as line 351-357)

This fix is necessary when the the number or order of Dynamixels in dynamixel_ and dxl_name (argument of getPresentPosition) is not same.
This can be happen easily when you send the trajectory of partial sets of joints. Even if you send the trajectory of all joints, this problem can be happen. It is because the order of dynamixel_ is not defined. It does not always become same order with the dynamixel_info yaml file.