class PositionControllerNode_ChihChun(object):
"""ROS interface for controlling the Crazyflie 2 in Gazebo."""
# write code here to define node publishers and subscribers
# publish to /crazyflie2/command/motor_speed topic
# subscribe to /crazyflie2/command/trajectory and /crazyflie2/odometry
def __init__(self):
# Publishers: rotor velocities
self.pub_rotor_vel = rospy.Publisher('/crazyflie2/command/motor_speed',
Actuators,
queue_size=1)
# Subscribers: desired posn, current posn
#self._goal_msg = MultiDOFJointTrajectory()
#self.sub_goal = rospy.Subscriber('crazyflie2/command/trajectory', MultiDOFJointTrajectory, self._goal_callback)
self._currpos_msg = Odometry()
self.sub_currpos = rospy.Subscriber('/crazyflie2/odometry', Odometry,
self._currpos_callback)
# end of publisher/subscriber
# initialize other parameters
self.data_list = [[
'Time', 'Desired_x', 'Desired_y', 'Desired_z', 'Actual_x',
'Actual_y', 'Actual_z', 'Desired_yaw', 'Desired_roll',
'Desired_pitch', 'Desired_yaw_r', 'Desired_climb_r', 'Actual_roll',
'Actual_pitch', 'Actual_yaw_r', 'Actual_climb_r'
]]
self.start = [1.0, 1.0]
self.wp_id = 1
self.previous_time = 0.0
self.change_time = -100.0
self.controller = PositionController()
self.init_time = rospy.get_time()
# initialize position and roll, pitch, yaw
self._x = 0
self._y = 0
self._z = 0
self.roll = 0
self.pitch = 0
self.yaw = 0
self._z_old = 0.0
self._z_oold = 0.0
self.yaw_old = 0.0
def set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch,
yaw, thrust): # change arg names?
# get conversions
rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie(
pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust)
rotor_velocities = rotorvel_converter.CalculateRotorVelocities()
# publish rotor velocities to Actuator
msg = Actuators()
msg.angular_velocities = rotor_velocities
msg.header.stamp = self._currpos_msg.header.stamp
self.pub_rotor_vel.publish(msg)
def _currpos_callback(self, msg):
self._currpos_msg = msg
def get_data(self):
self._z_oold = self._z_old
self._z_old = self._z
self.yaw_old = self.yaw
# self._pos = self._vicon_msg.transform.translation
self._x = self._currpos_msg.pose.pose.position.x
self._y = self._currpos_msg.pose.pose.position.y
self._z = self._currpos_msg.pose.pose.position.z
# Convert to quaternion object for use by euler_from_quaternion()
quaternion = np.array([
self._currpos_msg.pose.pose.orientation.x,
self._currpos_msg.pose.pose.orientation.y,
self._currpos_msg.pose.pose.orientation.z,
self._currpos_msg.pose.pose.orientation.w
])
# Determine the euler angles
euler = euler_from_quaternion(quaternion) # change? can i use this?
self.roll = euler[0]
self.pitch = euler[1]
self.yaw = euler[2]
# Determine the euler angular rates p,q,r and thrust
self.p = self._currpos_msg.twist.twist.angular.x
self.q = self._currpos_msg.twist.twist.angular.y
self.r = self._currpos_msg.twist.twist.angular.z
#self.thrust = self._currpos_msg.twist.twist.linear.z
self.x_d = 1.0
self.y_d = 1.0
self.z_d = 1.0
self.yaw_d = 0.0
self.vx_d = 0.0
self.vy_d = 0.0
self.vz_d = 0.0
# desired position
#self.x_d = self._goal_msg.points.transforms.translation.x
#self.y_d = self._goal_msg.points.transforms.translation.y
#self.z_d = self._goal_msg.points.transforms.translation.z
# desired orientation
#self.quat = np.array([self._goal_msg.points.transforms.rotation.x,
# self._goal_msg.points.transforms.rotation.y,
# self._goal_msg.points.transforms.rotation.z,
# self._goal_msg.points.transforms.rotation.w])
#euler = euler_from_quaternion(self.quat)
#self.yaw_d = euler[2]
# desired velocities
#self.vx_d = self._goal_msg.points.velocities.linear.x # may need to change in future
#self.vy_d = self._goal_msg.points.velocities.linear.y
#self.vz_d = self._goal_msg.points.velocities.linear.z
def iteration(self, event):
current_time = rospy.get_time() - self.init_time
dt = current_time - self.previous_time
self.previous_time = current_time
#print(dt)
if dt == 0:
dt = 0.01
act_climb_r = (self._z - 2 * self._z_old + self._z_oold) / (dt**2)
print("z_dd_real: ", act_climb_r)
#act_yaw_r = (self.yaw - self.yaw_old)/dt
self.get_data()
roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command(
self._x, self._y, self._z, self.roll, self.pitch, self.yaw,
self.x_d, self.y_d, self.z_d, self.vx_d, self.vy_d, self.vz_d,
self.yaw_d)
#self.set_vel(roll_c, pitch_c, z_dot_c, yaw_dot_c)
self.thrust_c = z_dot_c #+ 12000
print("thrust: ", z_dot_c)
self.set_rotor_vel(pitch_c, roll_c, yaw_dot_c, self.p, self.q, self.r,
self.roll, self.pitch, self.yaw, self.thrust_c)
class position_controller_flock_node(object):
"""ROS interface for controlling up to four Cf2.0's in Gazebo and running flocking algorithm."""
def __init__(self, uav_ids, init, fin, vx_ds, vy_ds, vz_ds, yaw_ds):
self.cf_ids = uav_ids
self.number_of_agents = np.shape(uav_ids)[0]
self.takoff_alt = 1.0 # change?
self._pos = {}
self._vel = {}
self._quat = {}
self._dist_to_goal = {}
self._euler_angles = {}
self._euler_angular_rates = {}
self._z_old = {}
self._z_oold = {}
self.yaw_old = {}
self.radius = 0.15
self.d_star = self.radius
self.MaxVelo = 1.0
# Tune these
self.c1 = 7 * 4.5
self.c2 = 9 * 4.5
self.RepulsiveGradient = 7.5 * 100
self.previous_time = 0.0
self.change_time = -100.0
self.controller = PositionController()
self.init_time = rospy.get_time()
### Publish ###
# waypoint messages
self.goal_msg_0, self.goal_msg_1, self.goal_msg_2, self.goal_msg_3 = PoseStamped(
), PoseStamped(), PoseStamped(), PoseStamped()
self.goal_msgs = {
'0': self.goal_msg_0,
'1': self.goal_msg_1,
'2': self.goal_msg_2,
'3': self.goal_msg_3
}
# rotor velocities messages
# self.cmdV_msg_0, self.cmdV_msg_1, self.cmdV_msg_2, self.cmdV_msg_3 = Actuators(), Actuators(), Actuators(), Actuators()
# self.rotor_vel_msgs = {
# '0' : self.cmdV_msg_0,
# '1' : self.cmdV_msg_1,
# '2' : self.cmdV_msg_2,
# '3' : self.cmdV_msg_3
# }
### Subscribe ###
# odometry messages
self._currpos_callbacks = {
'0': self._currpos_callback_0,
'1': self._currpos_callback_1,
'2': self._currpos_callback_2,
'3': self._currpos_callback_3
}
# set parameters
self.initials, self.finals = {}, {}
self.goal_pubs, self.cmdVtemp_pubs, self.cmdV_pubs, self.takeoffs, self.lands = {}, {}, {}, {}, {}
for index, cf_id in enumerate(self.cf_ids):
self.initials[str(cf_id)] = init[index][:]
self._z_old[str(cf_id)] = 0.0
self._z_oold[str(cf_id)] = 0.0
self.yaw_old[str(cf_id)] = 0.0
self.finals[str(cf_id)] = fin[index][:]
self.goal_pubs[str(cf_id)] = rospy.Publisher('/crazyflie2_' +
str(cf_id) + '/goal',
PoseStamped,
queue_size=1)
self.cmdV_pubs[str(cf_id)] = rospy.Publisher(
'/crazyflie2_' + str(cf_id) + '/command/motor_speed',
Actuators,
queue_size=1)
rospy.Subscriber("/crazyflie2_" + str(cf_id) + "/odometry",
Odometry, self._currpos_callbacks[str(cf_id)])
self.vx_d = vx_ds
self.vy_d = vy_ds
self.vz_d = vz_ds
self.yaw_d = yaw_ds
self.takeoffed = False
self.reached_1st = False
self.flag = {'flying': 0, 'landed': 0, 'preland': 0}
# self.vstate = {
# 'takeoff' : self.do_takeoff,
# 'wpnav' : self.do_wpnav,
# 'land' : self.do_land
# }
# Position controller
self.controller = PositionController()
def set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch,
yaw, thrust): # change arg names?
# get conversions
rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie(
pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust)
rotor_velocities = rotorvel_converter.CalculateRotorVelocities()
# publish rotor velocities to Actuator
msg = Actuators()
msg.angular_velocities = rotor_velocities
msg.header.stamp = self._currpos_msg.header.stamp
self.pub_rotor_vel.publish(msg)
def _currpos_callback(self, msg):
self._currpos_msg = msg
def get_data(self, id):
self._z_oold[str(id)] = self._z_old[str(id)]
self._z_old[str(id)] = self._pos[str(id)].z # current z
print("self._euler_angles[str(id)]: ", self._euler_angles[str(id)])
self.yaw_old[str(id)] = self._euler_angles[str(id)][2] # current yaw
def _currpos_callback_0(self, data):
self._pos['0'] = data.pose.pose.position
self._vel['0'] = data.twist.twist.linear
self._quat['0'] = np.array([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
]) # gives quaternion object
#print("quat 0: ", self._quat['0'])
self._euler_angles['0'] = euler_from_quaternion(
self._quat['0']) # gives roll, pitch, yaw
#print("euler angles 0: ", self._euler_angles['0'])
self._euler_angular_rates['0'] = np.asarray(
data.twist.twist.angular) # gives p, q, r
def _currpos_callback_1(self, data):
self._pos['1'] = data.pose.pose.position
self._vel['1'] = data.twist.twist.linear
self._quat['1'] = np.array([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
]) # gives quaternion object
self._euler_angles['1'] = euler_from_quaternion(
self._quat['1']) # gives roll, pitch, yaw
#print(self._euler_angles['1'], type(self._euler_angles['1']))
self._euler_angular_rates['1'] = np.asarray(
data.twist.twist.angular) # gives p, q, r
def _currpos_callback_2(self, data):
self._pos['2'] = data.pose.pose.position
self._vel['2'] = data.twist.twist.linear
self._quat['2'] = np.array([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
]) # gives quaternion object
self._euler_angles['2'] = euler_from_quaternion(
self._quat['2']) # gives roll, pitch, yaw
self._euler_angular_rates['2'] = np.asarray(
data.twist.twist.angular) # gives p, q, r
def _currpos_callback_3(self, data):
self._pos['3'] = data.pose.pose.position
self._vel['3'] = data.twist.twist.linear
self._quat['3'] = np.array([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
]) # gives quaternion object
self._euler_angles['3'] = euler_from_quaternion(
self._quat['3']) # gives roll, pitch, yaw
self._euler_angular_rates['3'] = np.asarray(
data.twist.twist.angular) # gives p, q, r
def update_pos(self, id, pos):
self.goal_msgs[id].header.seq += 1
self.goal_msgs[id].header.frame_id = '/world' # change?
self.goal_msgs[id].header.stamp = rospy.Time.now()
self.goal_msgs[id].pose.position.x = pos[0]
self.goal_msgs[id].pose.position.y = pos[1]
self.goal_msgs[id].pose.position.z = pos[2]
self.goal_msgs[id].pose.orientation.x = 0
self.goal_msgs[id].pose.orientation.y = 0
self.goal_msgs[id].pose.orientation.z = 0
self.goal_msgs[id].pose.orientation.w = 1
def update_rotor_vels(self, id):
# this function takes in the UAV's id and computes+publishes the rotor velocities to that UAV
# get z_oold, z_old, and desired position/yaw
self.get_data(id)
# get roll/pitch/yawrate/thrust commands from position controller
roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command(
self._pos[str(id)].x,
self._pos[str(id)].y,
self._pos[str(id)].z, # x,y,z
self._euler_angles[str(id)][0],
self._euler_angles[str(id)][1],
self._euler_angles[str(id)][0][2], # change? roll, pitch, yaw
self.initials[str(id)][0],
self.initials[str(id)][1],
self.initials[str(id)][2], # change? xd, yd, zd
self.vx_d[int(id)],
self.vy_d[int(id)],
self.vz_d[int(id)],
self.yaw_d[int(id)])
# obtain p,q,r/roll,pitch,yaw for UAV id from odometry subscription
p = self._euler_angular_rates[str(id)][0]
q = self._euler_angular_rates[str(id)][1]
r = self._euler_angular_rates[str(id)][2]
roll = self._euler_angles[str(id)][0]
pitch = self._euler_angles[str(id)][1]
yaw = self._euler_angles[str(id)][2]
# convert above commands to rotor velocity commands
rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie(
pitch_c, roll_c, yaw_dot_c, p, q, r, roll, pitch, yaw, z_dot_c)
rotor_velocities = rotorvel_converter.CalculateRotorVelocities(
) # this yields a 4-element list
# publish rotor velocities to Actuator
rotorvel_msg = Actuators()
rotorvel_msg.angular_velocities = rotor_velocities
#rotorvel_msg.header.stamp = self._currpos_msg.header.stamp
self.cmdV_pubs[str(cf_id)].publish(rotorvel_msg)
def publish_msg(self):
for cf_id in self.cf_ids:
self.goal_pubs[str(cf_id)].publish(self.goal_msgs[str(cf_id)])
def do_wpnav(self):
print('Navigating!!')
# Check all cfs reached assigened takeoff alt
if not self.takeoffed:
count = 0
for cf_id in self.cf_ids:
if abs(self._pos[str(cf_id)][2] - self.takoff_alt) < 0.05:
count += 1
position_d = np.array([
self._pos[str(cf_id)][0], self._pos[str(cf_id)][1],
self.takoff_alt
])
self.update_pos(str(cf_id), position_d)
if count == self.number_of_agents:
self.takeoffed = True
# Check all cfs reached their initial points
elif not self.reached_1st:
count = 0
for cf_id in self.cf_ids:
if np.linalg.norm(self._pos[str(cf_id)] -
self.initials[str(cf_id)]) < 0.1:
count += 1
position_d = np.array([
self.initials[str(cf_id)][0], self.initials[str(cf_id)][1],
self.initials[str(cf_id)][2]
])
self.update_pos(str(cf_id), position_d)
if count == self.number_of_agents:
self.reached_1st = True
print('Initial points reached!!')
raw_input(
"Press Enter to continue...") # use input() for python3
# flocking path planning
else:
# call flocking and get the desired position of next timestep
for cf_id in self.cf_ids:
other_cfs = self.cf_ids[:]
other_cfs.remove(cf_id)
self._dist_to_goal[str(cf_id)] = np.linalg.norm(
self._pos[str(cf_id)] - self.finals[str(cf_id)])
force = -self.c1 * self._vel[str(cf_id)] - self.c2 * (
self._pos[str(cf_id)] - self.finals[str(cf_id)])
for other_cf in other_cfs:
dist_v = self._pos[str(other_cf)] - self._pos[str(cf_id)]
dist = np.linalg.norm(dist_v)
d = 2 * self.radius + self.d_star
CommunicationRadious = np.cbrt(
3 * np.square(self.MaxVelo) /
(2 * self.RepulsiveGradient)) + d
if dist < CommunicationRadious:
ForceComponent = -self.RepulsiveGradient * np.square(
dist - CommunicationRadious)
force += ForceComponent * (dist_v) / dist
velocity_d = self._vel[str(cf_id)] + force * self.dt
position_d = self._pos[str(cf_id)] + velocity_d * self.dt
position_d[2] = 1.0 # for 2D sim
self.update_pos(str(cf_id), position_d)
# Check all cfs reached their final points
if self._dist_to_goal[max(self._dist_to_goal)] < 0.2:
self.flag['preland'] = 1
def iteration(self, event):
# publish goal messages for each uav
self.publish_msg()
# publish rotor velocities for each uav
for index, cf_id in enumerate(self.cf_ids):
self.update_rotor_vels(cf_id)
