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