def mavros_arm_offboard_cb(self, timer):
offb_set_mode = SetMode()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBool()
arm_cmd.value = True
if(self.mavros_state.mode != "OFFBOARD" and (rospy.Time.now() - self.last_mavros_request > rospy.Duration(5.0))):
resp1 = self.set_mode_client(0,offb_set_mode.custom_mode)
if resp1.mode_sent:
#rospy.loginfo("Requested OFFBOARD")
pass
self.last_mavros_request = rospy.Time.now()
elif (not self.mavros_state.armed and (rospy.Time.now() - self.last_mavros_request > rospy.Duration(5.0))):
arm_client_1 = self.arming_client(arm_cmd.value)
if arm_client_1.success:
#rospy.loginfo("Requested Vehicle ARM")
pass
self.last_mavros_request = rospy.Time.now()
armed = self.mavros_state.armed
mode = self.mavros_state.mode
rospy.loginfo("Vehicle armed: {}".format(armed))
rospy.loginfo("Vehicle mode: {}".format(mode))
if( (not armed ) or (mode != 'OFFBOARD')):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 0
self.local_pos_pub.publish(pose)
else:
self.status_timer.shutdown()
def handle_mission(file):
mode = SetMode()
mode.base_mode = 0
mode.custom_mode = 'GUIDED'
setmode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
setmode(0, 'GUIDED')
# arm
armsrv = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
armsrv(True)
rate.sleep()
print("Creating pose")
pose = PoseStamped()
#set position here
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 3
for i in range(100):
local_pos_pub.publish(pose)
rate.sleep()
print("Creating Objects for services")
offb_set_mode = SetMode()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBool()
arm_cmd.value = True
num = 0
land = 0
last_request = rospy.Time.now()
while not rospy.is_shutdown():
#print(current_state)
if land == 0:
if (current_state.mode != "OFFBOARD" and
(rospy.Time.now() - last_request > rospy.Duration(5.0))):
resp1 = set_mode_client(0, offb_set_mode.custom_mode)
if resp1.mode_sent:
print("Offboard enabled")
last_request = rospy.Time.now()
rate.sleep()
print("Creating pose")
pose = PoseStamped()
# set position here
pose.pose.position.x = waypoint[0][0]
pose.pose.position.y = waypoint[0][1]
pose.pose.position.z = 2
for i in range(100):
local_pos_pub.publish(pose)
rate.sleep()
print("Creating Objects for services")
offb_set_mode = SetMode()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBool()
arm_cmd.value = True
last_request = rospy.Time.now()
# m is segment
m = len(waypoint) - 1
index = 0
while not rospy.is_shutdown():
# print(current_state)
if current_state.mode != "OFFBOARD" and (
rospy.Time.now() - last_request > rospy.Duration(5.0)):
resp1 = set_mode_client(0, offb_set_mode.custom_mode)
if resp1.mode_sent:
print("Offboard enabled")
last_request = rospy.Time.now()
def takeoff(height):
rospy.Subscriber("/mavros/state", State, state_callback)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, poseCallback)
rate = rospy.Rate(30)
# Wait for MAVROS connection with AP
while not rospy.is_shutdown() and current_state.connected:
rospy.loginfo("Connected to AP!")
rate.sleep()
# Send a few set points initially
pose_stamped = PoseStamped()
pose_stamped.pose.position.z = 0.0
pose_stamped.header.frame_id = "map"
for _ in range(20):
pose_stamped.header.stamp = rospy.Time.now()
local_pose_publisher.publish(pose_stamped)
rate.sleep()
# Set guided and arm
last_request = rospy.Time.now()
set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)
guided_set_mode = SetMode()
guided_set_mode.custom_mode = "GUIDED"
arming_client = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
while not rospy.is_shutdown() and not current_state.armed:
if current_state.mode != "GUIDED" and (
rospy.Time.now() - last_request) > rospy.Duration(2.0):
if (set_mode_client.call(0, "GUIDED")):
rospy.loginfo("Guided enabled!")
last_request = rospy.Time.now()
else:
if not current_state.armed and current_state.mode == "GUIDED" and (
rospy.Time.now() - last_request) > rospy.Duration(2.0):
last_request = rospy.Time.now()
response = arming_client.call(True)
if (response.success):
rospy.loginfo("Vehicle armed")
pose_stamped.header.stamp = rospy.Time.now()
local_pose_publisher.publish(pose_stamped)
rate.sleep()
# Send take off command
takeoff_client = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL)
last_request = rospy.Time.now()
take_off_sent = False
while not rospy.is_shutdown() and not take_off_sent:
if (rospy.Time.now() - last_request) > rospy.Duration(3.0):
response = takeoff_client.call(0, 0, 0, 0, height)
if response.success:
rospy.loginfo("Take off sent!")
take_off_sent = True
else:
rospy.loginfo("Failed to send take off")
last_request = rospy.Time.now()
rate.sleep()