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)