class quad_obj: quad_pose = TransformStamped() quad_pose_x = TransformStamped() quad_pose_y = TransformStamped() quad_pose_z = TransformStamped() state = State() quad_goal = PoseStamped() offb_set_mode = SetMode() arm_cmd = CommandBool() takeoff_cmd = CommandTOL() landing_cmd = CommandTOL()
class quad_obj: vehicle = Point() float_x = float() float_y = float() float_z = float() vehicle_dot = Vector3() float_xdot = float() float_ydot = float() float_zdot = float() state = State() quad_goal = PoseStamped() offb_set_mode = SetMode() arm_cmd = CommandBool() takeoff_cmd = CommandTOL() landing_cmd = CommandTOL()
def constructCommandTOL(self, x, y, z, yaw, min_pitch=0):
command = CommandTOL()
command.min_pitch = min_pitch
command.latitude = x
command.longitude = y
command.altitude = z
command.yaw = yaw
return command
# following replanning path
'''
if current_state.mode == "OFFBOARD" and current_state.armed and rospy.Time.now() - last_request > rospy.Duration(2):
if len(waypoint) < 3:
break
print "move next"
pose.pose.position.x = waypoint[2][0]
pose.pose.position.y = waypoint[2][1]
pose.pose.position.z = 2
last_request = rospy.Time.now()
'''
print("Return")
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 0.1
for i in range(100):
local_pos_pub.publish(pose)
rate.sleep()
print("landing")
land_cmd = CommandTOL()
landing_client(0.0, 0.0, 0.0, 0.0, 0.0)
rospy.sleep(5)
print("disarming")
arm_cmd.value = False
arm_client_1 = arming_client(arm_cmd.value)