publish offset joint states

mikeferguson · Oct 23, 2020 · fcc1b6e · fcc1b6e
1 parent a2a59d8
commit fcc1b6e
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/robot_calibration/src/tools/viz.cpp b/robot_calibration/src/tools/viz.cpp
@@ -222,7 +222,13 @@ int main(int argc, char** argv)
     pub.publish(markers);
 
     // Publish the joint states
-    state.publish(data[i].joint_states);
+    sensor_msgs::JointState state_msg = data[i].joint_states;
+    for (size_t i = 0; i < state_msg.name.size(); ++i)
+    {
+      double offset = offsets.get(state_msg.name[i]);
+      state_msg.position[i] += offset;
+    }
+    state.publish(state_msg);
 
     // Wait to proceed
     std::cout << "Press enter to continue...";