plane_communication.py may be the key program of XTDrone , just like Prometheus's state_from_mavros.h command_to_mavros.h
Almost the same as GAAS's px4_mavros_run.py
After gnawing Prometheus thoroughly, and then looking at XTDrone and GAAS, you can quickly locate some core basic key modules. They should all be similar. They should all be the most basic. Start with the most basic control. After the control is built It is not difficult to build other advanced functions. The essence of those controls is that the onboard computer in the offboard mode sends the desired position to the flight controller through MAVROS and so on. No matter how much it changes, it is inseparable from this essence. Prometheus, XTDrone, GAAS are all based on this. Because dronekit does not use MAVROS on ROS, but the essence is still the realization of sending the desired position to the flight controller with the help of mavlink in offboard mode. Control of the flight controller. They just encapsulated this further to make it easier for us to use or build more advanced functions. The most advanced functions, such as round frame tracking, QR code landing, and obstacle avoidance, are all realized by calling these basic controls. As long as these basic controls are realized, these advanced functions are easy to talk about.
Prometheus, XTDrone, GAAS, gnaw through one, and the others will soon pass. So continue to nibble Prometheus, really eat it.
This kind of Belden feel is still great, and the whole string is integrated.
I feel that I have seen a lot of things, and I can see through. Really play around with one thing.
But I still want to find a way to go further.
I was able to find this file because I was inspired by it. I went to the communication folder and looked for it. Before, I went to the control folder and didn't find the key code. Because at first I thought that communication was like Prometheus to define some messages.
https://github.com/robin-shaun/XTDrone
Look at the news it subscribes to and the news it publishes
https://github.com/robin-shaun/XTDrone/blob/master/communication/plane_communication.py
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from geometry_msgs.msg import PoseStamped, Pose
from nav_msgs.msg import Odometry
from gazebo_msgs.srv import GetModelState
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
from multiprocessing import Process
import sys
class Communication:
def __init__(self, vehicle_id):
self.vehicle_type = 'plane'
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.current_state = None
self.target_motion = PositionTarget()
self.global_target = None
self.arm_state = False
self.offboard_state = False
self.motion_type = 0
self.flight_mode = None
self.mission = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
#self.imu_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback)
'''
ros publishers
'''
self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)
self.gazeboModelstate = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
print("communication initialized")
def start(self):
rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication")
rate = rospy.Rate(100)
'''
main ROS thread
'''
while not rospy.is_shutdown():
self.target_motion_pub.publish(self.target_motion)
try:
response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s"%e
odom = Odometry()
odom.header = response.header
odom.pose.pose = response.pose
odom.twist.twist = response.twist
self.odom_groundtruth_pub.publish(odom)
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg
def construct_target(self, x=0, y=0, z=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
if self.coordinate_frame == 1:
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
else:
target_raw_pose.position.x = -y
target_raw_pose.position.y = x
target_raw_pose.position.z = z
if self.mission == 'takeoff':
target_raw_pose.type_mask = 4096
elif self.mission == 'land':
target_raw_pose.type_mask = 8192
elif self.mission == 'loiter':
target_raw_pose.type_mask = 12288
else:
target_raw_pose.type_mask = 16384
return target_raw_pose
def cmd_pose_flu_callback(self, msg):
self.coordinate_frame = 9
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
def cmd_pose_enu_callback(self, msg):
self.coordinate_frame = 1
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
def cmd_callback(self, msg):
if msg.data == '':
return
elif msg.data == 'ARM':
self.arm_state =self.arm()
print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state))
elif msg.data == 'DISARM':
self.arm_state = not self.disarm()
print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state))
elif msg.data in ['takeoff', 'land', 'loiter', 'idle']:
self.mission = msg.data
print(self.mission)
else:
self.flight_mode = msg.data
self.flight_mode_switch()
def arm(self):
if self.armService(True):
return True
else:
print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!")
return False
def flight_mode_switch(self):
if self.flightModeService(custom_mode=self.flight_mode):
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
return True
else:
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed")
return False
if __name__ == '__main__':
communication = Communication(sys.argv[1])
communication.start()
And I found that this part is almost exactly the same as GAAS's px4_mavros_run.py, it feels like XTDrone copied GAAS, and at least one of them should be copied from the other.
I think the GAAS document is dated earlier, it should have been written by GAAS first.
https://blog.csdn.net/sinat_16643223/article/details/107830078
import rospy
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, Float64, String
import time
from pyquaternion import Quaternion
import math
import threading
class Px4Controller:
def __init__(self):
self.imu = None
self.gps = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.takeoff_height = 3.2
self.local_enu_position = None
self.cur_target_pose = None
self.global_target = None
self.received_new_task = False
self.arm_state = False
self.offboard_state = False
self.received_imu = False
self.frame = "BODY"
self.state = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_callback)
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback)
self.set_target_position_sub = rospy.Subscriber("gi/set_pose/position", PoseStamped, self.set_target_position_callback)
self.set_target_yaw_sub = rospy.Subscriber("gi/set_pose/orientation", Float32, self.set_target_yaw_callback)
self.custom_activity_sub = rospy.Subscriber("gi/set_activity/type", String, self.custom_activity_callback)
'''
ros publishers
'''
self.local_target_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
print("Px4 Controller Initialized!")
def start(self):
rospy.init_node("offboard_node")
for i in range(10):
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
self.cur_target_pose = self.construct_target(0, 0, self.takeoff_height, self.current_heading)
#print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose))
for i in range(10):
self.local_target_pub.publish(self.cur_target_pose)
self.arm_state = self.arm()
self.offboard_state = self.offboard()
time.sleep(0.2)
if self.takeoff_detection():
print("Vehicle Took Off!")
else:
print("Vehicle Took Off Failed!")
return
'''
main ROS thread
'''
while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False):
self.local_target_pub.publish(self.cur_target_pose)
if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.state = "DISARMED"
time.sleep(0.1)
def construct_target(self, x, y, z, yaw, yaw_rate = 1):
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
'''
cur_p : poseStamped
target_p: positionTarget
'''
def position_distance(self, cur_p, target_p, threshold=0.1):
delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x)
delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y)
delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z)
if (delta_x + delta_y + delta_z < threshold):
return True
else:
return False
def local_pose_callback(self, msg):
self.local_pose = msg
self.local_enu_position = msg
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
def imu_callback(self, msg):
global global_imu, current_heading
self.imu = msg
self.current_heading = self.q2yaw(self.imu.orientation)
self.received_imu = True
def gps_callback(self, msg):
self.gps = msg
def FLU2ENU(self, msg):
FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading)
FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading)
FLU_z = msg.pose.position.z
return FLU_x, FLU_y, FLU_z
def set_target_position_callback(self, msg):
print("Received New Position Task!")
if msg.header.frame_id == 'base_link':
'''
BODY_FLU
'''
# For Body frame, we will use FLU (Forward, Left and Up)
# +Z +X
# ^ ^
# | /
# |/
# +Y <------body
self.frame = "BODY"
print("body FLU frame")
ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg)
ENU_X = ENU_X + self.local_pose.pose.position.x
ENU_Y = ENU_Y + self.local_pose.pose.position.y
ENU_Z = ENU_Z + self.local_pose.pose.position.z
self.cur_target_pose = self.construct_target(ENU_X,
ENU_Y,
ENU_Z,
self.current_heading)
else:
'''
LOCAL_ENU
'''
# For world frame, we will use ENU (EAST, NORTH and UP)
# +Z +Y
# ^ ^
# | /
# |/
# world------> +X
self.frame = "LOCAL_ENU"
print("local ENU frame")
self.cur_target_pose = self.construct_target(msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z,
self.current_heading)
'''
Receive A Custom Activity
'''
def custom_activity_callback(self, msg):
print("Received Custom Activity:", msg.data)
if msg.data == "LAND":
print("LANDING!")
self.state = "LAND"
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
0.1,
self.current_heading)
if msg.data == "HOVER":
print("HOVERING!")
self.state = "HOVER"
self.hover()
else:
print("Received Custom Activity:", msg.data, "not supported yet!")
def set_target_yaw_callback(self, msg):
print("Received New Yaw Task!")
yaw_deg = msg.data * math.pi / 180.0
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
yaw_deg)
'''
return yaw from current IMU
'''
def q2yaw(self, q):
if isinstance(q, Quaternion):
rotate_z_rad = q.yaw_pitch_roll[0]
else:
q_ = Quaternion(q.w, q.x, q.y, q.z)
rotate_z_rad = q_.yaw_pitch_roll[0]
return rotate_z_rad
def arm(self):
if self.armService(True):
return True
else:
print("Vehicle arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print("Vehicle disarming failed!")
return False
def offboard(self):
if self.flightModeService(custom_mode='OFFBOARD'):
return True
else:
print("Vechile Offboard failed")
return False
def hover(self):
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
self.current_heading)
def takeoff_detection(self):
if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state:
return True
else:
return False
if __name__ == '__main__':
con = Px4Controller()
con.start()