Ejemplo n.º 1
0
def send_trajectory(traj_3d, sampling_rate):
    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 = False
        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
        pub.publish(att_msg)
        #inter = (currX-att_msg.posX_roll)*(currX-att_msg.posX_roll) + (currY- att_msg.posY_pitch)*(currY- att_msg.posY_pitch) + (currZ - att_msg.posZ_thrust)*(currZ - att_msg.posZ_thrust)
        #while (inter > 0.25):
        #    inter = (currX-att_msg.posX_roll)*(currX-att_msg.posX_roll) + (currY- att_msg.posY_pitch)*(currY- att_msg.posY_pitch) + (currZ - att_msg.posZ_thrust)*(currZ - att_msg.posZ_thrust)
        #pva.t = rospy.Time.from_sec(traj_3d.time[i])
        #pva.pos = traj_3d.position[i]
        #pva.vel = traj_3d.velocity[i]
        r.sleep()
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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()