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.")