Merge pull request #719 from elandini84/fix_segfault_yarprobotstatepu…

…blisher_onread bug-fix: JointIndex validity check
robotology · Aug 5, 2020 · 897291b · 897291b
2 parents ec2eb13 + 38ca326
commit 897291b
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
@@ -230,7 +230,9 @@ void YARPRobotStatePublisherModule::onRead(yarp::rosmsg::sensor_msgs::JointState
     {
         jntIndex = model.getJointIndex(v.name[i]);
 
-        if (!(model.getJoint(jntIndex)->getNrOfDOFs()) || jntIndex == iDynTree::JOINT_INVALID_INDEX)
+        if ( jntIndex == iDynTree::JOINT_INVALID_INDEX)
+            continue;
+        if (!(model.getJoint(jntIndex)->getNrOfDOFs()))
             continue;
 
         m_jointPos(model.getJoint(jntIndex)->getDOFsOffset()) = v.position[i];