def send_commands(self, event):
# Create the NED velocity vector.
ned_vec = np.array([[self.vel_x], [self.vel_y], [self.vel_z]],
dtype=np.float32)
# Rotate the NED vector to ENU (for MAVROS).
enu_vec = self.ned_to_enu(ned_vec)
# Create the PositionTarget command message.
command_msg = PositionTarget()
# Fill out the message.
command_msg.header.stamp = rospy.get_rostime()
# FRAME_BODY_NED is the vehicle-1 frame
command_msg.coordinate_frame = command_msg.FRAME_BODY_NED
command_msg.type_mask = self.ignore_mask
command_msg.velocity.x = enu_vec[0][0]
command_msg.velocity.y = enu_vec[1][0]
command_msg.velocity.z = enu_vec[2][0]
command_msg.yaw_rate = -self.yaw_rate
# Publish the message.
self.command_pub.publish(command_msg)
def offboard_loop(mode):
pub_pt = {}
#создаем топики, для публикации управляющих значений
for n in range(1, instances_num + 1):
pub_pt[n] = rospy.Publisher(f"/mavros{n}/setpoint_raw/local",
PositionTarget,
queue_size=10)
pt = PositionTarget()
#см. также описание mavlink сообщения https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED
pt.coordinate_frame = pt.FRAME_LOCAL_NED
t0 = time.time()
#цикл управления
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
dt = time.time() - t0
#управляем каждым аппаратом централизованно
for n in range(1, instances_num + 1):
set_mode(n, "OFFBOARD")
if mode == 0:
mc_example(pt, n, dt)
elif mode == 1:
vtol_example(pt, n, dt)
pub_pt[n].publish(pt)
rate.sleep()
def forward(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0
print("forward")
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 1.0
vel.velocity.z = 0
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
forward_t = rospy.get_time()
while rospy.get_time(
) - forward_t <= bt_missions.dist * 0.1: #bt_missions.dist *1.25*0.25:
#rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
def send_waypoint_command(self):
if self.mode_flag == 'mavros':
waypoint_command_msg = PositionTarget()
waypoint_command_msg.header.stamp = rospy.get_rostime()
waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED
waypoint_command_msg.type_mask = self.ned_pos_mask
waypoint_command_msg.position.x = self.wp_E # E
waypoint_command_msg.position.y = self.wp_N # N
waypoint_command_msg.position.z = self.rendezvous_height # U
# This converts an NED heading command into an ENU heading command
yaw = -self.heading_command + np.radians(90.0)
while yaw > np.radians(180.0):
yaw = yaw - np.radians(360.0)
while yaw < np.radians(-180.0):
yaw = yaw + np.radians(360.0)
waypoint_command_msg.yaw = yaw
# Publish.
self.command_pub_mavros.publish(waypoint_command_msg)
else:
wp_command_msg = Command()
wp_command_msg.x = self.wp_N
wp_command_msg.y = self.wp_E
wp_command_msg.F = -self.rendezvous_height
wp_command_msg.z = self.heading_command
wp_command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
# Publish.
self.command_pub_roscopter.publish(wp_command_msg)
def Aruco_marker_detector(self):
""" Use the build in python library to detect the aruco marker and its coordinates """
img = self.frame.copy()
# Detect the markers in the image
markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters)
# Get the transformation matrix to the marker
if markerCorners:
markerSize = 2.0
axisLength = 3.0
rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(markerCorners, markerSize, self.K, self.distCoeffs)
# Draw the axis on the marker
frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength)
rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0)
rotMat = rotMat[0:3, 0:3]
tvecs = np.matmul(rotMat, tvecs[0][0].T)
rotMat, _ = cv.Rodrigues(rvecs)
eul = tr.euler_from_matrix(rotMat)
yaw_angle = self.pos[3] - eul[2]
# Publish the position of the marker
marker_pos = PT()
marker_pos.position.x = tvecs[0]
marker_pos.position.y = - tvecs[2]
marker_pos.position.z = tvecs[1]
marker_pos.yaw = eul[2] * np.pi / 180.0
self.aruco_marker_pos_pub.publish(marker_pos)
# Publish the image with the detected marker
self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
def twist_obj(x, y, z, a, b, c):
move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime()))
move_cmd.velocity.x = x
move_cmd.velocity.y = y
move_cmd.velocity.z = z
move_cmd.yaw_rate = c
move_cmd.coordinate_frame = 8
move_cmd.type_mask = 1479 # 1479/ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0)
return move_cmd
def send_ned_velocity(v_x, v_y, v_z): set_velocity = PositionTarget() set_velocity.coordinate_frame = 1 # local NED set_velocity.velocity.x = v_y # verificare che le componenti siano invertite set_velocity.velocity.y = v_x set_velocity.velocity.z = v_z set_velocity.type_mask = 4039 setpoint_velocity_pub.publish(set_velocity) time.sleep(0.5)
def set_velocity_body(v_x, v_y, v_z, rate):
set_velocity = PositionTarget()
set_velocity.coordinate_frame = 8 # body NED
set_velocity.velocity.x = v_y # verificare che le componenti siano invertite
set_velocity.velocity.y = v_x
set_velocity.velocity.z = v_z
set_velocity.yaw_rate = rate
set_velocity.type_mask = 1479
setpoint_velocity_pub.publish(set_velocity)
def altitude_target(altitude, yaw=0.0, frame_id='world'):
pt = PositionTarget()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = frame_id
pt.coordinate_frame = 1
pt.type_mask = POSITION_MASK
pt.position.z = altitude
pt.yaw = yaw # in radians!
return pt
def Intarget_local():
set_target_local = PositionTarget()
set_target_local.type_mask = 0b100111111000
set_target_local.coordinate_frame = 1
set_target_local.position.x = 10
set_target_local.position.y = 10
set_target_local.position.z = 10
set_target_local.yaw = 0
return set_target_local
def custom(twisted):
#print("custom1")
move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime()))
move_cmd.velocity.x = twisted.linear.y
move_cmd.velocity.y = twisted.linear.x
move_cmd.velocity.z = twisted.linear.z
move_cmd.yaw_rate = twisted.angular.z
move_cmd.coordinate_frame = 8
move_cmd.type_mask = 1479 # 1479/ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0)
pub.publish(move_cmd)
def takeoff_obj(z):
print("--------------------Takeoff-----------------")
move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime()))
move_cmd.velocity.x = 0
move_cmd.velocity.y = 0
move_cmd.velocity.z = 0.5
move_cmd.position.z = z
move_cmd.type_mask = (0x1000 | 0b110111000011)
move_cmd.coordinate_frame = 9
return move_cmd
def get_descent(self,x,y,z):
des_vel = PositionTarget()
des_vel.header.frame_id = "world"
des_vel.header.stamp=rospy.Time.from_sec(time.time())
des_vel.coordinate_frame= 8
des_vel.type_mask = 3527
des_vel.velocity.x = x
des_vel.velocity.y = y
des_vel.velocity.z = z
return des_vel
def displacement(x_disp, y_disp, z_disp):
print("I am in displacement")
des_disp = PositionTarget()
des_disp.header.frame_id = "world"
des_disp.header.stamp = rospy.Time.from_sec(time.time())
des_disp.coordinate_frame = 8
des_disp.type_mask = 3527
des_disp.velocity.x = x_disp
des_disp.velocity.y = y_disp
des_disp.velocity.z = z_disp
return des_disp
def position_target(position, yaw=0.0, frame_id='world'):
pt = PositionTarget()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = frame_id
pt.coordinate_frame = 1
pt.type_mask = POSITION_MASK
pt.position.x = position[0]
pt.position.y = position[1]
pt.position.z = position[2]
pt.yaw = yaw # in radians!
return pt
def velocity_target(velocity, frame_id='world'):
pt = PositionTarget()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = frame_id
pt.coordinate_frame = 1
pt.type_mask = VELOCITY_MASK
pt.velocity.x = velocity[0]
pt.velocity.y = velocity[1]
pt.velocity.z = velocity[2]
pt.yaw = position_to_angle(velocity[0], velocity[1])
return pt
def listener():
rospy.init_node('listener', anonymous=True)
rate = rospy.Rate(20)
rospy.Subscriber('mavros/state', State, callback)
local_pos_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=2)
local_posi_raw_pub = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=2)
#local_vel_pub=rospy.Publisher('mavros/setpoint_position/local',PoseStamped,queue_size=2)
set_mode_cmd = rospy.ServiceProxy('mavros/set_mode', SetMode)
arm_cmd = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
newpos = PoseStamped()
newpos.pose.position.z = 10.0 #=Point(0,0,2)
#set_mode_cmd('POSITION CONTROL','')
#for _ in range(10):
# rate.sleep()
newvel = PositionTarget()
newvel.velocity.x = -0.5
newvel.yaw = -90.0 / 180.0 * 3.142
newvel.type_mask = newvel.FRAME_LOCAL_NED | newvel.IGNORE_AFX | newvel.IGNORE_AFY | newvel.IGNORE_AFZ
newvel.type_mask = newvel.type_mask | newvel.IGNORE_PX | newvel.IGNORE_PY | newvel.IGNORE_PZ
for _ in range(100):
rate.sleep()
local_pos_pub.publish(newpos)
mymode = 'OFFBOARD'
last_req = rospy.get_time()
start_time = last_req
#print '---',rospy.get_time(),start_time
while rospy.get_time() - start_time < 70:
if rospy.get_time() - last_req > 5:
if state.mode != mymode:
set_mode_cmd(0, mymode)
rospy.loginfo('setting mode...')
elif not state.armed:
arm_cmd(True)
rospy.loginfo('arming...')
last_req = rospy.get_time()
dt = rospy.get_time() - start_time
if dt < 20:
local_pos_pub.publish(newpos)
elif dt < 40:
local_posi_raw_pub.publish(newvel)
elif dt < 60:
newvel.velocity.x = 0.5
local_posi_raw_pub.publish(newvel)
else:
newpos.pose.position.z = 0
local_pos_pub.publish(newpos)
rate.sleep()
def twist_obj(x, y, z, a, b, c):
# move_cmd = Twist()
move_cmd = PositionTarget()
move_cmd.velocity.x = x
move_cmd.velocity.y = y
move_cmd.velocity.z = z
# move_cmd.yaw_rate = c
move_cmd.header.stamp = rospy.get_rostime()
move_cmd.header.frame_id = "world"
move_cmd.coordinate_frame = 8
move_cmd.type_mask = 3527
return move_cmd
def getvelBody(self, u, v, w):
msg = PositionTarget()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 8
msg.type_mask = 4039
msg.velocity.x = u
msg.velocity.y = v
msg.velocity.z = w
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.pub2.publish(msg)
r.sleep()
def global_position():
global home_
global home_set_
global current_
global brng
global dirct, scale
global current_heading
global current_yaw
guide()
hdg_subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg',
Float64, compass)
home_sub = rospy.Subscriber("/mavros/home_position/home", HomePosition,
setHomeGeoPointCB)
set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
set_v_pub = rospy.Publisher('/mavros/setpoint_yaw/local',
PositionTarget,
queue_size=10)
gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix,
current_position)
rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, gpsstate_cb)
gps_pub = rospy.Publisher('/mavros/global_position/raw/gps_vel',
TwistStamped,
queue_size=10)
#altitude_sub = rospy.Subscriber('/mavros/altitude', Altitude, alt_cb)
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position',
'local'),
PoseStamped,
queue_size=10)
time_now = rospy.get_time()
while rospy.get_time() - time_now <= 3:
pass
print("gps now: %s" % gps_state_.status)
if gps_state_.status == -1:
RTL()
else:
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0
print("forward")
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 1
vel.velocity.z = 0
forward_t = rospy.get_time()
while rospy.get_time() - forward_t <= 5:
set_vel_pub.publish(msg)
set_v_pub.publish(vel)
rate.sleep()
def circle(self, x, y, z):
msg = PositionTarget()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 8
msg.type_mask = 1543
msg.velocity.x = 0
msg.velocity.y = -1
msg.velocity.z = 0
msg.acceleration_or_force.x = -0.2
msg.acceleration_or_force.y = 0
msg.acceleration_or_force.z = 0
msg.yaw_rate = -0.2
self.pub2.publish(msg)
def forward(self):
print(123)
forward_t = rospy.get_time()
#while rospy.get_time() - forward_t <= 5:
bt_missions.dis_count = False
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 0.8
vel.velocity.z = 0
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
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
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
if(self.motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW_RATE
if(self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW
if(self.motion_type == 2):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_YAW
return target_raw_pose
def main():
while True:
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
callback_pos)
local_pos_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
pose = PositionTarget()
pose.position.x = 0.9
pose.velocity.z = 0
pose.acceleration_or_force.z = 0
global i
t0 = rospy.Time.now().to_sec(
) # To just wander left and ri8 to get a detection
while rospy.Time.now().to_sec() - t0 < 2: # its enough i think
# if i%2 == 0:
# pose.position.y = 1
# else:
# pose.position.y = -1
pose.position.y = 0.2
pose.position.z = 3
local_pos_pub.publish(pose)
i = i + 1
def __init__(self):
# Communication variables
self._last_pose = PoseStamped()
self._setpoint_msg = PositionTarget()
self._setpoint_msg.type_mask = _DEFAULT
self._last_state = State()
# ROS Communication
self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local",
PositionTarget, queue_size=1)
self._pose_sub = rospy.Subscriber("mavros/local_position/pose",
PoseStamped, self._pose_cb)
self._state_sub = rospy.Subscriber('mavros/state',
State, self._state_cb)
# wait for mavros to start publishing
rospy.logdebug("Waiting for MAVROS to start")
rospy.wait_for_message("mavros/mission/waypoints", WaypointList)
# Make drone less aggressive
rospy.wait_for_service("mavros/param/set")
mavparam = rospy.ServiceProxy('mavros/param/set', ParamSet)
mavparam("MC_PITCH_P", ParamValue(0, 2.0)).success
mavparam("MC_ROLL_P", ParamValue(0, 2.0)).success
mavparam("MPC_XY_VEL_MAX", ParamValue(0, 3.0)).success
# Send a few setpoint to make MAVROS happy
rate = rospy.Rate(20.0)
for _ in range(40):
rate.sleep()
self._publish_setpoint(None)
rospy.Timer(rospy.Duration(0.05), self._publish_setpoint)
rospy.loginfo("Drone Initialized")
def __init__(self):
self.state = "disarm"
#создаем топики, для публикации управляющих значений:
self.pub_pt = rospy.Publisher(f"/mavros{1}/setpoint_raw/local",
PositionTarget,
queue_size=10)
self.pt = PositionTarget()
self.pt.coordinate_frame = self.pt.FRAME_LOCAL_NED
self.t0 = time.time()
self.dt = 0
#params
self.p_gain = 2
self.max_velocity = 5
self.arrival_radius = 0.6
self.waypoint_list = [
np.array([6., 7., 6.]),
np.array([0., 14., 7.]),
np.array([18., 14., 7.]),
np.array([0., 0., 5.])
]
self.current_waypoint = np.array([0., 0., 0.])
self.pose = np.array([0., 0., 0.])
self.velocity = np.array([0., 0., 0.])
self.mavros_state = State()
self.subscribe_on_topics()
def __init__(
self,
flight_instruction_type='generic_mission_state',
timeout=10,
mavros_message_node=None,
parent_ref=None,
# tol_heading_deg=1.0,
state_label="unspecified", # use this to store information about the purpose of the state, e.g. "explore the world"
timeout_OK=True, # this state should be set to false if timouts should be considered a mission failure
tol_distance=0.2,
tol_heading_deg=5,
**kwargs):
self.flight_instruction_type = flight_instruction_type
self._timeout = timeout
self._setpoint_raw = PositionTarget()
self.state_label = state_label
self.timeout_OK = timeout_OK
self.mission_state_busy = False # this flag is used to indicate that the flight state is still processing an instruction and should not be polled again
# Initialise mission state setpoint with the previous states setpoint
self.update_sp_locals()
# initialise local properties
self._ros_message_node = mavros_message_node
self._parent_ref = parent_ref
self.stay_alive = True
self.preconditions_satisfied = False
self._prerun_complete = False
self.tol_distance = tol_distance
self.tol_heading_deg = tol_heading_deg
self.tol_heading_rad = np.deg2rad(tol_heading_deg)
def __init__(self):
# Communication variables
self._last_pose = None
self._last_twist = None
self._setpoint_msg = PositionTarget()
self._last_state = State()
self._camera = Camera(imgtopic="drone/camera_front/image",
infotopic="drone/camera_front/info")
# ROS Communication
self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local",
PositionTarget,
queue_size=1)
self._pose_sub = rospy.Subscriber("mavros/local_position/pose",
PoseStamped, self._pose_cb)
self._twist_sub = rospy.Subscriber("mavros/local_position/velocity",
TwistStamped, self._twist_cb)
self._state_sub = rospy.Subscriber('mavros/state', State,
self._state_cb)
self._publish_setpoint_active = False
rospy.Timer(rospy.Duration(0.05), self._publish_setpoint)
rospy.loginfo("Drone Initialized")
def __init__(self, altitude = flight_alt):
rospy.init_node('quadsim_teleop') # creates the node
self.state_sub = rospy.Subscriber("%s/mavros/state" % uav, State, self.state_cb)
self.local_pose_pub = rospy.Publisher("%s/mavros/setpoint_position/local" % uav, PoseStamped, queue_size=10)
self.landing = False
# Clients
self.arm_client = rospy.ServiceProxy("%s/mavros/cmd/arming" % uav, CommandBool)
self.land_client = rospy.ServiceProxy("%s/mavros/cmd/land" % uav, CommandTOL)
self.land_sub = rospy.Subscriber('/quadcopter_land', Empty, self.land_cb) # Listens for a command to land drone
self.current_state = None
self.des_z = altitude
self.rate = rospy.Rate(20)
self.arm()
# subscribes to cmd_vel to receive velocity commands
self.sub = rospy.Subscriber('/cmd_vel', Twist, self.twist_cb)
# publishes velocity commands to hover and teleop
self.pub = rospy.Publisher('%s/mavros/setpoint_raw/local' % uav, PositionTarget, queue_size=3)
# define msg
self.setvel_msg = PositionTarget()
self.define_msg()
def __init__(self, vehicle_id):
self.vehicle_type = 'plane'
self.vehicle_id = vehicle_id
self.local_pose = None
self.target_motion = PositionTarget()
self.arm_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.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")
