Example #1
0
    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_reached = rospy.Publisher('/check_reach',
                                           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 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()
        """
Example #2
0
    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.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.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.run_process)

        self.postcom = PositionController()
    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()
Example #4
0
    def __init__(self):

        # Publishers: rotor velocities
        self.pub_rotor_vel = rospy.Publisher('/crazyflie2/command/motor_speed',
                                             Actuators,
                                             queue_size=1)

        # Subscribers: desired posn, current posn

        #self._goal_msg = MultiDOFJointTrajectory()
        #self.sub_goal = rospy.Subscriber('crazyflie2/command/trajectory', MultiDOFJointTrajectory, self._goal_callback)

        self._currpos_msg = Odometry()
        self.sub_currpos = rospy.Subscriber('/crazyflie2/odometry', Odometry,
                                            self._currpos_callback)

        # end of publisher/subscriber
        # initialize other parameters

        self.data_list = [[
            'Time', 'Desired_x', 'Desired_y', 'Desired_z', 'Actual_x',
            'Actual_y', 'Actual_z', 'Desired_yaw', 'Desired_roll',
            'Desired_pitch', 'Desired_yaw_r', 'Desired_climb_r', 'Actual_roll',
            'Actual_pitch', 'Actual_yaw_r', 'Actual_climb_r'
        ]]

        self.start = [1.0, 1.0]

        self.wp_id = 1
        self.previous_time = 0.0
        self.change_time = -100.0

        self.controller = PositionController()
        self.init_time = rospy.get_time()

        # initialize position and roll, pitch, yaw
        self._x = 0
        self._y = 0
        self._z = 0
        self.roll = 0
        self.pitch = 0
        self.yaw = 0

        self._z_old = 0.0
        self._z_oold = 0.0

        self.yaw_old = 0.0
    def __init__(self):

        # Publishers
        self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC',
                                           Twist,
                                           queue_size=32)

        self.pub_flat_state = rospy.Publisher('/aer1217/flat_state',
                                              FlatState,
                                              queue_size=32)

        #Subscriber
        self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
                                          TransformStamped,
                                          self.update_quadrotor_state)

        self.sub_flat_in = rospy.Subscriber('/aer1217/flat_in', FlatInput,
                                            self.update_flat_in)
        self.sub_flat_state_op = rospy.Subscriber('/aer1217/flat_state_op',
                                                  FlatState,
                                                  self.update_flat_state_op)

        self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position',
                                                TransformStamped,
                                                self.update_desired_pos)

        #initialize  variables
        self.actual_pos = TransformStamped()
        self.actual_pos_prev = TransformStamped()
        self.actual_vel = np.array([0, 0, 0, 0])
        self.actual_vel_prev = np.array([0, 0, 0, 0])
        self.actual_vel_prev2 = np.array([0, 0, 0, 0])
        self.actual_vel_prev3 = np.array([0, 0, 0, 0])
        self.flat_state_msg = FlatState()

        self.desired_pos = TransformStamped()
        self.desired_pos.transform.translation.z = 2  #initialize z to 2
        self.desired_pos_prev = TransformStamped()
        self.desired_vel = np.array([0, 0, 2, 0])

        self.flat_state = list(np.zeros(9))
        self.flat_input = list(np.zeros(4))
        self.flat_state_op = list(np.zeros(9))

        #initialize position controller
        self.pos_controller = PositionController()
        self.flat_input = np.array([0, 0, 0, 0])

        #publish vel commands
        self.dt = 1 / 300.0
        self.freq = 300

        #run ros loop
        rate = rospy.Rate(self.freq)

        while not rospy.is_shutdown():
            self.send_vel_cmd()
            rate.sleep()
Example #6
0
    def __init__(self):

        # Publishers
        self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC',
                                           Twist,
                                           queue_size=32)

        self.pub_error = rospy.Publisher('/aer1217/position_error',
                                         Twist,
                                         queue_size=32)

        #Subscriber
        self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
                                          TransformStamped,
                                          self.update_quadrotor_state)
        self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position',
                                                TransformStamped,
                                                self.update_desired_pos)

        #initialize  variables
        self.actual_pos = TransformStamped()
        self.actual_pos_prev = TransformStamped()
        self.actual_vel = np.array([0, 0, 0, 0])

        self.desired_pos = TransformStamped()
        self.desired_pos.transform.translation.z = 2  #initialize z to 2
        self.desired_pos_prev = TransformStamped()
        self.desired_vel = np.array([0, 0, 2, 0])

        #Define Published msgs
        self.pos_error = Twist()
        self.vel_cmd_msg = Twist()

        #initialize position controller
        self.pos_controller = PositionController()

        #publish vel commands
        self.dt = 1 / 100.0
        #press_key_wait = rospy.wait_for_message('/aer1217/move_type', Int16)
        self.pub_prop_vel = rospy.Timer(rospy.Duration(self.dt), \
                  self.send_vel_cmd)
Example #7
0
    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
Example #8
0
    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)
Example #9
0
    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)
Example #10
0
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")
Example #11
0
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
Example #12
0
    def __init__(self, **kwargs):
        self.mainloop = gobject.MainLoop()
        self.pipeline = gst.Pipeline("pipeline")
        self.verbose = kwargs.get('verbose', True)
        self.debug = kwargs.get('debug', False)
        cam_width = kwargs.get('cam_width', 640)
        cam_height = kwargs.get('cam_height', 480)
        host = kwargs.get('host', '127.0.0.1')
        port = kwargs.get('port', 5000)
        h264 = kwargs.get('h264', False)
        self.marker_spotted = False
        self.image_processing = ImageProcessing(area_threshold=10)
        self.state_estimate = StateEstimationAltitude()
        self.autopilot = AutoPilot(self.state_estimate)
        self.position_controller = PositionController(
            self.autopilot, self.state_estimate)
        if h264:
            self.videosrc = gst.parse_launch(
                'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc')
        else:
            self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src')
        fps = 30
        self.vfilter = gst.element_factory_make("capsfilter", "vfilter")
        self.vfilter.set_property('caps', gst.caps_from_string(
            'image/jpeg, width=%s, height=%s, framerate=20/1' % (str(cam_width), str(cam_height))))
        self.queue = gst.element_factory_make("queue", "queue")

        self.udpsink = gst.element_factory_make('udpsink', 'udpsink')
        self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay')
        self.udpsink.set_property('host', host)
        self.udpsink.set_property('port', port)

        self.pipeline.add_many(
            self.videosrc,
            self.queue,
            self.vfilter,
            self.rtpjpegpay,
            self.udpsink)
        gst.element_link_many(
            self.videosrc,
            self.queue,
            self.vfilter,
            self.rtpjpegpay,
            self.udpsink)

        pad = next(self.queue.sink_pads())
        # Sending frames to onVideBuffer where openCV can do processing.
        pad.add_buffer_probe(self.onVideoBuffer)
        self.pipeline.set_state(gst.STATE_PLAYING)
        self.i = 0
        gobject.threads_init()
        context = self.mainloop.get_context()
        #previous_update = time.time()
        fpstime = time.time()
        while True:
            try:
                context.iteration(False)
                self.autopilot._read_sensors()
                if self.autopilot.auto_switch > 1700:
                    self.position_controller.headingHold()
                    self.position_controller.holdAltitude()
                    self.autopilot.send_control_commands()
                else:
                    self.position_controller.reset_targets()
                    print self.autopilot.print_commands()
            except KeyboardInterrupt:
                fps = self.i / (time.time() - fpstime)
                print 'fps %f ' % fps
                self.autopilot.dump_log()
                self.autopilot.disconnect_from_drone()
	#Print to screen using curses
	if col == 0:
		screen.addstr(line, 0, string)
	if col == 1:
		screen.addstr(line, 40, string)

	screen.refresh()


logging.basicConfig(filename='flight_test.log', level=logging.DEBUG)

#Setup of vidro and controller
vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)

#Initalization of curses
screen = curses.initscr()
screen.clear()
screen.refresh()
plot.ion()

switch = False

timer = time.time()

while (vidro.current_rc_channels[4] > 1600) and (flight_ready == True):

	controller.I_error_alt = 0
	controller.I_error_pitch = 0
	#print 'Num Objs: ',repr(num_objects),' Cx: ',repr(cx),' Cy: ',repr(cy),' Area: ',repr(area_max)
	#image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects]
	image_data = [cx,cy,area_max,num_objects]
	return image_data

def get_camera_frame():
	global frame
	global new_frame
	_, frame = cap.read()
	frame = cv2.flip(frame,-1,-1)
	new_frame = True

#Setup of vidro and controller
vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)

start_time = time.time()
previous_time = time.time()

cycle = 0

controller.update_gains()

sequence = 0
seq0_cnt = 0
seq1_cnt = 0
seq2_cnt = 0
seq3_cnt = 0
pos_bound_err = 150
yaw_bound_err = 0.2
    def __init__(self, uav_ids, init, fin, vx_ds, vy_ds, vz_ds, yaw_ds):

        self.cf_ids = uav_ids
        self.number_of_agents = np.shape(uav_ids)[0]
        self.takoff_alt = 1.0  # change?
        self._pos = {}
        self._vel = {}
        self._quat = {}
        self._dist_to_goal = {}

        self._euler_angles = {}
        self._euler_angular_rates = {}
        self._z_old = {}
        self._z_oold = {}
        self.yaw_old = {}

        self.radius = 0.15
        self.d_star = self.radius
        self.MaxVelo = 1.0
        # Tune these
        self.c1 = 7 * 4.5
        self.c2 = 9 * 4.5
        self.RepulsiveGradient = 7.5 * 100

        self.previous_time = 0.0
        self.change_time = -100.0

        self.controller = PositionController()
        self.init_time = rospy.get_time()

        ### Publish ###
        # waypoint messages
        self.goal_msg_0, self.goal_msg_1, self.goal_msg_2, self.goal_msg_3 = PoseStamped(
        ), PoseStamped(), PoseStamped(), PoseStamped()
        self.goal_msgs = {
            '0': self.goal_msg_0,
            '1': self.goal_msg_1,
            '2': self.goal_msg_2,
            '3': self.goal_msg_3
        }

        # rotor velocities messages
        #    self.cmdV_msg_0, self.cmdV_msg_1, self.cmdV_msg_2, self.cmdV_msg_3 = Actuators(), Actuators(), Actuators(), Actuators()
        #    self.rotor_vel_msgs = {
        #        '0' : self.cmdV_msg_0,
        #        '1' : self.cmdV_msg_1,
        #        '2' : self.cmdV_msg_2,
        #        '3' : self.cmdV_msg_3
        #    }

        ### Subscribe ###
        # odometry messages
        self._currpos_callbacks = {
            '0': self._currpos_callback_0,
            '1': self._currpos_callback_1,
            '2': self._currpos_callback_2,
            '3': self._currpos_callback_3
        }

        # set parameters
        self.initials, self.finals = {}, {}
        self.goal_pubs, self.cmdVtemp_pubs, self.cmdV_pubs, self.takeoffs, self.lands = {}, {}, {}, {}, {}
        for index, cf_id in enumerate(self.cf_ids):
            self.initials[str(cf_id)] = init[index][:]
            self._z_old[str(cf_id)] = 0.0
            self._z_oold[str(cf_id)] = 0.0
            self.yaw_old[str(cf_id)] = 0.0
            self.finals[str(cf_id)] = fin[index][:]
            self.goal_pubs[str(cf_id)] = rospy.Publisher('/crazyflie2_' +
                                                         str(cf_id) + '/goal',
                                                         PoseStamped,
                                                         queue_size=1)
            self.cmdV_pubs[str(cf_id)] = rospy.Publisher(
                '/crazyflie2_' + str(cf_id) + '/command/motor_speed',
                Actuators,
                queue_size=1)
            rospy.Subscriber("/crazyflie2_" + str(cf_id) + "/odometry",
                             Odometry, self._currpos_callbacks[str(cf_id)])
        self.vx_d = vx_ds
        self.vy_d = vy_ds
        self.vz_d = vz_ds
        self.yaw_d = yaw_ds

        self.takeoffed = False
        self.reached_1st = False

        self.flag = {'flying': 0, 'landed': 0, 'preland': 0}

        #        self.vstate = {
        #            'takeoff' : self.do_takeoff,
        #            'wpnav' : self.do_wpnav,
        #            'land' : self.do_land
        #        }

        # Position controller
        self.controller = PositionController()
		return

	if line > 22 or line < 0:
		return

	#Print to screen using curses
	if col == 0:
		screen.addstr(line, 0, string)
	if col == 1:
		screen.addstr(line, 40, string)

	screen.refresh()

vidro = Vidro(True)
vidro.connect()
controller = PositionController(vidro)

screen = curses.initscr()
screen.clear()
screen.refresh()

while vidro.current_rc_channels[4] > 1600:

	controller.update_gains()

	try:
		controller.rc_alt(500)
		#controller.rc_yaw(0)
		#controller.rc_xy(500,500)
		curses_print("No errors",2,0)
	except:
Example #17
0
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
Example #18
0
from vidro import Vidro, ViconStreamer
from position_controller import PositionController
import sys, math, time
import socket, struct, threading
import numpy as np
import Adafruit_BBIO.ADC as ADC
import logging

###############################
#Setup of vidro and controller#
###############################

vidro = Vidro(False, 1)
flight_ready = vidro.connect_vicon()
vidro.connect_mavlink()
controller = PositionController(vidro)

start_time = time.time()

#Load gains
controller.update_gains() 

####################
# Controller Values#
####################

take_off_alt = 1000
# Pos Commands
alt_com = 0
# Pos Values
alt_pos = 0
Example #19
0
File: main.py Project: jesson/drone
    def __init__(self, **kwargs):
        self.mainloop = gobject.MainLoop()
        self.pipeline = gst.Pipeline("pipeline")
        self.verbose = kwargs.get('verbose', True)
        self.debug = kwargs.get('debug', False)
        cam_width = kwargs.get('cam_width', 640)
        cam_height = kwargs.get('cam_height', 480)
        host = kwargs.get('host', '127.0.0.1')
        port = kwargs.get('port', 5000)
        h264 = kwargs.get('h264', False)
        self.marker_spotted = False
        self.image_processing = ImageProcessing(area_threshold=10)
        self.state_estimate = StateEstimationAltitude()
        self.autopilot = AutoPilot(self.state_estimate)
        self.position_controller = PositionController(self.autopilot,
                                                      self.state_estimate)
        if h264:
            self.videosrc = gst.parse_launch(
                'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc'
            )
        else:
            self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src')
        fps = 30
        self.vfilter = gst.element_factory_make("capsfilter", "vfilter")
        self.vfilter.set_property(
            'caps',
            gst.caps_from_string(
                'image/jpeg, width=%s, height=%s, framerate=20/1' %
                (str(cam_width), str(cam_height))))
        self.queue = gst.element_factory_make("queue", "queue")

        self.udpsink = gst.element_factory_make('udpsink', 'udpsink')
        self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay')
        self.udpsink.set_property('host', host)
        self.udpsink.set_property('port', port)

        self.pipeline.add_many(self.videosrc, self.queue, self.vfilter,
                               self.rtpjpegpay, self.udpsink)
        gst.element_link_many(self.videosrc, self.queue, self.vfilter,
                              self.rtpjpegpay, self.udpsink)

        pad = next(self.queue.sink_pads())
        # Sending frames to onVideBuffer where openCV can do processing.
        pad.add_buffer_probe(self.onVideoBuffer)
        self.pipeline.set_state(gst.STATE_PLAYING)
        self.i = 0
        gobject.threads_init()
        context = self.mainloop.get_context()
        #previous_update = time.time()
        fpstime = time.time()
        while True:
            try:
                context.iteration(False)
                self.autopilot._read_sensors()
                if self.autopilot.auto_switch > 1700:
                    self.position_controller.headingHold()
                    self.position_controller.holdAltitude()
                    self.autopilot.send_control_commands()
                else:
                    self.position_controller.reset_targets()
                    print self.autopilot.print_commands()
            except KeyboardInterrupt:
                fps = self.i / (time.time() - fpstime)
                print 'fps %f ' % fps
                self.autopilot.dump_log()
                self.autopilot.disconnect_from_drone()
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)
Example #21
0
#Filter for fencing values given by wand
def filter_value(low,high,value):
	if value < low:
		value = low
	if value > high:
		value = high
	return value

#Initialization of log
logging.basicConfig(filename='demo.log', level=logging.DEBUG)

#Creation of vidro and controller objects
vidro = Vidro(False, 2)
flight_ready = vidro.connect()
controller = PositionController(vidro)

#Setup the screen for curses
screen = curses.initscr()
screen.clear()
screen.refresh()

#switch for knowing if first time out of control loop
switch = False

#needed for live plotting
plot.ion()

#Setup of timer
timer = time.time()
Example #22
0
    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()
Example #23
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
Example #24
0
class PositionControllerNode_ChihChun(object):
    """ROS interface for controlling the Crazyflie 2 in Gazebo."""

    # write code here to define node publishers and subscribers
    # publish to /crazyflie2/command/motor_speed topic
    # subscribe to /crazyflie2/command/trajectory and /crazyflie2/odometry

    def __init__(self):

        # Publishers: rotor velocities
        self.pub_rotor_vel = rospy.Publisher('/crazyflie2/command/motor_speed',
                                             Actuators,
                                             queue_size=1)

        # Subscribers: desired posn, current posn

        #self._goal_msg = MultiDOFJointTrajectory()
        #self.sub_goal = rospy.Subscriber('crazyflie2/command/trajectory', MultiDOFJointTrajectory, self._goal_callback)

        self._currpos_msg = Odometry()
        self.sub_currpos = rospy.Subscriber('/crazyflie2/odometry', Odometry,
                                            self._currpos_callback)

        # end of publisher/subscriber
        # initialize other parameters

        self.data_list = [[
            'Time', 'Desired_x', 'Desired_y', 'Desired_z', 'Actual_x',
            'Actual_y', 'Actual_z', 'Desired_yaw', 'Desired_roll',
            'Desired_pitch', 'Desired_yaw_r', 'Desired_climb_r', 'Actual_roll',
            'Actual_pitch', 'Actual_yaw_r', 'Actual_climb_r'
        ]]

        self.start = [1.0, 1.0]

        self.wp_id = 1
        self.previous_time = 0.0
        self.change_time = -100.0

        self.controller = PositionController()
        self.init_time = rospy.get_time()

        # initialize position and roll, pitch, yaw
        self._x = 0
        self._y = 0
        self._z = 0
        self.roll = 0
        self.pitch = 0
        self.yaw = 0

        self._z_old = 0.0
        self._z_oold = 0.0

        self.yaw_old = 0.0

    def set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch,
                      yaw, thrust):  # change arg names?

        # get conversions
        rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie(
            pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust)
        rotor_velocities = rotorvel_converter.CalculateRotorVelocities()

        # publish rotor velocities to Actuator
        msg = Actuators()
        msg.angular_velocities = rotor_velocities
        msg.header.stamp = self._currpos_msg.header.stamp
        self.pub_rotor_vel.publish(msg)

    def _currpos_callback(self, msg):
        self._currpos_msg = msg

    def get_data(self):
        self._z_oold = self._z_old
        self._z_old = self._z

        self.yaw_old = self.yaw

        # self._pos = self._vicon_msg.transform.translation
        self._x = self._currpos_msg.pose.pose.position.x
        self._y = self._currpos_msg.pose.pose.position.y
        self._z = self._currpos_msg.pose.pose.position.z

        # Convert to quaternion object for use by euler_from_quaternion()
        quaternion = np.array([
            self._currpos_msg.pose.pose.orientation.x,
            self._currpos_msg.pose.pose.orientation.y,
            self._currpos_msg.pose.pose.orientation.z,
            self._currpos_msg.pose.pose.orientation.w
        ])

        # Determine the euler angles
        euler = euler_from_quaternion(quaternion)  # change? can i use this?
        self.roll = euler[0]
        self.pitch = euler[1]
        self.yaw = euler[2]

        # Determine the euler angular rates p,q,r and thrust
        self.p = self._currpos_msg.twist.twist.angular.x
        self.q = self._currpos_msg.twist.twist.angular.y
        self.r = self._currpos_msg.twist.twist.angular.z
        #self.thrust = self._currpos_msg.twist.twist.linear.z

        self.x_d = 1.0
        self.y_d = 1.0
        self.z_d = 1.0
        self.yaw_d = 0.0

        self.vx_d = 0.0
        self.vy_d = 0.0
        self.vz_d = 0.0

        # desired position
        #self.x_d = self._goal_msg.points.transforms.translation.x
        #self.y_d = self._goal_msg.points.transforms.translation.y
        #self.z_d = self._goal_msg.points.transforms.translation.z

        # desired orientation
        #self.quat = np.array([self._goal_msg.points.transforms.rotation.x,
        #                      self._goal_msg.points.transforms.rotation.y,
        #                      self._goal_msg.points.transforms.rotation.z,
        #                      self._goal_msg.points.transforms.rotation.w])
        #euler = euler_from_quaternion(self.quat)
        #self.yaw_d = euler[2]

        # desired velocities
        #self.vx_d = self._goal_msg.points.velocities.linear.x # may need to change in future
        #self.vy_d = self._goal_msg.points.velocities.linear.y
        #self.vz_d = self._goal_msg.points.velocities.linear.z

    def iteration(self, event):
        current_time = rospy.get_time() - self.init_time
        dt = current_time - self.previous_time
        self.previous_time = current_time
        #print(dt)

        if dt == 0:
            dt = 0.01

        act_climb_r = (self._z - 2 * self._z_old + self._z_oold) / (dt**2)
        print("z_dd_real: ", act_climb_r)
        #act_yaw_r = (self.yaw - self.yaw_old)/dt

        self.get_data()

        roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command(
            self._x, self._y, self._z, self.roll, self.pitch, self.yaw,
            self.x_d, self.y_d, self.z_d, self.vx_d, self.vy_d, self.vz_d,
            self.yaw_d)

        #self.set_vel(roll_c, pitch_c, z_dot_c, yaw_dot_c)
        self.thrust_c = z_dot_c  #+ 12000
        print("thrust: ", z_dot_c)
        self.set_rotor_vel(pitch_c, roll_c, yaw_dot_c, self.p, self.q, self.r,
                           self.roll, self.pitch, self.yaw, self.thrust_c)
Example #25
0
                plot_camera_time.append(current_time-start_time)
                plot_camera_cycle.append(current_time-previous_time)
                plot_camera_cycle_number.append(cycle)
                cycle += 1
                previous_time = current_time
                curses_print("In camera loop",3,0)
                camera.capture(stream, 'bgr', use_video_port=True)
                # stream.array now contains the image data in BGR order
        frame = stream.array
                new_frame = True
                stream.truncate(0)

#Setup of vidro and controller
vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)

event = threading.Event()

d = threading.Thread(name='camera', target=camera_thread, args=(event,))

d.start()

start_time = time.time()
previous_time = time.time()

cycle = 0

controller.update_gains()

sequence = 0
Example #26
0
	return image_data

def get_camera_frame():
	global frame
	global new_frame
	_, frame = cap.read()
	frame = cv2.flip(frame,-1,-1)
	new_frame = True

###############################
#Setup of vidro and controller#
###############################

vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)
start_time = time.time()
# Load gains
controller.update_gains() 

####################
# Controller Values#
####################

take_off_alt = 1000
# Pos Commands
x_com = 0
y_com = 0
alt_com = 0
yaw_com = 0
# Pos Values
class position_controller_flock_node(object):
    """ROS interface for controlling up to four Cf2.0's in Gazebo and running flocking algorithm."""
    def __init__(self, uav_ids, init, fin, vx_ds, vy_ds, vz_ds, yaw_ds):

        self.cf_ids = uav_ids
        self.number_of_agents = np.shape(uav_ids)[0]
        self.takoff_alt = 1.0  # change?
        self._pos = {}
        self._vel = {}
        self._quat = {}
        self._dist_to_goal = {}

        self._euler_angles = {}
        self._euler_angular_rates = {}
        self._z_old = {}
        self._z_oold = {}
        self.yaw_old = {}

        self.radius = 0.15
        self.d_star = self.radius
        self.MaxVelo = 1.0
        # Tune these
        self.c1 = 7 * 4.5
        self.c2 = 9 * 4.5
        self.RepulsiveGradient = 7.5 * 100

        self.previous_time = 0.0
        self.change_time = -100.0

        self.controller = PositionController()
        self.init_time = rospy.get_time()

        ### Publish ###
        # waypoint messages
        self.goal_msg_0, self.goal_msg_1, self.goal_msg_2, self.goal_msg_3 = PoseStamped(
        ), PoseStamped(), PoseStamped(), PoseStamped()
        self.goal_msgs = {
            '0': self.goal_msg_0,
            '1': self.goal_msg_1,
            '2': self.goal_msg_2,
            '3': self.goal_msg_3
        }

        # rotor velocities messages
        #    self.cmdV_msg_0, self.cmdV_msg_1, self.cmdV_msg_2, self.cmdV_msg_3 = Actuators(), Actuators(), Actuators(), Actuators()
        #    self.rotor_vel_msgs = {
        #        '0' : self.cmdV_msg_0,
        #        '1' : self.cmdV_msg_1,
        #        '2' : self.cmdV_msg_2,
        #        '3' : self.cmdV_msg_3
        #    }

        ### Subscribe ###
        # odometry messages
        self._currpos_callbacks = {
            '0': self._currpos_callback_0,
            '1': self._currpos_callback_1,
            '2': self._currpos_callback_2,
            '3': self._currpos_callback_3
        }

        # set parameters
        self.initials, self.finals = {}, {}
        self.goal_pubs, self.cmdVtemp_pubs, self.cmdV_pubs, self.takeoffs, self.lands = {}, {}, {}, {}, {}
        for index, cf_id in enumerate(self.cf_ids):
            self.initials[str(cf_id)] = init[index][:]
            self._z_old[str(cf_id)] = 0.0
            self._z_oold[str(cf_id)] = 0.0
            self.yaw_old[str(cf_id)] = 0.0
            self.finals[str(cf_id)] = fin[index][:]
            self.goal_pubs[str(cf_id)] = rospy.Publisher('/crazyflie2_' +
                                                         str(cf_id) + '/goal',
                                                         PoseStamped,
                                                         queue_size=1)
            self.cmdV_pubs[str(cf_id)] = rospy.Publisher(
                '/crazyflie2_' + str(cf_id) + '/command/motor_speed',
                Actuators,
                queue_size=1)
            rospy.Subscriber("/crazyflie2_" + str(cf_id) + "/odometry",
                             Odometry, self._currpos_callbacks[str(cf_id)])
        self.vx_d = vx_ds
        self.vy_d = vy_ds
        self.vz_d = vz_ds
        self.yaw_d = yaw_ds

        self.takeoffed = False
        self.reached_1st = False

        self.flag = {'flying': 0, 'landed': 0, 'preland': 0}

        #        self.vstate = {
        #            'takeoff' : self.do_takeoff,
        #            'wpnav' : self.do_wpnav,
        #            'land' : self.do_land
        #        }

        # Position controller
        self.controller = PositionController()

    def set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch,
                      yaw, thrust):  # change arg names?

        # get conversions
        rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie(
            pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust)
        rotor_velocities = rotorvel_converter.CalculateRotorVelocities()

        # publish rotor velocities to Actuator
        msg = Actuators()
        msg.angular_velocities = rotor_velocities
        msg.header.stamp = self._currpos_msg.header.stamp
        self.pub_rotor_vel.publish(msg)

    def _currpos_callback(self, msg):
        self._currpos_msg = msg

    def get_data(self, id):
        self._z_oold[str(id)] = self._z_old[str(id)]
        self._z_old[str(id)] = self._pos[str(id)].z  # current z
        print("self._euler_angles[str(id)]: ", self._euler_angles[str(id)])
        self.yaw_old[str(id)] = self._euler_angles[str(id)][2]  # current yaw

    def _currpos_callback_0(self, data):
        self._pos['0'] = data.pose.pose.position
        self._vel['0'] = data.twist.twist.linear
        self._quat['0'] = np.array([
            data.pose.pose.orientation.x, data.pose.pose.orientation.y,
            data.pose.pose.orientation.z, data.pose.pose.orientation.w
        ])  # gives quaternion object
        #print("quat 0: ", self._quat['0'])
        self._euler_angles['0'] = euler_from_quaternion(
            self._quat['0'])  # gives roll, pitch, yaw
        #print("euler angles 0: ", self._euler_angles['0'])
        self._euler_angular_rates['0'] = np.asarray(
            data.twist.twist.angular)  # gives p, q, r

    def _currpos_callback_1(self, data):
        self._pos['1'] = data.pose.pose.position
        self._vel['1'] = data.twist.twist.linear
        self._quat['1'] = np.array([
            data.pose.pose.orientation.x, data.pose.pose.orientation.y,
            data.pose.pose.orientation.z, data.pose.pose.orientation.w
        ])  # gives quaternion object
        self._euler_angles['1'] = euler_from_quaternion(
            self._quat['1'])  # gives roll, pitch, yaw
        #print(self._euler_angles['1'], type(self._euler_angles['1']))
        self._euler_angular_rates['1'] = np.asarray(
            data.twist.twist.angular)  # gives p, q, r

    def _currpos_callback_2(self, data):
        self._pos['2'] = data.pose.pose.position
        self._vel['2'] = data.twist.twist.linear
        self._quat['2'] = np.array([
            data.pose.pose.orientation.x, data.pose.pose.orientation.y,
            data.pose.pose.orientation.z, data.pose.pose.orientation.w
        ])  # gives quaternion object
        self._euler_angles['2'] = euler_from_quaternion(
            self._quat['2'])  # gives roll, pitch, yaw
        self._euler_angular_rates['2'] = np.asarray(
            data.twist.twist.angular)  # gives p, q, r

    def _currpos_callback_3(self, data):
        self._pos['3'] = data.pose.pose.position
        self._vel['3'] = data.twist.twist.linear
        self._quat['3'] = np.array([
            data.pose.pose.orientation.x, data.pose.pose.orientation.y,
            data.pose.pose.orientation.z, data.pose.pose.orientation.w
        ])  # gives quaternion object
        self._euler_angles['3'] = euler_from_quaternion(
            self._quat['3'])  # gives roll, pitch, yaw
        self._euler_angular_rates['3'] = np.asarray(
            data.twist.twist.angular)  # gives p, q, r

    def update_pos(self, id, pos):
        self.goal_msgs[id].header.seq += 1
        self.goal_msgs[id].header.frame_id = '/world'  # change?
        self.goal_msgs[id].header.stamp = rospy.Time.now()

        self.goal_msgs[id].pose.position.x = pos[0]
        self.goal_msgs[id].pose.position.y = pos[1]
        self.goal_msgs[id].pose.position.z = pos[2]

        self.goal_msgs[id].pose.orientation.x = 0
        self.goal_msgs[id].pose.orientation.y = 0
        self.goal_msgs[id].pose.orientation.z = 0
        self.goal_msgs[id].pose.orientation.w = 1

    def update_rotor_vels(self, id):
        # this function takes in the UAV's id and computes+publishes the rotor velocities to that UAV

        # get z_oold, z_old, and desired position/yaw
        self.get_data(id)

        # get roll/pitch/yawrate/thrust commands from position controller
        roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command(
            self._pos[str(id)].x,
            self._pos[str(id)].y,
            self._pos[str(id)].z,  # x,y,z
            self._euler_angles[str(id)][0],
            self._euler_angles[str(id)][1],
            self._euler_angles[str(id)][0][2],  # change? roll, pitch, yaw
            self.initials[str(id)][0],
            self.initials[str(id)][1],
            self.initials[str(id)][2],  # change? xd, yd, zd
            self.vx_d[int(id)],
            self.vy_d[int(id)],
            self.vz_d[int(id)],
            self.yaw_d[int(id)])

        # obtain p,q,r/roll,pitch,yaw for UAV id from odometry subscription
        p = self._euler_angular_rates[str(id)][0]
        q = self._euler_angular_rates[str(id)][1]
        r = self._euler_angular_rates[str(id)][2]
        roll = self._euler_angles[str(id)][0]
        pitch = self._euler_angles[str(id)][1]
        yaw = self._euler_angles[str(id)][2]

        # convert above commands to rotor velocity commands
        rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie(
            pitch_c, roll_c, yaw_dot_c, p, q, r, roll, pitch, yaw, z_dot_c)
        rotor_velocities = rotorvel_converter.CalculateRotorVelocities(
        )  # this yields a 4-element list

        # publish rotor velocities to Actuator
        rotorvel_msg = Actuators()
        rotorvel_msg.angular_velocities = rotor_velocities
        #rotorvel_msg.header.stamp = self._currpos_msg.header.stamp
        self.cmdV_pubs[str(cf_id)].publish(rotorvel_msg)

    def publish_msg(self):
        for cf_id in self.cf_ids:
            self.goal_pubs[str(cf_id)].publish(self.goal_msgs[str(cf_id)])

    def do_wpnav(self):
        print('Navigating!!')
        # Check all cfs reached assigened takeoff alt
        if not self.takeoffed:
            count = 0
            for cf_id in self.cf_ids:
                if abs(self._pos[str(cf_id)][2] - self.takoff_alt) < 0.05:
                    count += 1
                position_d = np.array([
                    self._pos[str(cf_id)][0], self._pos[str(cf_id)][1],
                    self.takoff_alt
                ])
                self.update_pos(str(cf_id), position_d)
            if count == self.number_of_agents:
                self.takeoffed = True

        # Check all cfs reached their initial points
        elif not self.reached_1st:
            count = 0
            for cf_id in self.cf_ids:
                if np.linalg.norm(self._pos[str(cf_id)] -
                                  self.initials[str(cf_id)]) < 0.1:
                    count += 1
                position_d = np.array([
                    self.initials[str(cf_id)][0], self.initials[str(cf_id)][1],
                    self.initials[str(cf_id)][2]
                ])
                self.update_pos(str(cf_id), position_d)
            if count == self.number_of_agents:
                self.reached_1st = True
                print('Initial points reached!!')
                raw_input(
                    "Press Enter to continue...")  # use input() for python3

        # flocking path planning
        else:
            # call flocking and get the desired position of next timestep
            for cf_id in self.cf_ids:
                other_cfs = self.cf_ids[:]
                other_cfs.remove(cf_id)
                self._dist_to_goal[str(cf_id)] = np.linalg.norm(
                    self._pos[str(cf_id)] - self.finals[str(cf_id)])
                force = -self.c1 * self._vel[str(cf_id)] - self.c2 * (
                    self._pos[str(cf_id)] - self.finals[str(cf_id)])
                for other_cf in other_cfs:
                    dist_v = self._pos[str(other_cf)] - self._pos[str(cf_id)]
                    dist = np.linalg.norm(dist_v)
                    d = 2 * self.radius + self.d_star
                    CommunicationRadious = np.cbrt(
                        3 * np.square(self.MaxVelo) /
                        (2 * self.RepulsiveGradient)) + d
                    if dist < CommunicationRadious:
                        ForceComponent = -self.RepulsiveGradient * np.square(
                            dist - CommunicationRadious)
                        force += ForceComponent * (dist_v) / dist
                velocity_d = self._vel[str(cf_id)] + force * self.dt
                position_d = self._pos[str(cf_id)] + velocity_d * self.dt
                position_d[2] = 1.0  # for 2D sim
                self.update_pos(str(cf_id), position_d)

            # Check all cfs reached their final points
            if self._dist_to_goal[max(self._dist_to_goal)] < 0.2:
                self.flag['preland'] = 1

    def iteration(self, event):

        # publish goal messages for each uav
        self.publish_msg()

        # publish rotor velocities for each uav
        for index, cf_id in enumerate(self.cf_ids):
            self.update_rotor_vels(cf_id)
