class ROSControllerNode(object):
#Initialize Ros Controller
def __init__(self):
self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32)
self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped, self.update_vicon)
self.desired_position_data = rospy.Subscriber(
'/desired_positions', PoseStamped, self.update_desired_position)
self.start_stop_command = rospy.Subscriber('/start_stop_toggle',
String, self.update_state)
self.request_pic = rospy.Publisher('/take_pic', String, queue_size=10)
self.request_land = rospy.Publisher('/ask_land', String, queue_size=2)
self.pub_errors = rospy.Publisher('/errors',
PoseStamped,
queue_size=100)
#self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100)
self.run_state = False
self.translation_x_old = 0.0
self.translation_y_old = 0.0
self.translation_z_old = 0.0
self.z_velocity_old = 0.0
self.translation_x_desired = 0.0
self.translation_y_desired = 0.0
self.translation_z_desired = 0.0
self.translation_x = 0.0
self.translation_y = 0.0
self.translation_z = 0.0
self.rotation_x = 0.0
self.rotation_y = 0.0
self.rotation_z = 0.0
self.rotation_w = 1.0
self.rotation_x_desired = 0.0
self.rotation_y_desired = 0.0
self.rotation_z_desired = 0.0
self.rotation_w_desired = 1.0
self.pic_counter = 0
self.pic_x = [
0, -2, -2, -2, -2, -2, -2, -2, -2, -2, -1, -1, -1, -1, -1, -1, -1,
-1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2,
2, 2, 2, 2, 2, 2, 2
]
self.pic_y = [
0, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000,
2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000, -1.0000,
-1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000,
1.0000, 1.5000, 2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000,
-1.0000, -1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0,
0.5000, 1.0000, 1.5000, 2.0000
]
self.pic_z = [
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3,
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3,
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3,
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3
]
self.waypoint_counter = 0
x = np.loadtxt(
'/home/mason/aer1217/labs/src/aer1217_ardrone_simulator/scripts/order.txt',
delimiter=None)
self.order_pic = x.astype(int)
self.x_position_array = [1, 4.26, 0.88, 4.33, 7.69, 1]
self.y_position_array = [1, 1.23, 5.48, 8.04, 4.24, 1]
self.time_interval = 0
self.current_time = rospy.get_time()
self.old_time = rospy.get_time()
self.postcom = PositionController()
# Run the publish commands at 100 Hz
self.onboard_loop_frequency = 100.
self.pic_taking_speed = 10.
self.waypoint_check_speed = 10.
# Run this ROS node at the onboard loop frequency
self.nutjobcase = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency), self.run_process)
#self.wedfsj = rospy.Timer(rospy.Duration(1. / self.pic_taking_speed), self.pic_initiator)
self.wedfsj = rospy.Timer(
rospy.Duration(1. / self.waypoint_check_speed),
self.mission_planner)
self.postcom = PositionController()
def update_state(self, engage_command):
if (engage_command.data == "start"):
self.run_state = True
elif (engage_command.data == "stop"):
self.run_state = False
#Publish Errors
def publish_errors(self):
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = self.translation_x_desired - self.translation_x
msg.pose.position.y = self.translation_y_desired - self.translation_y
msg.pose.position.z = self.translation_z_desired - self.translation_z
euler_angle = euler_from_quaternion([
self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w
])
angle_yaw = euler_angle[2]
msg.pose.orientation.x = angle_yaw
self.pub_errors.publish(msg)
#Controlling Process
def process_commands(self):
#Create Orientation Quaternion Arrays
self.rotation = np.array([
self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w
])
self.rotation_desired = np.array([
self.rotation_x_desired, self.rotation_y_desired,
self.rotation_z_desired, self.rotation_w_desired
])
#Calulate Time Interval from previous process
current_time = rospy.get_time()
self.time_interval = current_time - self.old_time
self.old_time = current_time
#Send Required Values to Position Controller
self.postcom.update_pos_controller_values(
self.translation_x, self.translation_y, self.translation_z,
self.rotation, self.translation_x_desired,
self.translation_y_desired, self.translation_z_desired,
self.rotation_desired, self.time_interval)
#Recieve Calculation of Commands
returnvalue = self.postcom.calculate_commands()
[roll, pitch, yaw, z_dot] = returnvalue
#Publish Errors
self.publish_errors()
#Call function to publish commands
self.set_vel(roll, pitch, z_dot, yaw)
# Take desired position value
def update_desired_position(self, desired_position_data):
#Update Current Position
self.translation_x_desired = desired_position_data.pose.position.x
self.translation_y_desired = desired_position_data.pose.position.y
self.translation_z_desired = desired_position_data.pose.position.z
#Update Current Orientation
self.rotation_x_desired = desired_position_data.pose.orientation.x
self.rotation_y_desired = desired_position_data.pose.orientation.y
self.rotation_z_desired = desired_position_data.pose.orientation.z
self.rotation_w_desired = desired_position_data.pose.orientation.w
# Obtain value from VICON
def update_vicon(self, vicon_data_msg):
#Update Current Position
self.translation_x = vicon_data_msg.transform.translation.x
self.translation_y = vicon_data_msg.transform.translation.y
self.translation_z = vicon_data_msg.transform.translation.z
#Update Current Orientation
self.rotation_x = vicon_data_msg.transform.rotation.x
self.rotation_y = vicon_data_msg.transform.rotation.y
self.rotation_z = vicon_data_msg.transform.rotation.z
self.rotation_w = vicon_data_msg.transform.rotation.w
self.current_time = vicon_data_msg.header.stamp
#Publish velocity to /cmd_vel_RHC
def set_vel(self, roll, pitch, z_dot, yaw_dot):
msg = Twist()
msg.linear.x = roll
msg.linear.y = pitch
msg.linear.z = z_dot
msg.angular.z = yaw_dot
self.pub_vel.publish(msg)
pass
def run_process(self, random):
if (self.run_state):
self.process_commands()
def pic_initiator(self, random):
margin = 0.25
picture_point_x = self.pic_x[self.pic_counter]
picture_point_y = self.pic_y[self.pic_counter]
picture_point_z = self.pic_z[self.pic_counter]
if (((picture_point_x - margin) < self.translation_x <
(picture_point_x + margin))
and ((picture_point_y - margin) < self.translation_y <
(picture_point_y + margin))
and ((picture_point_z - margin) < self.translation_z <
(picture_point_z + margin))):
capture_message = "capture"
self.pic_counter = self.pic_counter + 1
self.request_pic.publish(capture_message)
def mission_planner(self, random):
margin = 0.25
x = self.order_pic[self.waypoint_counter]
waypoint_x = self.x_position_array[x]
waypoint_y = self.x_position_array[x]
if (((waypoint_x - margin) < self.translation_x <
(waypoint_x + margin))
and ((waypoint_y - margin) < self.translation_y <
(waypoint_y + margin))):
self.waypoint_counter = self.waypoint_counter + 1
if (self.waypoint_counter == len(self.order_pic)):
end_message = "land"
self.request_land.publish(end_message)
print("reached")
class ROSControllerNode(object):
def __init__(self):
"""ROS interface for controlling the Parrot ARDrone in the Vicon Lab."""
self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32)
#self.random_test = rospy.Publisher('/random_stuff', Twist, queue_size = 32)
self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped, self.update_vicon)
#self.start_value = rospy.Subscriber('/start_execution', String, self.run_stuff)
self.desired_position_data = rospy.Subscriber(
'/desired_positions', PoseStamped, self.update_desired_position)
self.pub_check = rospy.Publisher('/check_mate', String, queue_size=100)
self.pub_counter = 0
self.translation_x_old = 0.0
self.translation_y_old = 0.0
self.translation_z_old = 0.0
self.z_velocity_old = 0.0
self.translation_x_desired = 0.0
self.translation_y_desired = 0.0
self.translation_z_desired = 0.5
self.translation_x = 0.0
self.translation_y = 0.0
self.translation_z = 0.0
self.rotation_x = 0.0
self.rotation_y = 0.0
self.rotation_z = 0.0
self.rotation_w = 1.0
self.rotation_x_desired = 0.0
self.rotation_y_desired = 0.0
self.rotation_z_desired = 0.0
self.rotation_w_desired = 1.0
self.time_interval = 0
self.current_time = rospy.get_time()
self.old_time = rospy.get_time()
self.postcom = PositionController()
"""
# Run the onboard controller at 50 Hz
self.onboard_loop_frequency = 50.
# Run this ROS node at the onboard loop frequency
self.nutjobcase = rospy.Timer(rospy.Duration(1. /
self.onboard_loop_frequency), self.process_commands)
self.postcom = PositionController()
"""
"""
new_message=String()
new_message.data="true"
self.pub_check.publish(new_message)
"""
def process_commands(self):
self.rotation = np.array([
self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w
])
self.rotation_desired = np.array([
self.rotation_x_desired, self.rotation_y_desired,
self.rotation_z_desired, self.rotation_w_desired
])
current_time = rospy.get_time()
self.time_interval = current_time - self.old_time
self.old_time = current_time
#postcom = PositionController(self.translation_x_old,self.translation_y_old,self.translation_z_old,self.translation_x, self.translation_y, self.translation_z, self.rotation, self.translation_x_desired, self.translation_y_desired, self.translation_z_desired, self.rotation_desired, self.z_velocity_old,self.time_interval)
self.postcom.update_pos_controller_values(
self.translation_x, self.translation_y, self.translation_z,
self.rotation, self.translation_x_desired,
self.translation_y_desired, self.translation_z_desired,
self.rotation_desired, self.time_interval)
returnvalue = self.postcom.member()
"""
roll = returnvalue[0]
pitch = returnvalue[1]
yaw = returnvalue[2]
z_dot = returnvalue[3]
old_x = returnvalue[4]
old_y = returnvalue[5]
old_z = returnvalue[6]
old_velocity_z = returnvalue[7]
"""
[roll, pitch, yaw, z_dot, old_x, old_y, old_z,
old_velocity_z] = returnvalue
yaw_dot = 0
self.set_vel(roll, pitch, z_dot, yaw)
self.store_old_values(old_x, old_y, old_z, old_velocity_z)
# Take desired position value
def update_desired_position(self, desired_position_data):
self.translation_x_desired = desired_position_data.pose.position.x
self.translation_y_desired = desired_position_data.pose.position.y
self.translation_z_desired = desired_position_data.pose.position.z
self.rotation_x_desired = desired_position_data.pose.orientation.x
self.rotation_y_desired = desired_position_data.pose.orientation.y
self.rotation_z_desired = desired_position_data.pose.orientation.z
self.rotation_w_desired = desired_position_data.pose.orientation.w
self.run_stuff()
self.process_commands()
test_var = 0
"""
while(test_var==0):
if (((self.translation_x_desired - 0.10) < self.translation_x < (self.translation_x_desired + 0.10)) and
((self.translation_y_desired - 0.10) < self.translation_y < (self.translation_y_desired + 0.10)) and
((self.translation_z_desired - 0.10) < self.translation_z < (self.translation_z_desired + 0.10))):
self.pub_check.publish("true")
test_var=1
else:
self.pub_check.publish("false")
"""
# Obtain value from VICON
def update_vicon(self, vicon_data_msg):
self.translation_x = vicon_data_msg.transform.translation.x
self.translation_y = vicon_data_msg.transform.translation.y
self.translation_z = vicon_data_msg.transform.translation.z
self.rotation_x = vicon_data_msg.transform.rotation.x
self.rotation_y = vicon_data_msg.transform.rotation.y
self.rotation_z = vicon_data_msg.transform.rotation.z
self.rotation_w = vicon_data_msg.transform.rotation.w
self.current_time = vicon_data_msg.header.stamp
def store_old_values(self, old_x, old_y, old_z, z_velocity_old):
self.translation_x_old = old_x
self.translation_y_old = old_y
self.translation_z_old = old_z
self.z_velocity_old = z_velocity_old
def set_vel(self, roll, pitch, z_dot, yaw_dot):
msg = Twist()
msg.linear.x = roll
msg.linear.y = pitch
msg.linear.z = z_dot
msg.angular.z = yaw_dot
self.pub_vel.publish(msg)
# 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 run_stuff(self):
while (True):
if (((self.translation_x_desired - 0.15) < self.translation_x <
(self.translation_x_desired + 0.15)) and
((self.translation_y_desired - 0.15) < self.translation_y <
(self.translation_y_desired + 0.15)) and
((self.translation_z_desired - 0.15) < self.translation_z <
(self.translation_z_desired + 0.15))):
self.pub_check.publish("true")
break
#else:
#self.process_commands()
#self.pub_check.publish("false")
#time.sleep(5)
pass
class ROSControllerNode(object):
#Initialize Ros Controller
def __init__(self):
self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32)
self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped, self.update_vicon)
self.desired_position_data = rospy.Subscriber(
'/desired_positions', PoseStamped, self.update_desired_position)
self.pub_errors = rospy.Publisher('/errors',
PoseStamped,
queue_size=100)
#self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100)
self.translation_x_old = 0.0
self.translation_y_old = 0.0
self.translation_z_old = 0.0
self.z_velocity_old = 0.0
self.translation_x_desired = 0.0
self.translation_y_desired = 0.0
self.translation_z_desired = 0.0
self.translation_x = 0.0
self.translation_y = 0.0
self.translation_z = 0.0
self.rotation_x = 0.0
self.rotation_y = 0.0
self.rotation_z = 0.0
self.rotation_w = 1.0
self.rotation_x_desired = 0.0
self.rotation_y_desired = 0.0
self.rotation_z_desired = 0.0
self.rotation_w_desired = 1.0
self.time_interval = 0
self.current_time = rospy.get_time()
self.old_time = rospy.get_time()
self.postcom = PositionController()
# Run the publish commands at 100 Hz
self.onboard_loop_frequency = 100.
# Run this ROS node at the onboard loop frequency
self.nutjobcase = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency),
self.process_commands)
self.postcom = PositionController()
#Publish Errors
def publish_errors(self):
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = self.translation_x_desired - self.translation_x
msg.pose.position.y = self.translation_y_desired - self.translation_y
msg.pose.position.z = self.translation_z_desired - self.translation_z
euler_angle = euler_from_quaternion([
self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w
])
angle_yaw = euler_angle[2]
msg.pose.orientation.x = angle_yaw
self.pub_errors.publish(msg)
#Controlling Process
def process_commands(self, random):
#Create Orientation Quaternion Arrays
self.rotation = np.array([
self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w
])
self.rotation_desired = np.array([
self.rotation_x_desired, self.rotation_y_desired,
self.rotation_z_desired, self.rotation_w_desired
])
#Calulate Time Interval from previous process
current_time = rospy.get_time()
self.time_interval = current_time - self.old_time
self.old_time = current_time
#Send Required Values to Position Controller
self.postcom.update_pos_controller_values(
self.translation_x, self.translation_y, self.translation_z,
self.rotation, self.translation_x_desired,
self.translation_y_desired, self.translation_z_desired,
self.rotation_desired, self.time_interval)
#Recieve Calculation of Commands
returnvalue = self.postcom.calculate_commands()
"""
roll = returnvalue[0]
pitch = returnvalue[1]
yaw = returnvalue[2]
z_dot = returnvalue[3]
"""
[roll, pitch, yaw, z_dot] = returnvalue
#Publish Errors
self.publish_errors()
#Call function to publish commands
self.set_vel(roll, pitch, z_dot, yaw)
# Take desired position value
def update_desired_position(self, desired_position_data):
#Update Current Position
self.translation_x_desired = desired_position_data.pose.position.x
self.translation_y_desired = desired_position_data.pose.position.y
self.translation_z_desired = desired_position_data.pose.position.z
#Update Current Orientation
self.rotation_x_desired = desired_position_data.pose.orientation.x
self.rotation_y_desired = desired_position_data.pose.orientation.y
self.rotation_z_desired = desired_position_data.pose.orientation.z
self.rotation_w_desired = desired_position_data.pose.orientation.w
# Obtain value from VICON
def update_vicon(self, vicon_data_msg):
#Update Current Position
self.translation_x = vicon_data_msg.transform.translation.x
self.translation_y = vicon_data_msg.transform.translation.y
self.translation_z = vicon_data_msg.transform.translation.z
#Update Current Orientation
self.rotation_x = vicon_data_msg.transform.rotation.x
self.rotation_y = vicon_data_msg.transform.rotation.y
self.rotation_z = vicon_data_msg.transform.rotation.z
self.rotation_w = vicon_data_msg.transform.rotation.w
self.current_time = vicon_data_msg.header.stamp
#Publish velocity to /cmd_vel_RHC
def set_vel(self, roll, pitch, z_dot, yaw_dot):
msg = Twist()
msg.linear.x = roll
msg.linear.y = pitch
msg.linear.z = z_dot
msg.angular.z = yaw_dot
self.pub_vel.publish(msg)
pass
