def timer_callback(self, event):
#print(self.alt_height)
# Pass wished yaw position (must be between -1 and 1) and measured yaw
_, _, set_y = get_rpy_orientation(
self.current_uav_setpoint.orientation)
self.yaw_setpoint_pub.publish(set_y)
self.yaw_state_pub.publish(self.yaw)
# Pass wished altitude and measured altitude
self.altitude_setpoint_pub.publish(
self.current_uav_setpoint.position.z)
self.altitude_state_pub.publish(self.alt_height)
# Pass wished alt-rate and estimated speed
self.altitude_rate_setpoint_pub.publish(self.alt_effort)
#self.altitude_rate_setpoint_pub.publish(self.current_uav_setpoint.position.z)
self.altitude_rate_state_pub.publish(self.alt_est_speed)
rpyt = RollPitchYawrateThrust()
# Define standard thrust, this should be roughly when the drone is standing still in the air.
thrust_ref = 7.1125
output = thrust_ref + self.alt_rate_effort
# Pass thrust to uav
rpyt.thrust.z = thrust_ref + self.alt_rate_effort
# Pass pitch to uav
rpyt.pitch = self.current_uav_setpoint.orientation.y
# Pass roll to uav
rpyt.roll = self.current_uav_setpoint.orientation.x
# Pass yaw-rate to uav
rpyt.yaw_rate = self.yaw_effort
self.rpyt_pub.publish(rpyt)
def sendCommand(self):
msg = RollPitchYawrateThrust()
msg.roll = self.cmd[0]
msg.pitch = self.cmd[1]
msg.yaw_rate = self.cmd[2]
msg.thrust.z = self.cmd[3]
self.pubcmd.publish(msg)
def pidaltitude(self, data):
# We wish that our drones current position error is 0, the setpoint is the distance to the object / position we-
# want to go to therefore we publish 0 to the PID state.
self.PIDyaw_state.publish(0)
self.PIDpitch_state.publish(0)
self.PIDroll_state.publish(0)
rotor_speed = RollPitchYawrateThrust()
rotor_speed.roll = self.roll_control_effort.data
rotor_speed.yaw_rate = self.yaw_control_effort.data
rotor_speed.pitch = self.pitch_control_effort.data
rotor_speed.thrust.z = float(data.data / 10.99934) + 7.31
self.humm_rotor_pub.publish(rotor_speed)
def timer_callback(self, event):
'''
Pass the variables to the relevant publishers.self
yaw PID:
input.orientation.z -> setpoint
measured_yaw -> state
Altitude PID:
input.position.z (how high we want to go) -> setpoint
measured_height -> state
Altitude-rate PID:
altitude_PID_output -> setpoint
estimated_speed -> state
RollPitchYawrateThrust (input to UAV):
input.pitch -> pitch
input.roll -> roll
yaw_PID_output -> yaw-rate
altitude_rate_PID_output -> thrust
'''
# Pass wished yaw position (must be between -1 and 1) and measured yaw
self.yaw_setpoint_pub.publish(self.current_uav_setpoint.orientation.z)
self.yaw_state_pub.publish(self.yaw)
# Pass wished altitude and measured altitude
self.altitude_setpoint_pub.publish(
self.current_uav_setpoint.position.z)
self.altitude_state_pub.publish(self.alt_height)
# Pass wished alt-rate and estimated speed
self.altitude_rate_setpoint_pub.publish(self.alt_effort)
#self.altitude_rate_setpoint_pub.publish(self.current_uav_setpoint.position.z)
self.altitude_rate_state_pub.publish(self.alt_est_speed)
rpyt = RollPitchYawrateThrust()
# Define standard thrust, this should be roughly when the drone is standing still in the air.
thrust_ref = 7.11
output = thrust_ref + self.alt_rate_effort
# Pass thrust to uav
rpyt.thrust.z = thrust_ref + self.alt_rate_effort
# Pass pitch to uav
rpyt.pitch = self.current_uav_setpoint.orientation.y
# Pass roll to uav
rpyt.roll = self.current_uav_setpoint.orientation.x
# Pass yaw-rate to uav
rpyt.yaw_rate = self.yaw_effort
self.rpyt_pub.publish(rpyt)
def callbackCmdVel(self, data):
if self.pub is None:
return
vel = data
cmd = RollPitchYawrateThrust()
cmd.header.stamp = rospy.Time.now()
# cmd.header.frame_id = "X3/base_link"
cmd.roll = -vel.linear.y
cmd.pitch = vel.linear.x
cmd.yaw_rate = vel.angular.z
cmd.thrust.z = vel.linear.z
self.pub.publish(cmd)
def main():
print "Calibration uwp"
rospy.init_node('calibration_unity')
status = 0
roll = 0.0
pitch = 0.0
yaw = 0.0
thrust = Vector3()
rpy = RollPitchYawrateThrust()
while (1):
key = getKey()
status = status + 1
if key == 'pageup':
roll = roll + 0.1
elif key == 'pagedown':
roll = roll - 0.1
elif key == 'up':
pitch = pitch + 0.1
elif key == 'down':
pitch = pitch - 0.1
elif key == 'right':
yaw = yaw + 0.1
elif key == 'left':
yaw = yaw - 0.1
else:
if (key == '\x03'):
break
if status == 20:
print roll + ", " + pitch + ", " + yaw
status = 0
rpy.roll = roll
rpy.pitch = pitch
rpy.yaw_rate = yaw
rpy.thrust = thrust
pub.publish(rpy)
def do_control():
odom_sub = rospy.Subscriber(model + '/ground_truth/odometry',
Odometry,
odom_cb,
queue_size=100)
mpc_sub = rospy.Subscriber(model + '/rpyth',
RollPitchYawrateThrust,
mpc_cb,
queue_size=100)
target_pose_sub = rospy.Subscriber(model + '/command/pose',
PoseStamped,
target_cb,
queue_size=100)
motor_speed_pub = rospy.Publisher(model +
"/command/roll_pitch_yawrate_thrust",
RollPitchYawrateThrust,
queue_size=100)
rospy.init_node('controller', anonymous=True)
rate = rospy.Rate(50.0)
while not rospy.is_shutdown():
x_f = odom.pose.pose.position.x
y_f = odom.pose.pose.position.y
z_f = odom.pose.pose.position.z
vx_f = odom.twist.twist.linear.x
vy_f = odom.twist.twist.linear.y
vz_f = odom.twist.twist.linear.z
(roll, pitch, yaw) = quaternion_to_euler_angle(
odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
r_f = math.radians(roll)
p_f = math.radians(pitch)
yaw_f = math.radians(yaw)
rs_f = (float(odom.twist.twist.angular.x))
ps_f = (float(odom.twist.twist.angular.y))
ys_f = (float(odom.twist.twist.angular.z))
target = np.array([
target_pose.pose.position.x, target_pose.pose.position.y,
target_pose.pose.position.z, 0., 0., 0., 0., 0., 0., 0.
],
ndmin=2)
# target = np.array([1.,1.,1., 0.,0.,0., 0.,0., 0.,0.],ndmin=2)
state = np.array(
[x_f, y_f, z_f, vx_f, vy_f, vz_f, r_f, p_f, rs_f, ps_f], ndmin=2)
inputs = state - target
# Xtr = [[0.14604905, 0.51560138, 0.27780092, 0.8901413 , 0.90359776, 0.20449533, 0.27998261, 0.48336262, 0.36420253, 0.37711515]]
# Xtr = np.reshape(Xtr, (1,10))
inputs = (inputs - mean) / std
motor_controls = np.ravel(get_actions(inputs))
actuator = RollPitchYawrateThrust()
actuator.header.stamp = rospy.Time.now()
actuator.roll = motor_controls[0]
actuator.pitch = motor_controls[1]
# actuator.yaw_rate = motor_controls[2]
actuator.thrust.z = (motor_controls[3] * 10.34) + (10.34)
# actuator.roll = mpc_u.roll
# actuator.pitch = mpc_u.pitch
actuator.yaw_rate = mpc_u.yaw_rate
# actuator.thrust.z = mpc_u.thrust.z
print('roll: {:.5f}, pitch: {}, yaw_rate: {}, thrust: {}'.format(
actuator.roll, actuator.pitch, actuator.yaw_rate,
actuator.thrust.z))
motor_speed_pub.publish(actuator)
rate.sleep()
def do_control():
odom_sub = rospy.Subscriber('/f_450/odometry',
Odometry,
odom_cb,
queue_size=100)
vrpn_sub = rospy.Subscriber('/f_450/vrpn_client_node/f_450/twist',
TwistStamped,
vrpn_cb,
queue_size=100)
state_sub = rospy.Subscriber('/f_450/state_machine/state_info',
String,
state_cb,
queue_size=100)
target_pose_sub = rospy.Subscriber('/f_450/command/current_reference',
MultiDOFJointTrajectory,
target_cb,
queue_size=100)
mpc_sub = rospy.Subscriber('/f_450/command/roll_pitch_yawrate_thrust',
RollPitchYawrateThrust,
mpc_cb,
queue_size=100)
control_pub = rospy.Publisher(
"/f_450/mavros/setpoint_raw/roll_pitch_yawrate_thrust",
RollPitchYawrateThrust,
queue_size=100)
rospy.init_node('controller', anonymous=True)
rate = rospy.Rate(50.0)
global target_flag_pub
target = np.array([-1.6, -0.5, 0.5, 0., 0., 0., 0., 0.], ndmin=2)
while not rospy.is_shutdown():
x_f = odom.pose.pose.position.x
y_f = odom.pose.pose.position.y
z_f = odom.pose.pose.position.z
vx_f = odom.twist.twist.linear.x
vy_f = odom.twist.twist.linear.y
vz_f = odom.twist.twist.linear.z
(roll, pitch, yaw) = quaternion_to_euler_angle(
odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
r_f = math.radians(roll)
p_f = math.radians(pitch)
yaw_f = math.radians(yaw)
rs_f = float(vrpn.twist.angular.x)
ps_f = float(vrpn.twist.angular.y)
ys_f = float(vrpn.twist.angular.z)
# target = np.array([target_pose.pose.position.x ,target_pose.pose.position.y, target_pose.pose.position.z, 0.,0.,0., 0.,0.,0., 0.,0.,0.],ndmin=2)
# state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f,yaw_f, rs_f,ps_f,ys_f],ndmin=2)
if target_flag_pub == True:
target = np.array([
target_pose.points[0].transforms[0].translation.x,
target_pose.points[0].transforms[0].translation.y,
target_pose.points[0].transforms[0].translation.z, 0., 0., 0.,
0., 0.
],
ndmin=2)
target_flag_pub = False
state = np.array([x_f, y_f, z_f, vx_f, vy_f, vz_f, r_f, p_f], ndmin=2)
inputs = state - target
inputs = (inputs - mean) / std
print(str(state_machine.data))
if str(state_machine.data) == 'PositionHold':
print('MPC: ')
controls = np.ravel(get_actions(inputs))
rpyth_mpc = RollPitchYawrateThrust()
# rpyth_mpc.header.stamp = rospy.Time.now()
# rpyth_mpc.roll = controls[0]
# rpyth_mpc.pitch = controls[1]
# rpyth_mpc.yaw_rate = mpc_u.yaw_rate
# rpyth_mpc.thrust.z = (controls[2] * 7.5) + 7.5
rpyth_mpc.header.stamp = rospy.Time.now()
rpyth_mpc.roll = mpc_u.roll
rpyth_mpc.pitch = mpc_u.pitch
rpyth_mpc.yaw_rate = mpc_u.yaw_rate
rpyth_mpc.thrust.z = mpc_u.thrust.z
control_pub.publish(rpyth_mpc)
elif str(state_machine.data) == 'RcTeleOp' or str(
state_machine.data) == 'RemoteControl' or str(
state_machine.data) == 'HaveOdometry':
print('MPC')
rpyth_mpc = RollPitchYawrateThrust()
rpyth_mpc.header.stamp = rospy.Time.now()
rpyth_mpc.roll = mpc_u.roll
rpyth_mpc.pitch = mpc_u.pitch
rpyth_mpc.yaw_rate = mpc_u.yaw_rate
rpyth_mpc.thrust.z = mpc_u.thrust.z
control_pub.publish(rpyth_mpc)
rate.sleep()
def do_control( ):
odom_sub = rospy.Subscriber('/'+model+'/ground_truth/odometry', Odometry, odom_cb, queue_size=100)
target_pose_sub = rospy.Subscriber('/'+ model + '/command/current_reference', MultiDOFJointTrajectory ,target_cb, queue_size=100)
mpc_sub = rospy.Subscriber('/' + model + '/rpyth', RollPitchYawrateThrust,mpc_cb, queue_size=100)
control_pub = rospy.Publisher(model + "/command/roll_pitch_yawrate_thrust", RollPitchYawrateThrust,queue_size=100)
rospy.init_node('controller',anonymous=True)
rate = rospy.Rate(50.0)
global target_flag_pub
target = np.array([-1.6, -0.3,0.5, 0.,0.,0., 0.,0.],ndmin=2)
while not rospy.is_shutdown():
x_f = odom.pose.pose.position.x
y_f = odom.pose.pose.position.y
z_f = odom.pose.pose.position.z
vx_f = odom.twist.twist.linear.x
vy_f = odom.twist.twist.linear.y
vz_f = odom.twist.twist.linear.z
(roll,pitch, yaw) = quaternion_to_euler_angle(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x , odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
r_f = math.radians(roll)
p_f = math.radians(pitch)
yaw_f = math.radians(yaw)
rs_f = (float(odom.twist.twist.angular.x))
ps_f = (float(odom.twist.twist.angular.y))
ys_f = (float(odom.twist.twist.angular.z))
# target = np.array([target_pose.pose.position.x ,target_pose.pose.position.y, target_pose.pose.position.z, 0.,0.,0., 0.,0.,0., 0.,0.,0.],ndmin=2)
# state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f,yaw_f, rs_f,ps_f,ys_f],ndmin=2)
if target_flag_pub == True:
target = np.array([target_pose.points[0].transforms[0].translation.x , target_pose.points[0].transforms[0].translation.y,target_pose.points[0].transforms[0].translation.z, 0.,0.,0., 0.,0., 0.,0.],ndmin=2)
# print(target_flag_pub)
target_flag_pub = False
state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f, rs_f,ps_f],ndmin=2)
inputs = state - target
inputs = (inputs-mean)/std
controls = np.ravel(get_actions(inputs))
rpyth_mpc = RollPitchYawrateThrust()
rpyth_mpc.header.stamp = rospy.Time.now()
if(controller_type == 'dnn'):
rpyth_mpc.roll = controls[0]
rpyth_mpc.pitch = controls[1]
rpyth_mpc.yaw_rate = mpc_u.yaw_rate
rpyth_mpc.thrust.z = (controls[3] * constant_thrust) + constant_thrust
print('DNN: roll: {:.3f} pitch: {:.3f} thrust: {:3f}'.format(rpyth_mpc.roll, rpyth_mpc.pitch, rpyth_mpc.thrust.z))
else:
rpyth_mpc.roll = mpc_u.roll
rpyth_mpc.pitch = mpc_u.pitch
rpyth_mpc.yaw_rate = mpc_u.yaw_rate
rpyth_mpc.thrust.z = mpc_u.thrust.z
print('MPC: roll: {:.3f} pitch: {:.3f} thrust: {:3f}'.format(rpyth_mpc.roll, rpyth_mpc.pitch, rpyth_mpc.thrust.z))
print('Thrust Error: {:.3f}'.format(abs(z_f - target_pose.points[0].transforms[0].translation.z)))
control_pub.publish(rpyth_mpc)
rate.sleep()
