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 __init__(self, drone_id):
# self.id = 0
self.id = drone_id
self.id_state = 0
self.id_pub = rospy.Publisher("ID", Int32)
self.arming_cmd = CommandBool()
self.arming_cmd.value = True
rospy.init_node("controller", anonymous=True)
rospy.Subscriber("request_ID", Int32, self.get_id, queue_size=1)
time.sleep(1)
while self.id_state == 0:
self.id_pub.publish(self.id_state)
print("Waiting for ID")
time.sleep(0.5)
self.rate = rospy.Rate(20)
print("ID received", self.id)
rospy.Subscriber(str(self.id) + "/mavros/state", State, self.state_cb)
rospy.Subscriber(
str(self.id) + "/mavros/local_position/pose", PoseStamped,
self.update_pose)
self.set_mode_client = rospy.ServiceProxy(
str(self.id) + "/mavros/set_mode", SetMode)
self.local_pos_pub = rospy.Publisher(str(self.id) +
"/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
self.state = State()
self.pose = PoseStamped()
self.mode = self.state.mode
self.target = PoseStamped()
time.sleep(2)
def arm(self, value):
self.arming_cmd = CommandBool()
self.arming_cmd.value = value
self.arming_client = rospy.ServiceProxy(
str(self.id) + "/mavros/cmd/arming", CommandBool)
while not self.arming_client(self.arming_cmd.value).success == True:
self.rate.sleep()
print("Arming/Disarming Completed")
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 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)
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("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
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 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 call_arming(self):
rospy.loginfo_once('Arming...')
msg = CommandBool()
resp = self.service_arming.call(True)
return resp.success
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
