ZGZSF
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify communication

This commit is contained in:
robin_shaun 2021-02-25 20:04:29 +08:00
parent bfbc27d8b1
commit e4becb504f
4 changed files with 17 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
communication/multirotor_communication.py
View File

@ -1,20 +1,12 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import CommandBool, SetMode
from geometry_msgs.msg import PoseStamped, Pose, Twist
from gazebo_msgs.srv import GetModelState
from nav_msgs.msg import Odometry
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
import platform
class Communication:
@ -22,16 +14,14 @@ class Communication:
self.vehicle_type = vehicle_type
self.vehicle_id = vehicle_id
self.local_pose = None
self.yaw = 0
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.platform = platform.platform()
'''
ros subscribers
@ -70,7 +60,7 @@ class Communication:
while not rospy.is_shutdown():
self.target_motion_pub.publish(self.target_motion)
if (self.flight_mode is "LAND") and (self.local_pose.position.z < 0.15):
if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
@ -87,8 +77,8 @@ class Communication:
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg.pose
self.yaw = self.q2yaw(msg.pose.orientation)
self.current_position = msg.pose.position
self.current_yaw = self.q2yaw(msg.pose.orientation)
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
@ -214,7 +204,7 @@ class Communication:
def hover(self):
self.coordinate_frame = 1
self.motion_type = 0
self.target_motion = self.construct_target(x=self.local_pose.position.x,y=self.local_pose.position.y,z=self.local_pose.position.z,yaw=self.yaw)
self.target_motion = self.construct_target(x=self.current_position.x,y=self.current_position.y,z=self.current_position.z,yaw=self.current_yaw)
def flight_mode_switch(self):
if self.flight_mode == 'HOVER':
@ -229,7 +219,7 @@ class Communication:
return False
def takeoff_detection(self):
if self.local_pose.position.z > 0.3 and self.arm_state:
if self.current_position.z > 0.3 and self.arm_state:
return True
else:
return False

15
communication/plane_communication.py
View File

@ -1,17 +1,11 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from mavros_msgs.msg import PositionTarget
from mavros_msgs.srv import CommandBool, 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:
@ -20,13 +14,9 @@ class Communication:
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
@ -35,7 +25,6 @@ class Communication:
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)

13
communication/rover_communication.py
View File

@ -1,17 +1,12 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import CommandBool, SetMode
from geometry_msgs.msg import PoseStamped, Pose, Twist
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:
@ -19,12 +14,8 @@ class Communication:
def __init__(self, vehicle_id):
self.vehicle_type = 'rover'
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.target_motion = PositionTarget()
self.global_target = None
self.arm_state = False
self.motion_type = 0
self.flight_mode = None

22
communication/vtol_communication.py
View File

@ -1,17 +1,11 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, PositionTarget
from mavros_msgs.msg import PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from geometry_msgs.msg import PoseStamped, Pose, Twist
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:
@ -20,15 +14,11 @@ class Communication:
self.vehicle_type = vehicle_type
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.yaw = 0
self.current_yaw = 0
self.current_state = None
self.target_motion = PositionTarget()
self.global_target = None
self.arm_state = False
self.offboard_state = False
self.current_heading = None
self.hover_flag = 0
self.coordinate_frame = 1
self.motion_type = 0
@ -39,7 +29,6 @@ class Communication:
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_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
@ -92,7 +81,7 @@ class Communication:
def local_pose_callback(self, msg):
self.local_pose = msg.pose
self.yaw = self.q2yaw(msg.pose.orientation)
self.current_yaw = self.q2yaw(msg.pose.orientation)
def q2yaw(self, q):
if isinstance(q, Quaternion):
@ -229,9 +218,6 @@ class Communication:
else:
self.flight_mode = msg.data
self.flight_mode_switch()
def imu_callback(self, msg):
self.current_heading = self.q2yaw(msg.orientation)
def arm(self):
if self.armService(True):
@ -250,7 +236,7 @@ class Communication:
def hover(self):
self.coordinate_frame = 1
self.motion_type = 0
self.target_motion = self.construct_target(x=self.local_pose.position.x,y=self.local_pose.position.y,z=self.local_pose.position.z,yaw=self.current_heading,yaw=self.yaw)
self.target_motion = self.construct_target(x=self.local_pose.position.x,y=self.local_pose.position.y,z=self.local_pose.position.z,yaw=self.current_heading,yaw=self.current_yaw)
def flight_mode_switch(self):
if self.flight_mode == 'HOVER':