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