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)
returnvalue = 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)
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
