def move_to_pos(cls, pos): rospy.loginfo("Moving long grippers to: " + str(pos)) long_grippers_goal = long_grippersGoal(grip_rad_goal=Float32(pos)) cls.long_grippers_client.send_goal(long_grippers_goal) cls.long_grippers_client.wait_for_result() if cls.long_grippers_client.get_result().goal_reached.data: rospy.loginfo("Long grippers moved to position") else: rospy.loginfo("Error with moving long grippers to position")
def move_to_pickup_pos(cls): print("Move Grippers to PickupPos") goalposition = long_grippersGoal(grip_rad_goal=Float32(0.50000000000)) cls.long_grippers_client.send_goal(goalposition) s = cls.long_grippers_client.wait_for_result() if s: print("Grippers moved to pickup pos") return s else: print("Grippers didn't move to pickup.") return s
def close_grippers(cls): print("Closing grippers...") goalposition = long_grippersGoal(grip_rad_goal=Float32(1.57000000000)) cls.long_grippers_client.send_goal(goalposition) s = cls.long_grippers_client.wait_for_result() if s: print("Grippers Closed all the way") return s else: print("Grippers didn't close.") return s
def open_grippers(cls): print("Open grippers...") goalposition = long_grippersGoal(grip_rad_goal=Float32(0.00000000000)) cls.long_grippers_client.send_goal(goalposition) s = cls.long_grippers_client.wait_for_result() if s: print("Grippers Closed") return s else: print("Grippers didn't close.") return s
rospy.loginfo("Taking off") goto_position_client = actionlib.SimpleActionClient( 'goto_position', goto_positionAction) goto_position_client.wait_for_server() goto_position_goal = goto_positionGoal() goto_position_goal.destination.pose.position.z = 5 goto_position_client.send_goal(goto_position_goal) goto_position_client.wait_for_result() rospy.loginfo("Takeoff succeded") # Close legs long_grippers_client = actionlib.SimpleActionClient( 'long_grippers', long_grippersAction) long_grippers_client.wait_for_server() rospy.loginfo("Moving legs to pickup position") long_grippers_goal = long_grippersGoal(grip_rad_goal=Float32(0.5000000000)) long_grippers_client.send_goal(long_grippers_goal) long_grippers_client.wait_for_result() if long_grippers_client.get_result().goal_reached.data: rospy.loginfo("Legs moved to pickup position") else: rospy.loginfo("Error with moving legs to pickup position") # /Close legs goto_position_goal.destination.pose.position.x = -105 goto_position_goal.destination.pose.position.y = 0 goto_position_goal.destination.pose.position.z = 3 goto_position_client.send_goal(goto_position_goal) goto_position_client.wait_for_result() rospy.loginfo("Arrived at search position.")