def get_telemetry(req):
res = {
'frame_id': req.frame_id or 'local_origin',
'x': float('nan'),
'y': float('nan'),
'z': float('nan'),
'lat': float('nan'),
'lon': float('nan'),
'vx': float('nan'),
'vy': float('nan'),
'vz': float('nan'),
'pitch': float('nan'),
'roll': float('nan'),
'yaw': float('nan'),
'pitch_rate': float('nan'),
'roll_rate': float('nan'),
'yaw_rate': float('nan'),
'voltage': float('nan'),
'cell_voltage': float('nan')
}
frame_id = req.frame_id or 'local_origin'
stamp = rospy.get_rostime()
if pose:
p = tf_buffer.transform(pose, frame_id, TRANSFORM_TIMEOUT)
res['x'] = p.pose.position.x
res['y'] = p.pose.position.y
res['z'] = p.pose.position.z
# Get yaw in the request's frame_in
_, _, res['yaw'] = euler_from_orientation(p.pose.orientation)
# Calculate pitch and roll as angles between the pose and fcu_horiz
attitude_pose = tf_buffer.transform(pose, 'fcu_horiz', TRANSFORM_TIMEOUT)
res['pitch'], res['roll'], _ = euler_from_orientation(attitude_pose.pose.orientation)
if velocity:
v = Vector3Stamped()
v.header.stamp = velocity.header.stamp
v.header.frame_id = velocity.header.frame_id
v.vector = velocity.twist.linear
linear = tf_buffer.transform(v, frame_id, TRANSFORM_TIMEOUT)
res['vx'] = linear.vector.x
res['vy'] = linear.vector.y
res['vz'] = linear.vector.z
# TODO pitch_rate, roll_rate, yaw_rate
if global_position and stamp - global_position.header.stamp < rospy.Duration(5):
res['lat'] = global_position.latitude
res['lon'] = global_position.longitude
if state:
res['connected'] = state.connected
res['armed'] = state.armed
res['mode'] = state.mode
if battery:
res['voltage'] = battery.voltage
res['cell_voltage'] = battery.cell_voltage[0]
return res
def publish_vpe(pose):
stamp = pose.header.stamp
global last_published, vpe_posted
vpe_posted = True
def lookup_transform(target_frame, source_frame):
return tf_buffer.lookup_transform(target_frame, source_frame, stamp,
LOOKUP_TIMEOUT)
# Refine aruco_map
reference_in_local_origin = lookup_transform('local_origin',
'aruco_map_reference')
roll, pitch, yaw = euler_from_orientation(
reference_in_local_origin.transform.rotation)
reference_in_local_origin.transform.rotation = orientation_from_euler(
0, 0, yaw)
send_transform(reference_in_local_origin, 'aruco_map_reference_horiz')
aruco_map_in_reference = lookup_transform('aruco_map_reference',
'aruco_map_raw')
aruco_map_in_reference.header.frame_id = 'aruco_map_reference_horiz'
send_transform(aruco_map_in_reference, 'aruco_map_vision')
# Reset VPE
if last_published is None or stamp - last_published > rospy.Duration(2):
rospy.loginfo('Reset VPE')
aruco_map_in_local_origin = lookup_transform('local_origin',
'aruco_map_vision')
aruco_map_in_local_origin.child_frame_id = 'aruco_map'
static_tf_broadcaster.sendTransform(aruco_map_in_local_origin)
# Calculate VPE
ps.header.frame_id = 'fcu_horiz'
ps.header.stamp = stamp
vpe_raw = tf_buffer.transform(ps, 'aruco_map_vision', LOOKUP_TIMEOUT)
vpe_raw.header.frame_id = 'aruco_map'
vpe = tf_buffer.transform(vpe_raw, 'local_origin', LOOKUP_TIMEOUT)
_vision_position_pub.publish(vpe_raw)
vision_position_pub.publish(vpe)
last_published = stamp
def get_telemetry(req):
res = {
'frame_id': req.frame_id or LOCAL_FRAME,
'x': float('nan'),
'y': float('nan'),
'z': float('nan'),
'lat': float('nan'),
'lon': float('nan'),
'alt': float('nan'),
'vx': float('nan'),
'vy': float('nan'),
'vz': float('nan'),
'pitch': float('nan'),
'roll': float('nan'),
'yaw': float('nan'),
'pitch_rate': float('nan'),
'roll_rate': float('nan'),
'yaw_rate': float('nan'),
'voltage': float('nan'),
'cell_voltage': float('nan')
}
frame_id = req.frame_id or LOCAL_FRAME
stamp = rospy.get_rostime()
if pose:
p = tf_buffer.transform(pose, frame_id, TRANSFORM_TIMEOUT)
res['x'] = p.pose.position.x
res['y'] = p.pose.position.y
res['z'] = p.pose.position.z
# Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(
p.pose.orientation, axes='rzyx')
if velocity:
v = Vector3Stamped()
v.header.stamp = velocity.header.stamp
v.header.frame_id = velocity.header.frame_id
v.vector = velocity.twist.linear
linear = tf_buffer.transform(v, frame_id, TRANSFORM_TIMEOUT)
res['vx'] = linear.vector.x
res['vy'] = linear.vector.y
res['vz'] = linear.vector.z
res['yaw_rate'] = velocity.twist.angular.z
res['pitch_rate'] = velocity.twist.angular.y
res['roll_rate'] = velocity.twist.angular.x
if global_position and stamp - global_position.header.stamp < rospy.Duration(
5):
res['lat'] = global_position.latitude
res['lon'] = global_position.longitude
res['alt'] = global_position.altitude
if state:
res['connected'] = state.connected
res['armed'] = state.armed
res['mode'] = state.mode
if battery:
res['voltage'] = battery.voltage
try:
res['cell_voltage'] = battery.cell_voltage[0]
except:
pass
return res
def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
ps.header.stamp = stamp
vs.header.stamp = stamp
if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
global current_nav_start, current_nav_start_stamp, current_nav_finish
if update_frame:
ps.header.frame_id = req.frame_id or LOCAL_FRAME
ps.pose.position = Point(getattr(req, 'x', 0),
getattr(req, 'y', 0), req.z)
ps.pose.orientation = orientation_from_euler(0,
0,
req.yaw,
axes='sxyz')
current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME,
TRANSFORM_TIMEOUT)
if isinstance(req, srv.NavigateGlobalRequest):
# Recalculate x and y from lat and lon
current_nav_finish.pose.position.x, current_nav_finish.pose.position.y = \
global_to_local(req.lat, req.lon)
if not continued:
current_nav_start = pose.pose.position
current_nav_start_stamp = stamp
if NAVIGATE_AFTER_ARMED and not state.armed:
current_nav_start_stamp = stamp
setpoint = get_navigate_setpoint(stamp, current_nav_start,
current_nav_finish.pose.position,
current_nav_start_stamp, req.speed)
yaw_rate_flag = math.isnan(req.yaw)
msg = pt
msg.coordinate_frame = PT.FRAME_LOCAL_NED
msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
msg.position = setpoint
msg.yaw = euler_from_orientation(current_nav_finish.pose.orientation,
'sxyz')[2]
msg.yaw_rate = req.yaw_rate
return position_pub, msg
elif isinstance(req,
(srv.SetPositionRequest, srv.SetPositionGlobalRequest)):
ps.header.frame_id = req.frame_id or LOCAL_FRAME
ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0),
req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
if isinstance(req, srv.SetPositionGlobalRequest):
pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(
req.lat, req.lon)
yaw_rate_flag = math.isnan(req.yaw)
msg = pt
msg.coordinate_frame = PT.FRAME_LOCAL_NED
msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
msg.position = pose_local.pose.position
msg.yaw = euler_from_orientation(pose_local.pose.orientation,
'sxyz')[2]
msg.yaw_rate = req.yaw_rate
return position_pub, msg
elif isinstance(req, srv.SetVelocityRequest):
vs.vector = Vector3(req.vx, req.vy, req.vz)
vs.header.frame_id = req.frame_id or LOCAL_FRAME
ps.header.frame_id = req.frame_id or LOCAL_FRAME
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
vector_local = tf_buffer.transform(vs, LOCAL_FRAME, TRANSFORM_TIMEOUT)
yaw_rate_flag = math.isnan(req.yaw)
msg = pt
msg.coordinate_frame = PT.FRAME_LOCAL_NED
msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
msg.velocity = vector_local.vector
msg.yaw = euler_from_orientation(pose_local.pose.orientation,
'sxyz')[2]
msg.yaw_rate = req.yaw_rate
return position_pub, msg
elif isinstance(req, srv.SetAttitudeRequest):
ps.header.frame_id = req.frame_id or LOCAL_FRAME
ps.pose.orientation = orientation_from_euler(req.roll, req.pitch,
req.yaw)
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
msg = at
msg.orientation = pose_local.pose.orientation
msg.thrust = req.thrust
msg.type_mask = AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE
return attitude_pub, msg
elif isinstance(req, srv.SetRatesRequest):
msg = at
msg.thrust = req.thrust
msg.type_mask = AT.IGNORE_ATTITUDE
msg.body_rate.x = req.roll_rate
msg.body_rate.y = req.pitch_rate
msg.body_rate.z = req.yaw_rate
return attitude_pub, msg
def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
ps.header.stamp = stamp
vs.header.stamp = stamp
if isinstance(req, srv.NavigateRequest):
global current_nav_start, current_nav_start_stamp, current_nav_finish
if update_frame:
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(req.x, req.y, req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
current_nav_finish = tf_buffer.transform(ps, 'local_origin',
TRANSFORM_TIMEOUT)
if not continued:
current_nav_start = pose.pose.position
current_nav_start_stamp = stamp
setpoint = get_navigate_setpoint(stamp, current_nav_start,
current_nav_finish.pose.position,
current_nav_start_stamp, req.speed)
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=setpoint,
yaw=euler_from_orientation(
current_nav_finish.pose.orientation)[2] - math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetPositionRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(req.x, req.y, req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
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=pose_local.pose.position,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] -
math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetPositionYawRateRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(req.x, req.y, req.z)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
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,
position=pose_local.pose.position,
yaw_rate=req.yaw_rate)
return position_pub, msg
elif isinstance(req, srv.SetPositionGlobalRequest):
x, y = global_to_local(req.lat, req.lon)
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(0, 0, req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
pose_local.pose.position.x = x
pose_local.pose.position.y = y
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=pose_local.pose.position,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] -
math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetPositionGlobalYawRateRequest):
x, y = global_to_local(req.lat, req.lon)
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(0, 0, req.z)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
pose_local.pose.position.x = x
pose_local.pose.position.y = y
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,
position=pose_local.pose.position,
yaw_rate=req.yaw_rate)
return position_pub, msg
elif isinstance(req, srv.SetVelocityRequest):
vs.vector = Vector3(req.vx, req.vy, req.vz)
vs.header.frame_id = req.frame_id or 'local_origin'
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
vector_local = tf_buffer.transform(vs, 'local_origin',
TRANSFORM_TIMEOUT)
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_RATE,
velocity=vector_local.vector,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] -
math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetVelocityYawRateRequest):
vs.vector = Vector3(req.vx, req.vy, req.vz)
vs.header.frame_id = req.frame_id or 'local_origin'
vector_local = tf_buffer.transform(vs, 'local_origin',
TRANSFORM_TIMEOUT)
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,
velocity=vector_local.vector,
yaw_rate=req.yaw_rate)
return position_pub, msg
elif isinstance(req, srv.SetAttitudeRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.orientation = orientation_from_euler(req.roll, req.pitch,
req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_YAW_RATE +
AttitudeTarget.IGNORE_PITCH_RATE +
AttitudeTarget.IGNORE_ROLL_RATE)
return attitude_pub, msg
elif isinstance(req, srv.SetAttitudeYawRateRequest):
msg = AttitudeTarget(orientation=orientation_from_euler(
req.roll, req.pitch, 0),
thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_PITCH_RATE +
AttitudeTarget.IGNORE_ROLL_RATE)
msg.body_rate.z = req.yaw_rate
return attitude_pub, msg
elif isinstance(req, srv.SetRatesYawRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_YAW_RATE,
body_rate=Vector3(req.roll_rate, req.pitch_rate,
0))
return attitude_pub, msg
elif isinstance(req, srv.SetRatesRequest):
msg = AttitudeTarget(thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_ATTITUDE,
body_rate=Vector3(req.roll_rate, req.pitch_rate,
req.yaw_rate))
return attitude_pub, msg
def update_pose(data):
tr.header.stamp = data.header.stamp
tr.transform.translation = vector3_from_point(data.pose.position)
yaw = euler_from_orientation(data.pose.orientation)[2]
tr.transform.rotation = orientation_from_euler(0, 0, yaw)
tf_broadcaster.sendTransform(tr)