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()
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 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, 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()
#cmdAction.arm_motors = 1
cmd_srv = rospy.ServiceProxy('/qcontrol/commands', CommandAction)
reponse = cmd_srv(cmdAction)
#reponse = cmd_srv(cmdAction.is_poshld,cmdAction.is_posctl,cmdAction.attctl,cmdAction.arm_motors,cmdAction.start_takeoff,cmdAction.start_landing)
wait_rate = rospy.Rate(20)
wait_rate.sleep()
wait_rate.sleep()
count = 0
att_msg = AttPVA()
att_msg.use_position = True
att_msg.use_speed = False
att_msg.use_acceleration = False
att_msg.use_rate = False
att_msg.use_yaw = False
att_msg.posZ_thrust = z_init
att_msg.posY_pitch = y_init
att_msg.posX_roll = x_init
while (count < 15):
count = count + 1
pub.publish(att_msg)
wait_rate.sleep()
print("Finish publishing start point ")
bof = raw_input('Enter your input:')
# cmdAction = CommandActionRequest()
# cmdAction.is_posctl = 1
# reponse = cmd_srv(cmdAction)