|
@ -182,12 +182,13 @@ class HelloNode: |
|
|
assert(self._point_cloud is not None) |
|
|
assert(self._point_cloud is not None) |
|
|
return self._point_cloud |
|
|
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) |
|
|
i = self._joint_states.name.index(joint_name) |
|
|
joint_position = self._joint_states.position[i] |
|
|
joint_position = self._joint_states.position[i] |
|
|
joint_velocity = self._joint_states.velocity[i] |
|
|
joint_velocity = self._joint_states.velocity[i] |
|
|
joint_effort = self._joint_states.effort[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): |
|
|
def get_tool(self): |
|
|
assert(self._tool is not None) |
|
|
assert(self._tool is not None) |
|
|