diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index b60ca3c..bbbf7fd 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -182,12 +182,13 @@ class HelloNode: assert(self._point_cloud is not None) return self._point_cloud - def get_joint_state(self, joint_name): + def get_joint_state(self, joint_name, moving_threshold=0.001): i = self._joint_states.name.index(joint_name) joint_position = self._joint_states.position[i] joint_velocity = self._joint_states.velocity[i] joint_effort = self._joint_states.effort[i] - return (joint_position, joint_velocity, joint_effort) + joint_is_moving = abs(joint_velocity) > moving_threshold + return (joint_position, joint_velocity, joint_effort, joint_is_moving) def get_tool(self): assert(self._tool is not None)