class ROSControllerNode(object):
"""ROS interface for controlling the Parrot ARDrone in the Vicon Lab."""
# write code here to define node publishers and subscribers
# publish to /cmd_vel topic
# subscribe to /vicon/ARDroneCarre/ARDroneCarre for position and attitude feedback
def __init__(self):
self.position_controller = PositionController()
self.sub_desired_state = rospy.Subscriber(
'/aer1217_ardrone/desired_state', DesiredStateMsg,
self.update_quadrotor_desired_state)
#######################################################################
# input is the estimated state x,y,z,roll,pitch,yaw from VICON
self.sub_estimated_state_vicon = rospy.Subscriber(
'/vicon/ARDroneCarre/ARDroneCarre', TransformStamped,
self.update_quadrotor_estimated_state_from_vicon)
#######################################################################
# output roll,pitch,yaw_rate,climb_rate commands
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=30)
self.pub_cmd_vel_RHC = rospy.Publisher('cmd_vel_RHC',
Twist,
queue_size=30)
#######################################################################
self.onboard_loop_frequency = 200.
self.pub_cmd_vel_timer = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency),
self.send_cmd_vel)
def update_quadrotor_desired_state(self, desired_state_msg):
self.position_controller.set_desired_state(
desired_state_msg.x[0], desired_state_msg.x[1],
desired_state_msg.x[2], desired_state_msg.x_dot[0],
desired_state_msg.x_dot[1], desired_state_msg.x_dot[2],
desired_state_msg.x_2dot[0], desired_state_msg.x_2dot[1],
desired_state_msg.x_2dot[2], desired_state_msg.yaw,
desired_state_msg.yaw_rate)
def update_quadrotor_estimated_state_from_vicon(self, estimated_state_msg):
self.estimated_state = estimated_state_msg
x = estimated_state_msg.transform.translation.x
y = estimated_state_msg.transform.translation.y
z = estimated_state_msg.transform.translation.z
a = estimated_state_msg.transform.rotation.x
b = estimated_state_msg.transform.rotation.y
c = estimated_state_msg.transform.rotation.z
d = estimated_state_msg.transform.rotation.w
(roll, pitch, yaw) = euler_from_quaternion([a, b, c, d])
self.position_controller.set_estimated_state(x, y, z, roll, pitch, yaw)
def send_cmd_vel(self, event):
# output roll,pitch,yaw_rate,climb_rate commands
(pitch, roll, yaw_rate, climb_rate
) = self.position_controller.get_pitch_roll_yawrate_climbrate()
cmd_vel = Twist()
cmd_vel.linear.x = roll #roll angle in radian
cmd_vel.linear.y = pitch #pitch angle in radian
cmd_vel.linear.z = climb_rate #climb rate
cmd_vel.angular.x = 0
cmd_vel.angular.y = 0
cmd_vel.angular.z = yaw_rate #raw rate in radian per second
self.pub_cmd_vel.publish(cmd_vel)
self.pub_cmd_vel_RHC.publish(cmd_vel)
