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
set_raw_local = rospy.Publisher( 'mavros/setpoint_raw/local', PositionTarget, queue_size=100) ''' set_att_pub = rospy.Publisher('mavros/setpoint_attiude/attitude', PoseStamped, queue_size=100) set_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) set_raw_att = rospy.Publisher('mavros/setpoint_raw/atittude', AttitudeTarget, queue_size=100) ''' rate = rospy.Rate(20) att_cmd = PositionTarget() att_cmd.coordinate_frame = 8 att_cmd.type_mask = 7 att_cmd.velocity = Vector3() att_cmd.velocity.x = 0 att_cmd.velocity.y = 0 att_cmd.velocity.z = 0.5 for i in range(0, 100): set_raw_local.publish(att_cmd) rate.sleep() set_mode("OFFBOARD", 5) set_arm(True, 5) att_cmd.velocity.x = 0 att_cmd.velocity.y = 0 att_cmd.velocity.z = 0.1 att_cmd.yaw = 0