@ijnek can you have a look at this?

I found a bug with this code. It doe solve the Port in Use bug. But it also means we can't read the current joint position when we active the plugin. As the reset_command() is called before activated_ = true; is set. Hence we can't read any values from the servo motor. This is the output from the this: joints_[i].command.position = joints_[i].state.position;

joints_[i].state.position: nan

I found a bug with this code. It doe solve the Port in Use bug. But it also means we can't read the current joint position when we active the plugin. As the reset_command() is called before activated_ = true; is set. Hence we can't read any values from the servo motor. This is the output from the this: joints_[i].command.position = joints_[i].state.position;

joints_[i].state.position: nan

aah, missed that. do you have any solution for the same?