def execute_cb(self, goal):
goto_position_goal = goto_positionGoal()
self.goto_position_client.wait_for_server()
self.detect_object_client.wait_for_server()
goto_position_goal = goto_positionGoal()
self.destination = goal.destination
self.calculate_waypoints()
idx= 0
while(self.detectedPos.pose.position.x == float("inf")):
goto_position_goal.destination.pose.position.x = self.waypoints.item((idx, 0))
goto_position_goal.destination.pose.position.y = self.waypoints.item((idx, 1))
goto_position_goal.destination.pose.position.z = self.waypoints.item((idx, 2))
self.goto_position_client.send_goal(goto_position_goal)
self.goto_position_client.wait_for_result()
idx = idx + 1
if idx == self.waypoints.shape[0]:
idx = 0
detect_object_goal = detect_objectGoal()
self.detect_object_client.send_goal(detect_object_goal)
self.detect_object_client.wait_for_result()
self.detectedPos = self.detect_object_client.get_result().detected_position
time.sleep(0.1)
if self.detectedPos.pose.position.x != float("inf"):
self.result.detected_object_pos = self.detectedPos
self.result.detected_object_pos.pose.position.z = self.searchHeight
self.action_server.set_succeeded(self.result)
def ascend_z_by(cls, z, pos):
goto_position_goal = goto_positionGoal()
goto_position_goal.destination.pose.position.x = pos.pose.position.x
goto_position_goal.destination.pose.position.y = pos.pose.position.y
goto_position_goal.destination.pose.position.z = pos.pose.position.z + z
cls.goto_position_client.send_goal(goto_position_goal)
return cls.goto_position_client.wait_for_result()
def fly_to_position(cls, x, y, z):
goto_position_goal = goto_positionGoal()
goto_position_goal.destination.pose.position.x = x
goto_position_goal.destination.pose.position.y = y
goto_position_goal.destination.pose.position.z = z
cls.goto_position_client.send_goal(goto_position_goal)
return cls.goto_position_client.wait_for_result()
class CPSVO2018:
detected = False # If the dead drone is detected by the Camera
mv_state = None
local_pose = PoseStamped() # Local position of the drone
# Things to consider deleting
goto_position_goal = goto_positionGoal()
detect_object_goal = detect_objectGoal()
goto_position_client = actionlib.SimpleActionClient(
'goto_position', goto_positionAction)
def __init__(self):
# Initialize variables and start some servers
rospy.init_node('action_controller') # Create a Node
rospy.loginfo('Setting offboard')
Flying.start_position_server() # Starting position server.
print("Init long grippers") # This will start the Longgrippersserver
LongGrippers.start_grippers()
print("Init done")
# End of Initializing
rospy.loginfo("Taking off")
d = Drone()
d.start() # This will perform the TakeOff action.
while not rospy.is_shutdown():
d.do_next_state()
def __init__(self):
self.vel_control = rospy.Publisher('/position_control/set_velocity',
PoseStamped,
queue_size=10)
rospy.Subscriber('/position_control/distance', Bool,
self.distance_reached_cb)
self.mode_control = rospy.Publisher('/position_control/set_mode',
String,
queue_size=10)
print('Setting offboard')
mv_state = mavros_state.mavros_state()
mv_state.set_mode('OFFBOARD')
print('Arming vehicle')
mv_state.arm(True)
rospy.loginfo("Taking off")
self.goto_position_client = actionlib.SimpleActionClient(
'goto_position', goto_positionAction)
# Will return True or False
self.goto_position_client.wait_for_server()
self.goto_position_goal = goto_positionGoal()
self.goto_position_goal.destination.pose.position.z = 3
self.goto_position_client.send_goal(self.goto_position_goal)
self.goto_position_client.wait_for_result()
self.des_pose = PoseStamped()
self.mode_control.publish('velctr')
rospy.sleep(0.1)
# Fly back and forth
print(str(PoseStamped))
self.des_pose.pose.position.x = 0
self.des_pose.pose.position.y = 1
self.des_pose.pose.position.z = 3
self.vel_control.publish(self.des_pose)
# while not self.target_reached:
rospy.sleep(20)
while True:
print(str(PoseStamped))
self.des_pose.pose.position.x = 0
self.des_pose.pose.position.y = -2
self.des_pose.pose.position.z = 3
self.vel_control.publish(self.des_pose)
# while not self.target_reached:
rospy.sleep(20)
print('IN WHILE222')
self.des_pose.pose.position.x = 0
self.des_pose.pose.position.y = 2
self.des_pose.pose.position.z = 3
self.vel_control.publish(self.des_pose)
# while not self.target_reached:
rospy.sleep(20)
print('IN WHILE333')
from simulation_control.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal, long_grippersAction, long_grippersGoal
from std_msgs.msg import Float32
if __name__ == '__main__':
rospy.init_node('action_controller')
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
mv_state.arm(True)
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")