diff --git a/src/devices/controlBoard_nws_ros2/ControlBoard_nws_ros2_callbacks.cpp b/src/devices/controlBoard_nws_ros2/ControlBoard_nws_ros2_callbacks.cpp index fb24c72..6511b9f 100644 --- a/src/devices/controlBoard_nws_ros2/ControlBoard_nws_ros2_callbacks.cpp +++ b/src/devices/controlBoard_nws_ros2/ControlBoard_nws_ros2_callbacks.cpp @@ -267,7 +267,7 @@ void ControlBoard_nws_ros2::positionDirectTopic_callback(const yarp_control_msgs std::vector convertedPos; std::vector selectedJoints; - for(size_t i=0; ipositions.size(); i++){ + for(size_t i=0; i<(noJoints ? m_subdevice_joints : msg->positions.size()); i++){ size_t index = noJoints ? i : m_quickJointRef[msg->names[i]]; if(!noJoints) {selectedJoints.push_back(index);} m_iAxisInfo->getJointType(index, jType); @@ -314,7 +314,7 @@ void ControlBoard_nws_ros2::velocityTopic_callback(const yarp_control_msgs::msg: std::vector selectedJoints; std::vector convertedAccel; - for(size_t i=0; ivelocities.size(); i++){ + for(size_t i=0; i<(noJoints ? m_subdevice_joints : msg->velocities.size()); i++){ size_t index = noJoints ? i : m_quickJointRef[msg->names[i]]; if(!noJoints) {selectedJoints.push_back(index);} m_iAxisInfo->getJointType(index, jType);