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 set_vel_body_yaw_rate(self, vel_body, yaw_rate, freq):
"""
Offboard method that sends velocity_body and yaw_rate references to the PX4 autopilot of the the drone.
Makes convertions from the body NED frame adopted for the ISR Flying Arena to the body ENU frame used by ROS.
Parameters
----------
vel_body : np.array of floats with shape (3,1)
Desired linear velocity for the drone, in body NED coordinates.
yaw_rate : float
Desired yaw rate for the vehicle, in radians per second.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
msg = PositionTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 8
msg.type_mask = 1991
msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel_body)
msg.yaw_rate = -yaw_rate
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
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):
# 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,
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 cmdvel2PosTarget(vel):
'''
Translates from JderobotTypes CMDVel to ROS Twist.
@param vel: JderobotTypes CMDVel to translate
@type img: JdeRobotTypes.CMDVel
@return a Twist translated from vel
'''
msg = PositionTarget(header=Header(stamp=rospy.Time.now(), frame_id=''), )
msg.coordinate_frame = 8
msg.type_mask = 1475
msg.position.x = vel.px
msg.position.y = vel.py
msg.position.z = vel.pz
msg.velocity.x = vel.vx
msg.velocity.y = vel.vy
msg.velocity.z = vel.vz
msg.acceleration_or_force.x = vel.ax
msg.acceleration_or_force.y = vel.ay
msg.acceleration_or_force.z = vel.az
msg.yaw = vel.yaw
msg.yaw_rate = vel.yaw_rate
return msg
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 construct_target(self,
x,
y,
z,
yaw,
x_vel=0.2,
y_vel=0.2,
z_vel=0.2,
yaw_rate=1):
target_raw_pose = PositionTarget(
) # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
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.velocity.x = x_vel
target_raw_pose.velocity.y = y_vel
target_raw_pose.velocity.z = z_vel
target_raw_pose.type_mask = 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
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 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 construct_target(self, x, y, z, yaw, yaw_rate=1):
"""
This is a function that will constuct a valid target message for the drone
:param x: Defining the target function in terms of x coordinates(Target)
:param y: Defining the target function in terms of y coordinates(Target)
:param z: Defining the target function in terms of z coordinates(Target)
:param yaw: yaw movement to move towards target
:param yaw_rate: Yaw rate is 1 ms
:rtype: target_raw_pose
:return: Target Position
"""
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
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")
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 construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, yaw=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
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
else:
target_raw_pose.position.x = -y
target_raw_pose.position.y = x
target_raw_pose.position.z = z
target_raw_pose.velocity.x = -vy
target_raw_pose.velocity.y = vx
target_raw_pose.velocity.z = vz
target_raw_pose.yaw = yaw
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_RATE
return target_raw_pose
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 __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 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 callback_odom(pose):
global counter, set_position,set_yaw, current_mode, TAKE_OFF, HOLD, LAND
if current_mode == 'OFFBOARD':
# when set to offboard, start the control procedure
print 'Detect OFFBOARD, start looping:', counter
if counter < 500000 and counter > 0:
#take off
if counter % 200 == 0 and TAKE_OFF == True:
set_position.pose.position.x = 0
set_position.pose.position.y = 0
set_position.pose.position.z += 0.03
set_yaw = 0
if set_position.pose.position.z >= top + 0.3:
TAKE_OFF = False
HOLD = True
elif counter % 200 == 0 and HOLD == True:
set_position.pose.position.x = 0
set_position.pose.position.y = 0
set_position.pose.position.z = top + 0.3
set_yaw = 0
if counter % 2000 == 0:
HOLD = False
LAND = True
elif counter % 200 == 0 and LAND == True:
set_position.pose.position.x = 0
set_position.pose.position.y = 0
set_position.pose.position.z -= 0.02
set_yaw = 0
print "STATE:",'X:',TAKE_OFF,HOLD,LAND,set_position.pose.position.x,'Y:',set_position.pose.position.y,'Z:',set_position.pose.position.z,top
stamp = rospy.get_rostime()
set_position.header.stamp = stamp
position_pub.publish(set_position)
if counter == 0:
print 'Flying test over hold still at 1 1 1'
set_position.z = 0
stamp = rospy.get_rostime()
set_position.header.stamp = stamp
position_pub.publish(set_position)
counter = counter -1
else:
# continuous set vel for OFFBOARD switch
setvel = Vector3()
setvel.x = 0
setvel.y = 0
setvel.z = 0
stamp = rospy.get_rostime()
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE,
velocity=setvel )
msg.header.stamp = stamp
raw_pub.publish(msg)
print 'No OFFBOARD, current mode is:', current_mode, 'set: vel_x',setvel.x, 'set: vel_y',setvel.y, 'set: vel_z',setvel.z
def __init__(self):
# Instantiate a setpoint topic structure
self.setpoint_ = PositionTarget()
# use velocity and yaw setpoints
self.setBodyVelMask()
# Velocity setpoint by user
self.vel_setpoint_ = Vector3()
# Position setpoint by user
self.pos_setpoint_ = Point()
# Current local velocity
self.local_vel_ = TwistStamped()
# Current body velocity
self.body_vel_ = TwistStamped()
# Yaw setpoint by user (degrees); will be converted to radians before it's published
self.yaw_setpoint_ = 0.0
# Current drone position (local frame)
self.drone_pos_ = Point()
# FCU modes
self.fcu_mode_ = FCUModes()
# setpoint publisher (velocity to Pixhawk)
self.setpoint_pub_ = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
# Subscriber for user setpoints (body velocity)
#rospy.Subscriber('setpoint/body_vel', Vector3, self.velSpCallback)
# Subscriber for user setpoints (local position)
#rospy.Subscriber('setpoint/local_pos', Point, self.posSpCallback)
# Subscriber for user setpoints (yaw in degrees)
rospy.Subscriber('setpoint/yaw_deg', Float32, self.yawSpCallback)
# Subscriber to current drone's position
rospy.Subscriber('mavros/local_position/pose', PoseStamped,
self.dronePosCallback)
# Subscriber to body velocity
rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped,
self.bodyVelCallback)
# Subscriber to local velocity
rospy.Subscriber('mavros/local_position/velocity_local', TwistStamped,
self.localVelCallback)
# Service for arming and setting OFFBOARD flight mode
rospy.Service('arm_and_offboard', Empty, self.armAndOffboard)
# Service for autoland
rospy.Service('auto_land', Empty, self.autoLand)
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 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 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 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 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 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
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.transition_state == 'plane':
if self.plane_mission == 'takeoff':
target_raw_pose.type_mask = 4096
elif self.plane_mission == 'land':
target_raw_pose.type_mask = 8192
elif self.plane_mission == 'loiter':
target_raw_pose.type_mask = 12288
else:
target_raw_pose.type_mask = 16384
else:
if self.coordinate_frame == 1:
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
else:
target_raw_pose.velocity.x = -vy
target_raw_pose.velocity.y = vx
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = -afy
target_raw_pose.acceleration_or_force.y = afx
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 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 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 callback_odom(pose):
global r, counter, theta, msg, set_position, set_yaw, wn, setvel
print 'looping:', counter
if counter < 500000 and counter > 450000:
# random fly
if counter % 1 == 0:
setvel.x = 0
setvel.y = 0
setvel.z += 0.001
print "vel fly ", 'set: vel_x', setvel.x, 'set: vel_y', setvel.y, 'set: vel_z', setvel.z
msg = PositionTarget(
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY +
PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFX +
PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE,
velocity=setvel)
stamp = rospy.get_rostime()
msg.header.stamp = stamp
position_pub.publish(msg)
if counter == 0:
print 'Flying test over hold still at 1 1 1'
set_position.x = 1
set_position.y = 1
set_position.z = 1
set_yaw = 0
msg = PositionTarget(
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY +
PositionTarget.IGNORE_VZ + PositionTarget.IGNORE_AFX +
PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW_RATE,
position=set_position,
yaw=set_yaw)
stamp = rospy.get_rostime()
msg.header.stamp = stamp
position_pub.publish(msg)
counter = counter - 1
def set_target(self, x, y, z):
# 事先设定移动的目标位置坐标，若不提前发布目标点坐标，则拒绝切换至“OFFBOARD”模式
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
return target_raw_pose
