The first step towards the final deployment of a brand new system
Date:
Here we provide the user guide for a quick start. \(e^{ i \theta_1} = \cos{\theta_1} + i \sin{\theta_1}\) The guide will be shown like that:
# you may download and check the install like that
fundamentalsystem -v
And the package sample for communication may look like:
class Communication:
def __init__(self, vehicle_type, vehicle_id):
self.vehicle_type = vehicle_type
self.vehicle_id = vehicle_id
self.current_position = None
self.current_yaw = 0
self.hover_flag = 0
self.target_motion = PositionTarget()
self.arm_state = False
self.motion_type = 0
self.flight_mode = None
self.mission = None
self.last_cmd = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback,queue_size=1)
......
'''
ros publishers
'''
self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=1)
'''
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)
print(self.vehicle_type+'_'+self.vehicle_id+": "+"communication initialized")
def start(self):
'''
main ROS thread
'''
while not rospy.is_shutdown():
self.target_motion_pub.publish(self.target_motion)
rate.sleep()
def local_pose_callback(self, msg):
self.current_position = msg.pose.position
self.current_yaw = self.q2yaw(msg.pose.orientation)
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
'......'
def hover_state_transition(self,x,y,z,w):
if abs(x) > 0.02 or abs(y) > 0.02 or abs(z) > 0.02 or abs(w) > 0.005:
self.hover_flag = 0
self.flight_mode = 'OFFBOARD'
elif not self.flight_mode == "HOVER":
self.hover_flag = 1
self.flight_mode = 'HOVER'
self.hover()
def cmd_callback(self, msg):
if msg.data == self.last_cmd or msg.data == '' or msg.data == 'stop controlling':
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[:-1] == "mission" and not msg.data == self.mission:
self.mission = msg.data
print(self.vehicle_type+'_'+self.vehicle_id+": "+msg.data)
else:
self.flight_mode = msg.data
self.flight_mode_switch()
self.last_cmd = msg.data
def flight_mode_switch(self):
if self.flight_mode == 'HOVER':
self.hover_flag = 1
self.hover()
elif 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