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 disarm(self):
cmd = CommandBool()
cmd.value = False
self.arming_service_client = rospy.ServiceProxy(
"uav" + str(self.id) + "/mavros/cmd/arming", CommandBool)
while not self.arming_service_client(cmd.value).success is True:
rospy.loginfo("UAV " + str(self.id) + " waiting to disarm")
self.rate.sleep()
rospy.loginfo("UAV " + str(self.id) + " DISARMED")
def arm(self):
cmd = CommandBool()
cmd.value = True
self.arming_service_client = rospy.ServiceProxy(
"uav" + str(self.id) + "/mavros/cmd/arming", CommandBool)
while not self.arming_service_client(cmd.value).success is True:
rospy.loginfo("UAV " + str(self.id) + " waiting to arm")
if not self.currently_avoiding:
for i in range(100):
self.target_pos_pub.publish(self.desired_pos_object)
self.rate.sleep()
self.rate.sleep()
rospy.loginfo("UAV " + str(self.id) + " ARMED")
def return_land_disarm(self):
# Return home
print("Returning home.")
rate = rospy.Rate(20)
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) < 10.0): # give 20s to go back home
self.command_pose = self.home_pose
self.publish_command(self.command_pose)
rate.sleep()
# Land
print("Landing.")
self.landing_client(0.0, 0.0, 0.0, 0.0, 0.0)
rospy.sleep(5)
# disarm
print("Disarm.")
arm_cmd = CommandBool()
arm_cmd.value = False
arm_client_1 = self.arming_client(arm_cmd.value)
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()
elif (not current_state.armed
and (rospy.Time.now() - last_request > rospy.Duration(5.0))):
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()
elif not current_state.armed and (rospy.Time.now() - last_request >
rospy.Duration(5.0)):