#!/usr/bin/env python
# -*- coding: utf-8 -*-

from vidro import Vidro, ViconStreamer
from position_controller import PositionController
import sys, math, time
import socket, struct, threading
import numpy as np
import cv2


#Setup of vidro and controller
vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)

start_time = time.time()
previous_time = time.time()

cycle = 0

controller.update_gains()

sequence = 0
seq0_cnt = 0
seq1_cnt = 0
seq2_cnt = 0
seq3_cnt = 0
pos_bound_err = 300
yaw_bound_err = 0.5
yaw = 0
Example #29
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):
        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)
Example #30
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):

        # Publishers
        self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC',
                                           Twist,
                                           queue_size=32)

        self.pub_error = rospy.Publisher('/aer1217/position_error',
                                         Twist,
                                         queue_size=32)

        #Subscriber
        self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
                                          TransformStamped,
                                          self.update_quadrotor_state)
        self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position',
                                                TransformStamped,
                                                self.update_desired_pos)

        #initialize  variables
        self.actual_pos = TransformStamped()
        self.actual_pos_prev = TransformStamped()
        self.actual_vel = np.array([0, 0, 0, 0])

        self.desired_pos = TransformStamped()
        self.desired_pos_prev = TransformStamped()
        self.desired_vel = np.array([0, 0, 2, 0])

        #Define Published msgs
        self.pos_error = Twist()
        self.vel_cmd_msg = Twist()

        #initialize position controller
        self.pos_controller = PositionController()

        #publish vel commands
        self.dt = 1 / 100.0
        self.pub_prop_vel = rospy.Timer(rospy.Duration(self.dt), \
                  self.send_vel_cmd)

    def update_quadrotor_state(self, transfrom_stamped_msg):
        """Callback function for sub_vicon topic, stores the quad rotor
		position and also calculates its velocity 

		Args:
			tranform_stamped_msg: msg received from subscriber
		"""

        #store received msg
        self.actual_pos = transfrom_stamped_msg

        #calculate velocity
        rotation = (self.actual_pos.transform.rotation.x,
                    self.actual_pos.transform.rotation.y,
                    self.actual_pos.transform.rotation.z,
                    self.actual_pos.transform.rotation.w)
        euler_angle = euler_from_quaternion(rotation)

        psi_act = euler_angle[2]

        pos_act = np.array([
            self.actual_pos.transform.translation.x,
            self.actual_pos.transform.translation.y,
            self.actual_pos.transform.translation.z, psi_act
        ])

        t_act = self.actual_pos.header.stamp.to_sec() + \
             self.actual_pos.header.stamp.to_nsec()/(10**9)

        rotation = (self.actual_pos_prev.transform.rotation.x,
                    self.actual_pos_prev.transform.rotation.y,
                    self.actual_pos_prev.transform.rotation.z,
                    self.actual_pos_prev.transform.rotation.w)
        euler_angle = euler_from_quaternion(rotation)

        psi_prev = euler_angle[2]

        pos_prev = np.array([
            self.actual_pos_prev.transform.translation.x,
            self.actual_pos_prev.transform.translation.y,
            self.actual_pos_prev.transform.translation.z, psi_prev
        ])

        t_prev = self.actual_pos_prev.header.stamp.to_sec() + \
            self.actual_pos_prev.header.stamp.to_nsec()/(10**9)

        try:
            delta = pos_act - pos_prev
            #Ensure difference in yaw doesn't exceed pi
            if delta[3] > np.pi:
                delta[3] = delta[3] - 2 * np.pi
            elif delta[3] < -np.pi:
                delta[3] = delta[3] + 2 * np.pi
            #calculate velocity
            self.actual_vel = delta / float(t_act - t_prev)
        except:
            ropsy.logwarn('Division by zero encountered when calculating \
									actual velocity')
            pass

        self.actual_pos_prev = self.actual_pos

    def update_desired_pos(self, transformstamped_msg):
        """Callback function for sub_desired_pos topic, stores the 
		desired position and also calculates its velocity

		Args:
		tranformstamped_msg: msg received from subscriber

		"""

        #store desired position
        self.desired_pos = transformstamped_msg

        pos_act = np.array([
            self.desired_pos.transform.translation.x,
            self.desired_pos.transform.translation.y,
            self.desired_pos.transform.translation.z,
            self.desired_pos.transform.rotation.z
        ])

        tdes_act = self.desired_pos.header.stamp.to_sec() + \
        self.desired_pos.header.stamp.to_nsec()/(10**9)

        pos_prev = np.array([
            self.desired_pos_prev.transform.translation.x,
            self.desired_pos_prev.transform.translation.y,
            self.desired_pos_prev.transform.translation.z,
            self.desired_pos_prev.transform.rotation.z
        ])

        tdes_prev = self.desired_pos_prev.header.stamp.to_sec() + \
        self.desired_pos_prev.header.stamp.to_nsec()/(10**9)

        try:
            delta = pos_act - pos_prev
            #Ensure difference in yaw doesn't exceed pi
            if delta[3] > np.pi:
                delta[3] = delta[3] - 2 * np.pi
            elif delta[3] < -np.pi:
                delta[3] = delta[3] + 2 * np.pi
            #calculate velocity
            self.desired_vel = delta / float(tdes_act - tdes_prev)
        except:
            ropsy.logwarn('Division by zero encountered when calculating \
				desired velocity')
            pass

        self.desired_pos_prev = self.desired_pos

    def send_vel_cmd(self, event):
        """Function that sends velocity commands to ardrone 
		"""

        #calculate commands using position controller
        phi_c, theta_c, z_acc, psi_acc = self.pos_controller.pos_cont(self.actual_pos, \
         self.desired_pos, self.actual_vel, self.desired_vel)

        #set velocity commands
        self.vel_cmd_msg.linear.x = phi_c
        self.vel_cmd_msg.linear.y = theta_c
        self.vel_cmd_msg.linear.z = np.clip(
            self.vel_cmd_msg.linear.z + z_acc * self.dt, -1.0, 1.0)
        self.vel_cmd_msg.angular.z = np.clip(
            self.vel_cmd_msg.angular.z + psi_acc * self.dt, -1000.0, 1000.0)

        #record position error
        self.pos_error.linear.x = self.desired_pos.transform.translation.x - \
              self.actual_pos.transform.translation.x

        self.pos_error.linear.y = self.desired_pos.transform.translation.y - \
              self.actual_pos.transform.translation.y

        self.pos_error.linear.z = self.desired_pos.transform.translation.z - \
              self.actual_pos.transform.translation.z
        rotation = (self.actual_pos.transform.rotation.x,
                    self.actual_pos.transform.rotation.y,
                    self.actual_pos.transform.rotation.z,
                    self.actual_pos.transform.rotation.w)
        euler_angle = euler_from_quaternion(rotation)

        psi_act = euler_angle[2]
        print('yaw = ', psi_act, 'yaw_error=',
              self.desired_pos.transform.rotation.z - psi_act)
        #Publish position error
        self.pub_error.publish(self.pos_error)

        #publish velocity commands
        self.pub_vel_cmd.publish(self.vel_cmd_msg)
Example #31
0
        return euler_from_quaternion(orient_q)

    def get_time(self):
        return self._vicon_msg.header.stamp.nsecs

    def land(self):
        self.pub_land.publish()

    def set_vel(self, traj):
        self.pub_traj.publish(traj)


if __name__ == '__main__':
    # write code to create ROSControllerNode
    rospy.init_node("ros_interface", disable_signals=True)
    positionCtrl = PositionController()
    ardrone = ROSControllerNode()
    # ardrone.time_stamp = ardrone.get_time()
    #first test
    # x_des = 4
    # y_des = 4
    # z_des = 4
    # yaw_des = 1

    try:
        while not rospy.is_shutdown():

            #get position nad orientation from vicon
            currentPosition = ardrone.get_pos()
            currentOrientation = ardrone.get_orient()
            #compute desired pose