def start_loop():
global current_pub, current_msg, current_req
r = rospy.Rate(SETPOINT_RATE)
while not rospy.is_shutdown():
with handle_lock:
if current_pub is not None:
try:
stamp = rospy.get_rostime()
if getattr(current_req, 'update_frame', False) or \
isinstance(current_req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
current_pub, current_msg = get_publisher_and_message(
current_req, stamp, True,
getattr(current_req, 'update_frame', False))
current_msg.header.stamp = stamp
current_pub.publish(current_msg)
# For monitoring
if isinstance(current_msg, PositionTarget):
p = PoseStamped()
p.header.frame_id = LOCAL_FRAME
p.header.stamp = stamp
p.pose.position = current_msg.position
p.pose.orientation = orientation_from_euler(
0, 0, current_msg.yaw + math.pi / 2)
target_pub.publish(p)
except Exception as e:
rospy.logwarn_throttle(10, str(e))
r.sleep()
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_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_message_publisher(task):
if isinstance(task, SetPositionRequest):
pose = PoseStamped()
pose.header.frame_id = task.frame_id or 'local_origin'
pose.header.stamp = rospy.Time(0)
pose.pose.position = Point(task.x, task.y, task.z)
pose.pose.orientation = orientation_from_euler(0, 0, task.yaw)
pose_local = transform_listener.transformPose('local_origin', pose)
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=eurler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2
)
return position_pub, msg
elif isinstance(task, SetPositionYawRateRequest):
pose = PoseStamped()
pose.header.frame_id = task.frame_id or 'local_origin'
pose.header.stamp = rospy.Time(0)
pose.pose.position = Point(task.x, task.y, task.z)
pose_local = transform_listener.transformPose('local_origin', pose)
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=task.yaw_rate
)
return position_pub, msg
elif isinstance(task, SetVelocityRequest):
vector = Vector3Stamped()
vector.vector = Vector3(task.vx, task.vy, task.vz)
vector.header.frame_id = task.frame_id or 'local_origin'
pose = PoseStamped()
pose.header.frame_id = task.frame_id or 'local_origin'
pose.header.stamp = rospy.Time(0)
pose.pose.orientation = orientation_from_euler(0, 0, task.yaw)
pose_local = transform_listener.transformPose('local_origin', pose)
vector_local = transform_listener.transformVector3(
'local_origin', vector)
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,
yaw=eurler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2
)
msg.velocity = vector_local.vector
return position_pub, msg
elif isinstance(task, SetVelocityYawRateRequest):
vector = Vector3Stamped()
vector.vector = Vector3(task.vx, task.vy, task.vz)
vector.header.frame_id = task.frame_id or 'local_origin'
vector_local = transform_listener.transformVector3(
'local_origin', vector)
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,
yaw_rate=task.yaw_rate
)
msg.velocity = vector_local.vector
return position_pub, msg
elif isinstance(task, SetAttitudeRequest):
pose = PoseStamped()
pose.header.frame_id = task.frame_id or 'local_origin'
pose.header.stamp = rospy.Time(0)
pose.pose.orientation = orientation_from_euler(task.roll, task.pitch,
task.yaw)
pose_local = transform_listener.transformPose('local_origin', pose)
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
thrust=task.thrust,
type_mask=AttitudeTarget.IGNORE_YAW_RATE +
AttitudeTarget.IGNORE_PITCH_RATE +
AttitudeTarget.IGNORE_ROLL_RATE)
return attitude_pub, msg
elif isinstance(task, SetAttitudeYawRateRequest):
msg = AttitudeTarget(orientation=orientation_from_euler(
task.roll, task.pitch, 0),
thrust=task.thrust,
type_mask=AttitudeTarget.IGNORE_PITCH_RATE +
AttitudeTarget.IGNORE_ROLL_RATE)
msg.body_rate.z = task.yaw_rate
return attitude_pub, msg
elif isinstance(task, SetRatesYawRequest):
pose = PoseStamped()
pose.header.frame_id = task.frame_id or 'local_origin'
pose.header.stamp = rospy.Time(0)
pose.pose.orientation = orientation_from_euler(0, 0, task.yaw)
pose_local = transform_listener.transformPose('local_origin', pose)
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
thrust=task.thrust,
type_mask=AttitudeTarget.IGNORE_YAW_RATE)
msg.body_rate = Vector3(task.roll_rate, task.pitch_rate, 0)
return attitude_pub, msg
elif isinstance(task, SetRatesRequest):
msg = AttitudeTarget(thrust=task.thrust,
type_mask=AttitudeTarget.IGNORE_ATTITUDE)
msg.body_rate = Vector3(task.roll_rate, task.pitch_rate, task.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)