def send_position(pos):
att_msg = AttPVA()
att_msg.use_position = True
att_msg.posZ_thrust = pos.z
att_msg.posY_pitch = pos.y
att_msg.posX_roll = pos.x
att_msg.use_speed = False
att_msg.use_yaw = True
att_msg.yaw = -0.07
#att_msg.use_rate = False
#att_msg.velZ = val
#att_msg.velX = val
#att_msg.velY = val
#and so on
pub.publish(att_msg)
def get_att_pva(current_target_pos, current_target_vel):
att_msg = AttPVA()
att_msg.use_position = True
att_msg.use_speed = True
att_msg.use_acceleration = False
att_msg.use_rate = False
att_msg.use_yaw = True
att_msg.yaw = -0.0
att_msg.posZ_thrust = current_target_pos.z
att_msg.posY_pitch = current_target_pos.y
att_msg.posX_roll = current_target_pos.x
att_msg.velZ = current_target_vel.z
att_msg.velY = current_target_vel.y
att_msg.velX = current_target_vel.x
return att_msg
def get_att_pva(current_target_pos, current_target_vel, current_target_acc):
att_msg = AttPVA()
att_msg.use_position = True
att_msg.use_speed = True
att_msg.use_acceleration = False
att_msg.use_rate = False
att_msg.use_yaw = True
att_msg.yaw = -0.08
att_msg.posZ_thrust = current_target_pos.position.z
att_msg.posY_pitch = current_target_pos.position.y
att_msg.posX_roll = current_target_pos.position.x
att_msg.velZ = current_target_vel.linear.z
att_msg.velY = current_target_vel.linear.y
att_msg.velX = current_target_vel.linear.x
att_msg.accZ = current_target_acc.force.z
att_msg.accY = current_target_acc.force.y
att_msg.accX = current_target_acc.force.x
return att_msg
def send_trajectory(traj_3d, sampling_rate,publisher): r = rospy.Rate(sampling_rate) for i in range(len(traj_3d.time)): att_msg = AttPVA() att_msg.use_position = True att_msg.use_speed = True att_msg.use_acceleration = False att_msg.use_rate = False att_msg.use_yaw = True att_msg.yaw = 0 att_msg.posZ_thrust =traj_3d.position[i].position.z att_msg.posY_pitch = traj_3d.position[i].position.y att_msg.posX_roll = traj_3d.position[i].position.x att_msg.velZ=traj_3d.velocity[i].linear.z att_msg.velY=traj_3d.velocity[i].linear.y att_msg.velX=traj_3d.velocity[i].linear.x att_msg.accZ=traj_3d.acceleration[i].force.z att_msg.accY=traj_3d.acceleration[i].force.y att_msg.accX=traj_3d.acceleration[i].force.x publisher.publish(att_msg) r.sleep()