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
