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)
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()
def setmode(self, MODE):
for i in range(100):
self.local_pos_pub.publish(self.pose)
self.rate.sleep()
print("ID of the drone is ", self.id)
print("Mode is ", str(self.id) + "/mavros/set_mode")
self.set_mode = SetMode()
self.set_mode.custom_mode = MODE
while (self.state.mode != MODE):
self.set_mode_client(0, self.set_mode.custom_mode)
print("setting mode", self.set_mode.custom_mode)
print(self.state.mode, MODE)
time.sleep(1)
def start(self):
rospy.init_node("offboard_node")
rate = rospy.Rate(20)
while (not self.mavros_state.connected):
print(self.mavros_state.connected)
rate.sleep()
for i in range(10):
self.vel_pub.publish(self.command)
rate.sleep()
self.offboard_state = SetMode()
self.offboard_state.custom_mode = "OFFBOARD"
self.arm_state = CommandBool()
self.arm_state.value = True
start_time = rospy.Time.now()
'''
main ROS thread
'''
cnt = -1
while (rospy.is_shutdown() is False):
cnt += 1
self.record_stast()
self.drone_state_pub.publish(self.drone_state)
if self.is_offboard == False:
if self.mavros_state.mode == "OFFBOARD":
resp1 = self.flightModeService(0, "POSCTL")
if cnt % 10 == 0:
print("Enter MANUAL mode")
rate.sleep()
# self.start_height = self.mav_pos[2]
continue
else:
if self.mavros_state.mode != "OFFBOARD":
resp1 = self.flightModeService(
0, self.offboard_state.custom_mode)
if resp1.mode_sent:
print("Offboard enabled")
start_time = rospy.Time.now()
self.command.twist.linear.x = self.cmd_vel[0]
self.command.twist.linear.y = self.cmd_vel[1]
self.command.twist.linear.z = self.cmd_vel[2]
self.command.twist.angular.z = self.cmd_yaw
self.vel_pub.publish(self.command)
rate.sleep()
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 __init__(self):
# Define state variable
self.current_state = State()
self.offb_set_mode = SetMode()
self.arm_cmd = CommandBool()
# Start node
rospy.init_node('offb_node_python', anonymous = True)
# Subscribe/Publish to relavant channels
self.state_sub = rospy.Subscriber('mavros/state', State, state_cb, queue_size=10)
self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size = 10)
# Connect to arming client
rospy.wait_for_service('mavros/cmd/arming')
try:
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
except rospy.ServiceException, e:
rospy.logerr("Service Call failed: %s", e)
print(current_state.connected)
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")
print "waiting waypoint"
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")
def __init__(self):
# Initialize the UAV
if rospy.has_param('ID'):
self.id = rospy.get_param("ID") # input("Drone ID #: ")
else:
self.id = input("Drone ID #: ")
self.uav_id = "uav" + str(self.id)
rospy.init_node("controller" + str(self.id), anonymous=True)
self.rate = rospy.Rate(
10000
) # Determine what this value should be to maximize performance in sim and real world scenarios
# Define Collision Cylinders
# Change these later to a dynamic value that depends on breaking
# distance so Rc = Rc + Dbr 'breaking distance'.
self.reserved_cyl_radius = 2
self.reserved_cyl_vertical = 0.5
self.blocking_cyl_radius = self.reserved_cyl_radius
self.blocking_cyl_vertical = 1
self.conflict_state = {'xy': 'xy-free', 'z': 'z-free'}
self.escape_angle = 0
# Setup the cylindrical obstacle diagram (COD)
self.COD = {'distance': {}, 'angle': {}, 'z-diff': {}}
# Create required publsihers for control
self.target_pos_pub = rospy.Publisher(
self.uav_id + "/mavros/setpoint_position/local",
PoseStamped,
queue_size=1)
self.target_vel_pub = rospy.Publisher(
self.uav_id + "/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=1)
self.uav_stats_pub = rospy.Publisher(self.uav_id + "/stats/",
Int32MultiArray,
queue_size=1)
# Create required subcribers for information and control
self.state_sub = rospy.Subscriber(self.uav_id + "/mavros/state", State,
self.state_changed_callback)
self.actual_pos_sub = rospy.Subscriber(
self.uav_id + "/mavros/local_position/pose", PoseStamped,
self.position_callback)
self.actual_pos_sub = rospy.Subscriber(
self.uav_id + "/mavros/global_position/global", NavSatFix,
self.global_pos_changed_callback)
self.actual_vel_sub = rospy.Subscriber(
self.uav_id + "/mavros/local_position/velocity_local",
TwistStamped, self.velocity_callback)
self.heading_sub = rospy.Subscriber(
self.uav_id + "/mavros/global_position/compass_hdg", Float64,
self.heading_changed_callback)
self.battery_voltage_sub = rospy.Subscriber(
self.uav_id + "/mavros/battery", BatteryState,
self.battery_voltage_callback)
self.network_state_sub = rospy.Subscriber(
"/network_map", Int32, self.network_state_changed_callback)
# Create Objects to pair with above pubs and subs
self.desired_velocity_object = TwistStamped()
self.battery_voltage_object = BatteryState()
self.actual_global_pos_object = NavSatFix()
self.neighbor_global_position_objects = {}
self.desired_pos_object = PoseStamped()
self.actual_pos_object = PoseStamped()
self.stats_object = Int32MultiArray()
self.actual_velocity_object = Twist()
self.neighbor_velocity_objects = {}
self.neighbor_position_objects = {}
self.neighbor_global_pos_sub = {}
self.num_nodes_in_swarm = Int32()
self.neighbor_state_objects = {}
self.neighbor_state_sub = {}
self.mode_object = SetMode()
self.neighbor_pos_sub = {}
self.neighbor_vel_sub = {}
self.heading = Float64()
self.state = State()
self.currently_avoiding = False
self.count = 0
self.set_failsafe_action(2)
#!/usr/bin/env python
import rospy
import mavros
from geometry_msgs.msg import PoseStamped
from ar_track_alvar_msgs.msg import AlvarMarkers
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode
from tf.transformations import *
from math import *
import numpy as np
import argparse
import sys
current_state = State()
offb_set_mode = SetMode()
global sp
sp = PoseStamped()
# callback method for state sub
def state_cb(state):
global current_state
current_state = state
# callback method for local pos sub
def locpos_cb(locpos):
global lp
lp = locpos
# callback method for marker local position
ardata = AlvarMarkers()
def start(self):
rospy.init_node("offboard_node")
rate = rospy.Rate(20)
while (not self.mavros_state.connected):
print(self.mavros_state.connected)
rate.sleep()
for i in range(10):
self.vel_pub.publish(self.command)
self.drone_state.data == self.task_takeoff
self.drone_state_pub.publish(self.drone_state)
rate.sleep()
self.offboard_state = SetMode()
self.offboard_state.custom_mode = "OFFBOARD"
self.arm_state = CommandBool()
self.arm_state.value = True
start_time = rospy.Time.now()
'''
main ROS thread
'''
self.drone_state.data == self.task_takeoff
cnt = -1
while (rospy.is_shutdown() is False):
cnt += 1
# self.record_stast()
# self.drone_state_pub.publish(self.drone_state)
print("self.mav_yaw: {}".format(self.mav_yaw))
self.state_pub()
if self.is_offboard == False:
if self.mavros_state.mode == "OFFBOARD":
resp1 = self.flightModeService(0, "POSCTL")
if cnt % 10 == 0:
print("Enter MANUAL mode")
rate.sleep()
# self.start_height = self.mav_pos[2]
continue
else:
if self.mavros_state.mode != "OFFBOARD":
resp1 = self.flightModeService(
0, self.offboard_state.custom_mode)
if resp1.mode_sent:
print("Offboard enabled")
start_time = rospy.Time.now()
cmd_control = self.state_control(self.drone_state.data)
self.cmd_vel = np.array(
[cmd_control[0], cmd_control[1], cmd_control[2]])
self.cmd_yaw = cmd_control[3]
if self.is_start == True:
self.cmd_vel = np.array([0, 0, 0])
self.cmd_yaw = 0
if (self.drone_state.data == self.task_takeoff
or self.drone_state.data == self.task_in_search
or self.drone_state.data == self.task_recognize_target
or self.drone_state.data == self.task_attack
or self.drone_state.data == self.task_finish
or self.drone_state.data == 0):
self.command.twist.linear.x = self.cmd_vel[0]
self.command.twist.linear.y = self.cmd_vel[1]
self.command.twist.linear.z = self.cmd_vel[2]
self.command.twist.angular.z = self.cmd_yaw
self.vel_pub.publish(self.command)
# self.command.twist.linear.x = self.cmd_vel[0]
# self.command.twist.linear.y = self.cmd_vel[1]
# self.command.twist.linear.z = self.cmd_vel[2]
# self.command.twist.angular.z = self.cmd_yaw
# self.vel_pub.publish(self.command)
rate.sleep()
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()
def start(self):
rospy.init_node("offboard_node")
rate = rospy.Rate(20)
obj_pub = rospy.Publisher(
"ue4_ros/obj", Obj, queue_size=10)
# type_pub = rospy.Publisher(
# "ue4_ros/obj/vehicle_type", UInt64, queue_size=10)
# id_pub = rospy.Publisher(
# "ue4_ros/obj/vehicle_id", UInt64, queue_size=10)
obj_msg = Obj()
# type_msg = UInt64()
# id_msg = UInt64()
while(not self.mavros_state.connected):
print(self.mavros_state.connected)
rate.sleep()
for i in range(10):
self.vel_pub.publish(self.command)
self.drone_state.data == self.task_takeoff
self.drone_state_pub.publish(self.drone_state)
rate.sleep()
self.offboard_state = SetMode()
self.offboard_state.custom_mode = "OFFBOARD"
self.arm_state = CommandBool()
self.arm_state.value = True
start_time = rospy.Time.now()
'''
main ROS thread
'''
self.drone_state.data == self.task_takeoff
cnt = -1
obj_msg.position.x = 0
obj_msg.position.y = 3
obj_msg.position.z = 2
obj_msg.angule.x = 0
obj_msg.angule.y = 0
obj_msg.angule.z = 0
obj_msg.size.x = 5
obj_msg.size.y = 5
obj_msg.size.z = 1
obj_msg.type = 24
obj_msg.id = 100
while (rospy.is_shutdown() is False):
cnt += 1
# self.record_stast()
# self.drone_state_pub.publish(self.drone_state)
print("self.mav_yaw: {}".format(self.mav_yaw))
self.state_pub()
if self.is_offboard == False:
if self.mavros_state.mode == "OFFBOARD":
resp1 = self.flightModeService(0, "POSCTL")
if cnt % 10 == 0:
print("Enter MANUAL mode")
rate.sleep()
# start_yaw = self.mav_yaw
continue
else:
if self.mavros_state.mode != "OFFBOARD":
resp1 = self.flightModeService(
0, self.offboard_state.custom_mode)
if resp1.mode_sent:
print("Offboard enabled")
start_time = rospy.Time.now()
# cmd_control = self.state_control(self.drone_state.data)
# self.cmd_vel = np.array(
# [cmd_control[0], cmd_control[1], cmd_control[2]])
# self.cmd_yaw = cmd_control[3]
self.cmd_vel = np.array([0, 0, 0])
des_pos = np.array([10, 0, 3])
self.cmd_yaw = 0.1
if self.is_start == True:
self.cmd_vel = self.pos_control(
des_pos, self.mav_pos, self.P_pos, 1)
# self.cmd_vel = np.array([0, 0, 0])
# self.cmd_vel[0] = 1
self.cmd_yaw = 0.1
obj_msg.position.x = obj_msg.position.x + 0.05
# obj_msg.angule.x = obj_msg.angule.x + 0.05
# if (self.drone_state.data == self.task_takeoff or self.drone_state.data == self.task_in_search or
# self.drone_state.data == self.task_recognize_target or self.drone_state.data == self.task_attack or
# self.drone_state.data == self.task_finish or self.drone_state.data == 0):
# self.command.twist.linear.x = self.cmd_vel[0]
# self.command.twist.linear.y = self.cmd_vel[1]
# self.command.twist.linear.z = self.cmd_vel[2]
# self.command.twist.angular.z = self.cmd_yaw
# self.vel_pub.publish(self.command)
# print("mav_pos: {}".format(self.mav_pos))
self.command.twist.linear.x = self.cmd_vel[0]
self.command.twist.linear.y = self.cmd_vel[1]
self.command.twist.linear.z = self.cmd_vel[2]
self.vel_pub.publish(self.command)
obj_pub.publish(obj_msg)
# type_pub.publish(type_msg)
# id_pub.publish(id_msg)
rate.sleep()
def __init__(self):
rospy.init_node('sim_tello_interface_node', anonymous=True)
#无人机状态
self.current_state_ = State()
self.flight_state_ = self.FlightState.WAITING
self.mavstateSub_ = rospy.Subscriber('/mavros/state', State,
self.mavstateCallback)
self.gazeboPoseSub_ = rospy.Subscriber(
'/gazebo/model_states', ModelStates, self.gazeboPoseCallback
) #gazebo world中各种模型的状态，包括name,pose={position(x,y,z)+orition(x,y,z,w)}和twist={linear(x,y,z)+angular(x,y,z)}
#无人机在世界坐标系下的位姿
self.R_wu_ = R.from_quat([0, 0, 0, 1])
self.t_wu_ = np.zeros([3], dtype='float')
#无人机的初始位姿，在第一次收到相关topic后初始化，用于进行Gazebo世界坐标与MAVROS本地坐标的转换
self.t_init_ = np.zeros([3], dtype='float')
self.yaw_init_ = 0.0 #角度制
self.R_init_ = R.from_quat([0, 0, 0, 1])
self.is_traj_local_init_ = False
#封装实现tello控制函数
self.cmdTello_ = rospy.Subscriber('/tello/cmd_string', String,
self.cmdUpdate)
self.cmdBuffer = None
#接收指令最小间隔/s
self.cmd_interval = 0.5
self.last_cmd_ = rospy.Time.now()
#无人机控制噪声
self.ctrl_noise_std = 10
self.truncation = 20
#定时发送控制命令(至少2Hz)
self.pub_interval_ = rospy.Duration(0.02)
self.publishloop_timer_ = rospy.Timer(self.pub_interval_,
self.publishloopCallback)
#设置无人机状态
self.arming_client_ = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.land_client_ = rospy.ServiceProxy('/mavros/cmd/land', CommandBool)
self.set_mode_client_ = rospy.ServiceProxy('/mavros/set_mode', SetMode)
#用于发送起降命令和追踪飞行状态
self.last_request_ = rospy.Time.now()
self.offb_set_mode_ = SetMode()
self.arm_cmd_ = CommandBool()
#目标位置信息
self.mutex_target_pose = Lock()
self.uav_target_pose_local_ = PoseStamped()
self.uav_target_pose_local_.header.seq = 1
self.uav_target_pose_local_.header.frame_id = 'map'
self.uav_target_pose_local_.header.stamp = rospy.Time.now()
self.uav_target_pose_local_.pose.position.x = 0
self.uav_target_pose_local_.pose.position.y = 0
self.uav_target_pose_local_.pose.position.z = 1.5
self.uav_target_pose_local_.pose.orientation.x = 0
self.uav_target_pose_local_.pose.orientation.y = 0
self.uav_target_pose_local_.pose.orientation.z = 0
self.uav_target_pose_local_.pose.orientation.w = 1
#当前姿态信息
self.current_pose_ = PoseStamped() #世界坐标
self.current_pose_.header.seq = 1
self.current_pose_.header.frame_id = 'map'
self.current_pose_.pose.orientation.w = 1
#发布目标位置
self.uav_target_pose_local_pub_ = rospy.Publisher(
'/mavros/setpoint_position/local', PoseStamped, queue_size=100)
#在切换到offboard模式之前，必须先发送一些期望点信息到飞控中。 不然飞控会拒绝切换到offboard模式。
rate = rospy.Rate(20)
for i in range(10):
self.uav_target_pose_local_pub_.publish(
self.uav_target_pose_local_)
rate.sleep()
try:
rospy.spin()
except KeyboardInterrupt:
pass
