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 __init__(self, namespace):
self.namespace = namespace
self.start_time = rospy.Time.now()
self.got_odom = False
self.pose_x = []
self.pose_y = []
self.pose_z = []
# self.waypoint = MultiDOFJointTrajectoryPoint()
self.action = RollPitchYawrateThrust
self.active = 'None'
self.got_dnn = False
self.got_mpc = False
self.restarting = False
self.dnn = RollPitchYawrateThrust()
self.mpc = RollPitchYawrateThrust()
self.dagger_beta = 1.0
waypoint_string = '/' + str(self.namespace) + '/command/trajectory'
selection_string = '/' + str(self.namespace +
'/dagger/roll_pitch_yawrate_thrust')
odom_string = '/' + str(self.namespace) + '/ground_truth/odometry'
dnn_string = '/' + str(
self.namespace) + '/dnn/roll_pitch_yawrate_thrust'
mpc_string = '/' + str(
self.namespace) + '/command/roll_pitch_yawrate_thrust'
# self.waypoint_pub = rospy.Publisher(waypoint_string, Rol, queue_size=5)
self.selection_pub = rospy.Publisher(selection_string,
RollPitchYawrateThrust,
queue_size=5)
self.odom_sub = rospy.Subscriber(odom_string,
Odometry,
self.callback_odom,
queue_size=10)
self.dnn_sub = rospy.Subscriber(dnn_string,
RollPitchYawrateThrust,
self.callback_dnn,
queue_size=1)
self.mpc_sub = rospy.Subscriber(mpc_string,
RollPitchYawrateThrust,
self.callback_astar,
queue_size=1)
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 onClose(self):
self.logfile.close()
print "Closing..."
self.enabled = False
rpytmsg = RollPitchYawrateThrust()
self.pubcmd.publish(rpytmsg)
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
msmsg = ModelState()
msmsg.model_name = 'firefly'
msmsg.reference_frame = 'world'
try:
rep = set_model_state(msmsg)
print 'Model Reset'
except rospy.ServiceException as exc:
print 'Service Error:' + str(exc)
self.pubcmd.publish(rpytmsg)
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 callback_image(self, data):
cv_image = []
try:
np_arr = np.fromstring(data.data, np.uint8)
cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
# text_for_image = 'DNN Image Input'
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(cv_image, text_for_image, (10, 25), font, 0.5, (0, 0, 0), 1, cv2.LINE_AA)
cv2.imshow('test', cv_image)
cv2.waitKey(1)
except CvBridgeError as e:
print(e)
self.image = np.asarray(cv_image, dtype=np.uint8)
self.transformed_image = self.do_transforms(self.image)
# DO PREDICTION!
if (self.got_odom and self.init):
# setting up pose (x,y) input
pose_input_np = np.asarray([self.x_pos, self.y_pos])
norm = np.linalg.norm(pose_input_np)
if norm == 0:
pose_input_norm = pose_input_np
else:
pose_input_norm = pose_input_np / norm
pose_input = Variable((torch.from_numpy(pose_input_norm)))
# setting up image input
img_input = Variable(self.transformed_image)
img_input = img_input.reshape((1, 1, 64, 64))
pose_input = pose_input.reshape((1, 1, 2))
if next(self.model.parameters()).is_cuda == True:
img_input = img_input.to(torch.device("cuda"))
pose_input = pose_input.to(torch.device("cuda"))
output = self.model(pose_input, img_input)
# _, pred = torch.max(output,1)
#
# self.dnn_classification = np.asarray(pred)[0]
if next(self.model.parameters()).is_cuda == True:
output_cpu = output.to(torch.device("cpu"))
action = output_cpu.data.numpy()
else:
action = output.data.numpy()
action = action.squeeze()
self.rollrate = action[0]
self.pitchrate = action[1]
self.yawrate = action[2]
self.thrust = action[3]
self.dnn_output = RollPitchYawrateThrust()
self.dnn_output.roll = self.rollrate
self.dnn_output.pitch = self.pitchrate
self.dnn_output.yaw_rate = self.yawrate
self.dnn_output.thrust.z = self.thrust
self.dnn_publisher.publish(self.dnn_output)
import sys
import copy
import rospy
from mav_msgs.msg import RollPitchYawrateThrust
from mavros_msgs.msg import AttitudeTarget
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Quaternion
import tf_conversions
from armf import armtakeoff
rospy.init_node('voxboxTOmavros',anonymous=True)
msg = RollPitchYawrateThrust()
setpoint = PoseStamped()
setpoint.pose.position.x = 10
setpoint.pose.position.y = 10
setpoint.pose.position.z = 20
setpoint.header.frame_id = 'map'
setpoint.header.stamp= rospy.Time.now()
def callback(data):
global msg
msg = data
main1()
mean = np.load(mean_std_dir + 'mean.npy')
std = np.load(mean_std_dir + 'std.npy')
print('mean', mean)
print('std', std)
exit()
target_pose = PoseStamped()
def target_cb(data):
global target_pose
target_pose = data
mpc_u = RollPitchYawrateThrust()
def mpc_cb(data):
global mpc_u
mpc_u = data
def quaternion_to_euler_angle(w, x, y, z):
ysqr = y * y
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = math.degrees(math.atan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
# initalize and set all the variables
pid_x = PIDController(kp=kp_x,
ki=ki_x,
kd=kd_x,
max_windup=1e6,
u_bounds=[0, umax],
alpha=alpha)
pid_y = PIDController(kp=kp_y,
ki=ki_y,
kd=kd_y,
max_windup=1e6,
u_bounds=[0, umax],
alpha=alpha)
pid = PIDController(kp=kp,
ki=ki,
kd=kd,
max_windup=1e6,
u_bounds=[0, umax],
alpha=alpha)
empuje_global = 15.3000000
empuje_angles = RollPitchYawrateThrust()
rospy.loginfo("Iniciando el nodo")
talker()
#rospy.spin()
except rospy.ROSInterruptException:
pass
kd = 0.3
umax = 5.0 # max controller output, (N)
alpha = 1 # derivative filter smoothing factor
# Simulation parameters
N = 400 # number of simulation points
t0 = 0 # starting time, (sec)
tf = 4 # end time, (sec)
time = np.linspace(t0, tf, N)
dt = time[1] - time[0] # delta t, (sec)
#y = [data.pose.pose.position.z,data.twist.twist.linear.z]
y = [0, 0]
# Initialize array to store values
soln = np.zeros((len(time), len(y)))
j = 0
# Create instance of PID_Controller class and
# initalize and set all the variables
pid = PIDController(kp=kp,
ki=ki,
kd=kd,
max_windup=1e6,
u_bounds=[0, umax],
alpha=alpha)
empuje_global = 15.3000000
empuje = RollPitchYawrateThrust()
rospy.loginfo("Iniciando el nodo")
talker()
#rospy.spin()
except rospy.ROSInterruptException:
pass
def odom_cb(data):
global odom
odom = data
vrpn = TwistStamped()
def vrpn_cb(data):
global vrpn
vrpn = data
rpyth = RollPitchYawrateThrust()
def rpyth_cb(data):
global rpyth
rpyth = data
def quaternion_to_euler_angle(w, x, y, z):
ysqr = y * y
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = math.degrees(math.atan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
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()
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = math.degrees(math.atan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
Y = math.degrees(math.asin(t2))
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (ysqr + z * z)
Z = math.degrees(math.atan2(t3, t4))
return X, Y, Z
actuators = RollPitchYawrateThrust()
def control_cb(data):
global actuators
actuators = data
trajectory = MarkerArray()
def trajectory_cb(data):
global trajectory
trajectory = data
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('/'+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()