Merge pull request #1225 from tue-robotics/fix/open_gripper_remove_object

check goal not state

Author: PetervDooren

robot_skills/src/robot_skills/arm/gripper.py

@@ -103,10 +103,10 @@ def send_goal(self, state, timeout=5.0, max_torque=0.1): #Todo: should send goal
 
         self._ac_gripper.send_goal(goal)
 
-        if state == GripperState.OPEN:
+        if goal.command.direction == GripperState.OPEN:
             if self.occupied_by is not None:
                 rospy.logerr("ParrallelGripper.send_goal open is called but there is still an entity with id '%s' \
-                occupying the gripper, please update the world model and remove this entity" % self.occupied_by.uuid)
+                occupying the gripper, removing this entity from the robot interface." % self.occupied_by.uuid)
             self.occupied_by = None
 
         goal_status = GoalStatus.SUCCEEDED