def run(self):
while (not self.attitude_command_received_flag) and (
not rospy.is_shutdown()):
print "Waiting for attitude controller to start"
rospy.sleep(0.5)
print "Attitude control started."
while (not self.mot_vel_ref_received_flag) and (
not rospy.is_shutdown()):
print "Waiting for height controller to start"
rospy.sleep(0.5)
print "Height control started."
while not rospy.is_shutdown():
self.ros_rate.sleep()
# Compute motor velocities, + configuration
mot1 = self.mot_vel_ref - self.vpc_pitch_command + self.yaw_command
mot2 = self.mot_vel_ref + self.vpc_roll_command - self.yaw_command
mot3 = self.mot_vel_ref + self.vpc_pitch_command + self.yaw_command
mot4 = self.mot_vel_ref - self.vpc_roll_command - self.yaw_command
mot_speed_msg = Actuators()
mot_speed_msg.header.stamp = rospy.Time.now()
mot_speed_msg.angular_velocities = [mot1, mot2, mot3, mot4]
self.mot_vel_pub.publish(mot_speed_msg)
self.q1_left_pub.publish(Float64(self.q1_left))
self.q2_left_pub.publish(Float64(self.q2_left))
self.q3_left_pub.publish(Float64(self.q3_left))
self.q1_right_pub.publish(Float64(self.q1_right))
self.q2_right_pub.publish(Float64(self.q2_right))
self.q3_right_pub.publish(Float64(self.q3_right))
def step1(self, thrust_cmds, dt):
# thrust_cmds = np.array([0., 0., 0., 0.])
# print("DYN: step1: wait_for_message: odometry")
# print('DYN: thrust: ', thrust_cmds)
# time_start = current_time_ms()
# Publish the action
actuator_msg = Actuators()
## Direct angular velocity control
# angular_velocities = (self.max_angular_val*np.array(thrust_cmds)).astype(dtype=np.int)
## Approximate torque control (converting torque to angular velocity for quad input)
angular_velocities = np.clip(np.sqrt(
(thrust_cmds * self.thrust) / 8.54858e-06),
a_min=0.,
a_max=self.max_angular_val)
# print('Rotor commands: ', angular_velocities)
actuator_msg.angular_velocities = angular_velocities
self.action_publisher.publish(actuator_msg)
## Delay monitoring
self.step_delay = (current_time_ms() - self.time_last) / 1000.
self.time_last = current_time_ms()
def send_motor_command(self):
actuator = Actuators()
actuator.header.stamp = rospy.Time.now()
# print ("motor_speed: ", self.motor_speed)
actuator.angular_velocities = self.motor_speed
self.pub_actuator.publish(actuator)
float64 = Float64()
float64.data = self.motor_speed[0, 0]/1000.0
self.pub_motor_speed_des.publish(float64)
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting height control."
while not rospy.is_shutdown():
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant reference values for motor velocity should be stored in variable self.mot_speed.
hover_0 = 650.95
Ts = datetime.now() - self.t_old
self.t_old = datetime.now()
#print( Ts.total_seconds())
print(self.z_sp)
print(self.z_mv)
self.vz_sp = self.pid_z.compute(self.z_sp, self.z_mv)
self.mot_speed = self.pid_vz.compute(self.vz_sp,
self.vz_mv) + hover_0
#self.mot_speed = hover_0
print("mot_speed ", self.mot_speed)
########################################################
########################################################
if self.attitude_ctl == 0:
# Publish motor velocities
mot_speed_msg = Actuators()
mot_speed_msg.angular_velocities = [
self.mot_speed, self.mot_speed, self.mot_speed,
self.mot_speed
]
self.pub_mot.publish(mot_speed_msg)
else:
# publish reference motor velocity to attitude controller
mot_speed_msg = Float32(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
# Publish PID data - could be usefule for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def rotor_s_message(self,U,PsiStar): # creating actuators message actuators_message = Actuators() # this is just for testing # actuators_message.angular_velocities = np.array([100,100,100,100,100,100]) # copy motor speeds into message previously created # actuators_message.angular_velocities = n # just for debug pruposes # actuators_message.angular_velocities = np.array([200,200,200,200,200,200]) actuators_message.angular_velocities = self.rotor_s_standard_converter(U,PsiStar) return actuators_message
def run(self):
"""Run ros node, compute PID algorithms for z and vz control"""
# init publisher
while not self.first_measurement:
print("Waiting for pose measurement")
rospy.sleep(0.5)
print("Start height control.")
self.t_old = rospy.Time.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
if dt > 0.105 or dt < 0.095:
print(dt)
self.t_old = t
# publish angles for tilt (in order to have rotors in normal position)
self.tilt_0_pub.publish(0.0)
self.tilt_1_pub.publish(0.0)
self.tilt_2_pub.publish(0.0)
self.tilt_3_pub.publish(0.0)
self.motor_hover_speed = math.sqrt(293 / 0.000456874 / 4)
# prefilter for reference (Why we use prefilter for reference)
a = 0.1
self.z_ref_filt = (1 - a) * self.z_ref_filt + a * self.z_sp
vz_ref = self.pid_z.compute(self.z_ref_filt, self.z_mv, dt)
self.motor_speed = self.motor_hover_speed + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
motor_speed_msg = Actuators()
motor_speed_msg.angular_velocities = [
self.motor_speed, self.motor_speed, self.motor_speed,
self.motor_speed
]
self.pub_mot.publish(motor_speed_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
self.z_sp = 1
def pub_llc(self):
pos_err = self.pos_des - self.pos
vel_err = self.vel_des - self.vel
# acc_des : feedforward, error : feedback
g = 9.81
e3 = np.array([0, 0, 1])
t = self.kx * pos_err + self.kv * vel_err + self.m * g * e3 - self.m * self.acc_des
# Desired Thrust
F = t * self.R * e3
if F < 0:
F = 0
elif F > self.max_thrust:
F = self.max_thrust
rot_err = 1 / 2 * self.R_des.T * self.R - self.R.T * self.R_des
# v map from 3x3 skew-symmetric to 3x1 vector
eR = np.array([0, 0, 0])
eR[0] = -rot_err[1][2] # x
eR[1] = rot_err[0][2] # y
eR[2] = -rot_err[0][1] # z
ew = self.w - self.R.T * self.R_des * self.w_des
# h map from 3x1 vector to 3x3 skew-symmetric
w_h = np.array([[0, -self.w[2], self.w[1]], [self.w[2], 0, -self.w[0]],
[-self.w[1], self.w[0], 0]])
M = -self.kR * eR - self.kw * eR + np.cross(
self.w, self.Inertia *
self.w) - self.Inertia * (w_h * self.R.T * self.R_des * self.w_des
- self.R.T * self.R_des * self.w_dot_des)
U = np.array([F, M[0], M[1], M[2]])
R2 = self.Kinv * U
R = np.array(
[np.sqrt(R2[0]),
np.sqrt(R2[1]),
np.sqrt(R2[2]),
np.sqrt(R2[3])])
actuators = Actuators()
actuators.angular_velocities = R
actuators.header.stamp = rospy.Time.now()
self.actuators_pub.publish(actuators)
def __init__(self):
"""
Quadrotor position resetter
"""
quadrotor = "hummingbird"
# That is for us to command a new trajectory
trajectory_topic = "command_trajectory"
# Topic to get feedback from the quadrotor
odometry_topic = "odometry_sensor1/odometry"
# Topic to send commands to quadrotor
actuators_topic = "command/motor_speed"
# Resettting quadrotor
reset_topic = "/gazebo/set_model_state"
# Sync publisher (send syncing messages)
sync_topic = "/world_control"
self.start_time = time.time()
self.odo_msg_count = 0
#Initializing the node
rospy.init_node('quadrotor_env', anonymous=True)
action_publisher = rospy.Publisher(quadrotor + "/" + actuators_topic,
Actuators,
queue_size=1)
# Waiting for reset service to appear
rospy.wait_for_service(reset_topic)
reset_service = rospy.ServiceProxy(reset_topic, SetModelState)
# Resetting
self.reset(reset_service)
for i in range(10):
actuator_msg = Actuators()
actuator_msg.angular_velocities = 0. * np.array([1., 1., 1., 1.])
action_publisher.publish(actuator_msg)
rospy.sleep(.1)
# print('Motors are reset: ', actuator_msg)
print("Motors are reset")
