Ejemplo n.º 1
0
class ROSControllerNode(object):
    """ROS interface for controlling ARDrone, using position controller"""
    def __init__(self):
        """Initialize the ROSControllerNode class."""
        self.model_name = 'ARDroneCarre'

        # Publishers
        self.pub_cmd_vel = rospy.Publisher('cmd_vel_RHC',
                                           Twist,
                                           queue_size=300)

        # Subscribers
        self.sub_vicon_data = rospy.Subscriber(
            '/vicon/{0}/{0}'.format(self.model_name), TransformStamped,
            self.process_vicon_data)

        self.sub_des_pos = rospy.Subscriber('des_pos', String,
                                            self.process_desired_position)

        # Initialize messages for publishing
        self.cmd_vel = Twist()

        # Run the onboard controller at 200 Hz
        self.onboard_loop_frequency = 200.

        # Calling the position controller to pass the data
        self.pos_class = PositionController()

        # Run this ROS node at the onboard loop frequency
        self.run_pub_cmd_vel = rospy.Timer(
            rospy.Duration(1. / self.onboard_loop_frequency),
            self.process_commands)

        # Initialize time
        self.nonunix_time = 0
        self.old_time = rospy.get_time()
        self.start_time = rospy.get_time()
        self.dt = 0

        # Initialize points
        self.current_point = 1
        self.num_points_old = 0

    def process_commands(self, event):
        """Determine the motor speeds and and publishes these."""

        # Calculate time step
        current_time = rospy.get_time()
        self.dt = current_time - self.old_time

        # Get commands from the position controller
        commands = self.pos_class.update_position_controller(self.dt)
        [roll_command, pitch_command, yaw_rate_command,
         climb_rate_command] = commands

        # Set cmd_vel message values to commands
        self.cmd_vel.linear.x = roll_command
        self.cmd_vel.linear.y = pitch_command
        self.cmd_vel.linear.z = climb_rate_command
        self.cmd_vel.angular.z = yaw_rate_command

        # Publish commands
        self.pub_cmd_vel.publish(self.cmd_vel)

        # Update timestamp
        self.old_time = current_time

    def process_vicon_data(self, vicon_data_msg):

        self.pos_class.current_trans_x = vicon_data_msg.transform.translation.x
        self.pos_class.current_trans_y = vicon_data_msg.transform.translation.y
        self.pos_class.current_trans_z = vicon_data_msg.transform.translation.z

        self.pos_class.current_rot_x = vicon_data_msg.transform.rotation.x
        self.pos_class.current_rot_y = vicon_data_msg.transform.rotation.y
        self.pos_class.current_rot_z = vicon_data_msg.transform.rotation.z
        self.pos_class.current_rot_w = vicon_data_msg.transform.rotation.w

    def process_desired_position(self, pos_msg):

        # Get message containing desired trajectory coordinates from desired_positions
        self.des_pos_msg = np.fromstring(pos_msg.data, dtype=float, sep=' ')

        # Calculate total number of waypoints
        num_points = self.des_pos_msg.size / 4

        # Update number of waypoints
        if num_points != self.num_points_old:
            self.num_points_old = num_points
            self.current_point = 0

        # Convert 1x12 array into 3 arrays of 4 elements (x,y,z)
        trajectory = np.reshape(self.des_pos_msg, (-1, 4))

        self.pos_class.desired_x = trajectory[self.current_point - 1,
                                              0]  # x coordinates
        self.pos_class.desired_y = trajectory[self.current_point - 1,
                                              1]  # y coordinates
        self.pos_class.desired_z = trajectory[self.current_point - 1,
                                              2]  # z coordinates

        self.nonunix_time += self.dt
        # setting deviation tolerance, the higher tolerance, the faster the drone navigates
        if ((
            (self.pos_class.desired_x - 0.5) < self.pos_class.current_trans_x <
            (self.pos_class.desired_x + 0.5))
                and ((self.pos_class.desired_y - 0.5) <
                     self.pos_class.current_trans_y <
                     (self.pos_class.desired_y + 0.5))
                and ((self.pos_class.desired_z - 0.5) <
                     self.pos_class.current_trans_z <
                     (self.pos_class.desired_z + 0.5))):
            if self.current_point < num_points and self.nonunix_time >= 0.01:
                self.current_point += 1
                self.nonunix_time = 0
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):
        """Initialize the ROSControllerNode class."""

        # Publishers
        """
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', 
                                            Twist,
                                            queue_size = 300)
        """

        self.pub_cmd_vel = rospy.Publisher('cmd_vel_RHC',
                                           Twist,
                                           queue_size=300)

        self.pubTakePic = rospy.Publisher('/ardrone/take_pic',
                                          String,
                                          queue_size=10)

        # Subscribers
        self.model_name = 'ARDroneCarre'

        self.sub_vicon_data = rospy.Subscriber(
            '/vicon/{0}/{0}'.format(self.model_name), TransformStamped,
            self.update_vicon_data)

        self.sub_des_pos = rospy.Subscriber('des_pos', String,
                                            self.update_desired_position)

        # Initialize messages for publishing
        self.cmd_vel_msg = Twist()

        # Run the onboard controller at 200 Hz
        self.onboard_loop_frequency = 200.

        # Calling the position controller to pass the data
        self.pos_class = PositionController()

        # Run this ROS node at the onboard loop frequency
        self.run_pub_cmd_vel = rospy.Timer(
            rospy.Duration(1. / self.onboard_loop_frequency),
            self.update_roll_pitch_yaw)

        self.current_point = 1
        self.nonunix_time = 0
        self.dt = 0

        self.num_points_old = 3

        self.pic_number = 1

        # Keep time for differentiation and integration within the controller
        self.old_time = rospy.get_time()
        self.start_time = rospy.get_time()

    def update_roll_pitch_yaw(self, event):
        """Determine the motor speeds and and publishes these."""

        # Determine the time step for differentiation and integration
        current_time = rospy.get_time()
        self.dt = current_time - self.old_time

        # Get the next set of postioning commands from the position controller
        new_controls = self.pos_class.update_position_controller(self.dt)

        # Rearranging the data for ease
        [new_roll, new_pitch, new_yaw_rate, new_climb_rate] = new_controls

        # Setting the cmd_vel msg values to the desired ones
        self.cmd_vel_msg.linear.x = new_roll
        self.cmd_vel_msg.linear.y = new_pitch
        self.cmd_vel_msg.angular.z = new_yaw_rate
        self.cmd_vel_msg.linear.z = new_climb_rate

        # Publish the motor commands for the ardrone plugin
        self.pub_cmd_vel.publish(self.cmd_vel_msg)

        # Set the old time to the current for future time step calculations
        self.old_time = current_time

    def update_vicon_data(self, vicon_data_msg):
        (self.pos_class.current_trans_x, self.pos_class.current_trans_y,
         self.pos_class.current_trans_z) = (
             vicon_data_msg.transform.translation.x,
             vicon_data_msg.transform.translation.y,
             vicon_data_msg.transform.translation.z)

        (self.pos_class.current_rot_x, self.pos_class.current_rot_y,
         self.pos_class.current_rot_z,
         self.pos_class.current_rot_w) = (vicon_data_msg.transform.rotation.x,
                                          vicon_data_msg.transform.rotation.y,
                                          vicon_data_msg.transform.rotation.z,
                                          vicon_data_msg.transform.rotation.w)

    def update_desired_position(self, pos_msg):
        self.des_pos_msg = np.fromstring(pos_msg.data, dtype=float, sep=' ')

        num_points = self.des_pos_msg.size / 4
        if num_points != self.num_points_old:
            self.num_points_old = num_points
            self.current_point = 1

        trajectory = np.reshape(self.des_pos_msg, (-1, 4))

        self.pos_class.desired_x = trajectory[self.current_point - 1, 0]
        self.pos_class.desired_y = trajectory[self.current_point - 1, 1]
        self.pos_class.desired_z = trajectory[self.current_point - 1, 2]
        self.pos_class.desired_yaw = trajectory[self.current_point - 1, 3]

        self.nonunix_time += self.dt

        #---------------------------------------------------
        # SCANNING PHASE.
        #---------------------------------------------------
        self.pubTakePic.publish("no")
        #---------------------------------------------------

        if (((self.pos_class.desired_x - 0.15) < self.pos_class.current_trans_x
             < (self.pos_class.desired_x + 0.15))
                and ((self.pos_class.desired_y - 0.15) <
                     self.pos_class.current_trans_y <
                     (self.pos_class.desired_y + 0.15))
                and ((self.pos_class.desired_z - 0.05) <
                     self.pos_class.current_trans_z <
                     (self.pos_class.desired_z + 0.05)) and
            ((self.pos_class.desired_yaw - 0.25) < self.pos_class.current_yaw <
             (self.pos_class.desired_yaw + 0.25))):
            if self.current_point < num_points and self.nonunix_time >= 0.01:
                self.current_point += 1
                self.nonunix_time = 0

                #---------------------------------------------------
                # SCANNING PHASE.
                #---------------------------------------------------

                current_mission_time = int(self.start_time -
                                           (rospy.get_time()))
                file_name = str(round(self.pic_number)) + "_t" + str(
                    current_mission_time) + "_X" + str(
                        round(self.pos_class.current_trans_x, 2)) + "_Y" + str(
                            round(self.pos_class.current_trans_y, 2)
                        ) + "_Z" + str(round(
                            self.pos_class.current_trans_z, 2)) + "_W" + str(
                                round(self.pos_class.current_yaw, 2)) + ".jpg"
                self.pic_number = self.pic_number + 1
                self.pubTakePic.publish(file_name)