def publish_desired_mode(self, mode):
     ''' Publish the desired Mode message
     mode : a String that is either 'ARMED', 'DISARMED', or 'FLYING'
     '''
     desired_mode_msg = Mode()
     desired_mode_msg.mode = mode
     self.desired_mode_pub.publish(desired_mode_msg)
Esempio n. 2
0
def img_callback(data):
    curr_time = rospy.get_time()
    img = bridge.imgmsg_to_cv2(data, 'mono8')
#   img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    global first_img
    global first_kp
    global first_des
    global prev_time
    global lr_err, fb_err, lr_pid, fb_pid
    global frame_counter
    global smoothed_pos
    alpha = 0.6
    if first_img is None:
        first_img = deepcopy(img)
        first_kp, first_des = orb.detectAndCompute(first_img, None)
        print 'Found {} features for the first img!'.format(len(first_kp))
    else:
        H = get_H(img)
        if H is not None:
            R, t, n = get_RT(H)
            RT = np.identity(4)
            RT[0:3, 0:3] = R[0:3, 0:3]
            if np.linalg.norm(t) < 50:
                RT[0:3, 3] = t.T[0]
            new_pos = compose_pose(RT)
            smoothed_pos.header = new_pos.header
            smoothed_pos.pose.position.x = (1. - alpha) * smoothed_pos.pose.position.x + alpha * new_pos.pose.position.x
            smoothed_pos.pose.position.y = (1. - alpha) * smoothed_pos.pose.position.y + alpha * new_pos.pose.position.y
            mode = Mode()
            mode.mode = 5
            lr_err.err = smoothed_pos.pose.position.x
            fb_err.err = smoothed_pos.pose.position.y
            mode.x_velocity = lr_pid.step(lr_err.err, prev_time - curr_time)
            mode.y_velocity = fb_pid.step(fb_err.err, prev_time - curr_time)
            print 'lr', mode.x_velocity, 'fb', mode.y_velocity
            prev_time = curr_time
#           mode.header.stamp = rospy.get_rostime()
            pospub.publish(mode)
        else:
            mode = Mode()
            mode.mode = 5
            mode.x_velocity = 0
            mode.y_velocity = 0
            print "LOST"
            prev_time = curr_time
            pospub.publish(mode)
Esempio n. 3
0
    
    # stefie10: Use labeled constants for the four different modes.  I
    # should never have to remember that mode 0 is disarmed or
    # whatever it really is.  http://wiki.ros.org/msg#Constants.  It
    # is okay if they are integer constants but they should be
    # referenced by name (e.g., Mode.DISARMED) in the code.  I had to
    # inspect the code to figure out what mode was what, and still got
    # it wrong when writing the keyboard controller - it added a whole
    # bunch of extra time and needless errors.  Even if ROS didn't
    # support enums, this code could have used them by defining them
    # as variable constants.

    mode_to_pub = Mode()

    while not rospy.is_shutdown():
        mode_to_pub.mode = current_mode
        modepub.publish(mode_to_pub)
        errpub.publish(error)
        mw_data = board.getData(MultiWii.ATTITUDE)
        analog_data = board.getData(MultiWii.ANALOG)

        publishRos(board, imupub, markerpub, markerarraypub, statepub)        
        
        if current_mode != 4: # stefie10: ENUM ENUM ENUM!
            # angle compensation calculations
            try:
                # stefie10: Put this code inside of a method or several methods. 

                new_angt = time.time()
                new_angx = mw_data['angx']/180.0*np.pi
                new_angy = mw_data['angy']/180.0*np.pi
    rospy.Subscriber("/pidrone/vrpn_pos", PoseStamped, sc.vrpn_callback)
    rospy.Subscriber("/pidrone/set_mode_vel", Mode, sc.mode_callback)
    rospy.Subscriber("/pidrone/heartbeat", String, sc.heartbeat_callback)

    # Non-ROS Setup
    ###############
    signal.signal(signal.SIGINT, sc.ctrl_c_handler)

    sc.prev_angt = time.time()

    mode_to_pub = Mode()
    state_to_pub = State()

    while not rospy.is_shutdown():
        # Publishes current mode message
        mode_to_pub.mode = sc.current_mode
        modepub.publish(mode_to_pub)
        # Publishes current error message
        errpub.publish(sc.error)

        # Obtains data from the flight controller
        mw_data = sc.board.getData(MultiWii.ATTITUDE)
        analog_data = sc.board.getData(MultiWii.ANALOG)

        # Publishes current battery voltage levels to display on the web interface
        state_to_pub.vbat = sc.board.analog['vbat'] * 0.10
        state_to_pub.amperage = sc.board.analog['amperage']
        statepub.publish(state_to_pub)

        try:
            if not sc.current_mode == sc.DISARMED:
Esempio n. 5
0
 def write(self, data):
     img = np.reshape(np.fromstring(data, dtype=np.uint8), (240, 320, 3))
     #       cv2.imshow("img", img)
     #       cv2.waitKey(1)
     if self.first:
         self.first = False
         self.first_kp, self.first_des = self.orb.detectAndCompute(
             img, None)
         print len(self.first_kp), "features for first"
         self.prev_kp = deepcopy(self.first_kp)
         self.prev_des = deepcopy(self.first_des)
         self.prev_time = rospy.get_time()
     else:
         curr_time = rospy.get_time()
         H, int_H = self.get_H(img)
         if H is not None:
             print "first"
             R, t, n = self.get_RT(H)
             RT = np.identity(4)
             RT[0:3, 0:3] = R[0:3, 0:3]
             if np.linalg.norm(t) < 50:
                 RT[0:3, 3] = t.T[0]
             self.est_RT = RT
             new_pos = self.compose_pose(self.est_RT)
             self.smoothed_pos.header = new_pos.header
             self.smoothed_pos.pose.position.x = (
                 1. - self.alpha
             ) * self.smoothed_pos.pose.position.x + self.alpha * new_pos.pose.position.x
             self.smoothed_pos.pose.position.y = (
                 1. - self.alpha
             ) * self.smoothed_pos.pose.position.y + self.alpha * new_pos.pose.position.y
             mode = Mode()
             mode.mode = 5
             self.lr_err.err = self.smoothed_pos.pose.position.x
             self.fb_err.err = self.smoothed_pos.pose.position.y
             mode.x_i += self.lr_pid.step(self.lr_err.err,
                                          self.prev_time - curr_time)
             mode.y_i += self.fb_pid.step(self.fb_err.err,
                                          self.prev_time - curr_time)
             self.prev_time = curr_time
             mode.featureset = 1.
             self.pospub.publish(mode)
         elif int_H is not None:
             print "only integrated"
             R, t, n = self.get_RT(int_H)
             RT = np.identity(4)
             RT[0:3, 0:3] = R[0:3, 0:3]
             if np.linalg.norm(t) < 50:
                 RT[0:3, 3] = t.T[0]
             self.est_RT = np.dot(RT, self.est_RT)
             new_pos = self.compose_pose(self.est_RT)
             self.smoothed_pos.header = new_pos.header
             self.smoothed_pos.pose.position.x = (
                 1. - self.alpha
             ) * self.smoothed_pos.pose.position.x + self.alpha * new_pos.pose.position.x
             self.smoothed_pos.pose.position.y = (
                 1. - self.alpha
             ) * self.smoothed_pos.pose.position.y + self.alpha * new_pos.pose.position.y
             mode = Mode()
             mode.mode = 5
             self.lr_err.err = self.smoothed_pos.pose.position.x
             self.fb_err.err = self.smoothed_pos.pose.position.y
             mode.x_i += self.lr_pid.step(self.lr_err.err,
                                          self.prev_time - curr_time)
             mode.y_i += self.fb_pid.step(self.fb_err.err,
                                          self.prev_time - curr_time)
             self.prev_time = curr_time
             mode.featureset = -1.
             self.pospub.publish(mode)
         else:
             print "LOST"
             self.prev_time = curr_time
Esempio n. 6
0
    def write(self, data):
        global first_counter
        global max_first_counter
        global replan_period
        global replan_last_reset
        global replan_next_deadline
        global replan_until_deadline
        global replan_vel_x
        global replan_vel_y
        global replan_scale
        global vel_average
        global vel_alpha
        global vel_average_time
        img = np.reshape(np.fromstring(data, dtype=np.uint8), (240, 320, 3))
#       cv2.imshow("img", img)
#       cv2.waitKey(1)
        curr_rostime = rospy.Time.now()
        curr_time = curr_rostime.to_sec()

        shouldi_set_velocity = 1 #0
        if np.abs(curr_time - replan_last_reset) > replan_period:
            replan_last_reset = curr_time
            replan_next_deadline = replan_last_reset + replan_period
            shouldi_set_velocity = 1
        replan_until_deadline = replan_next_deadline-curr_time
        #print curr_time, replan_last_reset, replan_next_deadline, replan_until_deadline, replan_vel_x, replan_vel_y


        if self.first:
            print "taking new first"
            self.first = False
            #self.first_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)            
            self.first_img = img
            cv2.imwrite("first_img" + str(self.i) + ".jpg", self.first_img)
            #self.prev_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
            self.prev_img = img
            self.prev_rostime = curr_rostime
            self.prev_time = curr_time

            self.i += 1
            image_message = self.bridge.cv2_to_imgmsg(img, encoding="bgr8")
            self.first_image_pub.publish(image_message)

            
        elif self.transforming:
            #curr_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
            curr_img = img
            corr_first = cv2.estimateRigidTransform(self.first_img, curr_img, False)
            corr_int = cv2.estimateRigidTransform(self.prev_img, curr_img, False)
            self.prev_img = curr_img
            self.prev_rostime = curr_rostime
            self.prev_time = curr_time


            if corr_first is not None:
                self.last_first_time = curr_time
                if curr_time - self.last_first_time > 2:
                    self.first = True
                first_displacement = [corr_first[0, 2] / 320., corr_first[1, 2] / 240.]
                scalex = np.linalg.norm(corr_first[:, 0])
                scalez = np.linalg.norm(corr_first[:, 1])
                corr_first[:, 0] /= scalex
                corr_first[:, 1] /= scalez
                self.yaw_observed = math.atan2(corr_first[1, 0], corr_first[0, 0])
                #print first_displacement, yaw_observed
                #yaw = yaw_observed
                self.smoothed_yaw = (1.0 - self.alpha_yaw) * self.smoothed_yaw + (self.alpha_yaw) * self.yaw_observed 
                yaw = self.smoothed_yaw
                vel_average[0] = (1.0 - vel_alpha) * vel_average[0] + (vel_alpha) * self.pos[0]
                # jgo XXX see what happens if we dont reset upon seeing first
                #self.pos = [first_displacement[0] * self.z, first_displacement[1] * self.z / 240., yaw]
                # jgo XXX see what happens if we use alpha blending 
                hybrid_alpha = 0.1 # needs to be between 0 and 1.0
                self.pos[0:2] = [(hybrid_alpha) * first_displacement[0] * self.z + (1.0 - hybrid_alpha) * self.pos[0],
                            (hybrid_alpha) * first_displacement[1] * self.z  + (1.0 - hybrid_alpha) * self.pos[1]]
                vel_average[0] = (1.0 - vel_alpha) * vel_average[0] + (vel_alpha) * self.pos[0]
                vel_average[1] = (1.0 - vel_alpha) * vel_average[1] + (vel_alpha) * self.pos[1]
                vel_average_time = (1.0 - vel_alpha) * vel_average_time + (vel_alpha) * curr_time
                #print "times: ", vel_average_time, curr_time, curr_time - vel_average_time
                #self.lr_err.err = vel_average[0] + self.target_x
                #self.fb_err.err = vel_average[1] + self.target_y
                self.lr_err.err = self.pos[0] + self.target_x
                self.fb_err.err = self.pos[1] + self.target_y
                print "ERR", self.lr_err.err, self.fb_err.err
                mode = Mode()
                mode.mode = 5
                mode.x_i += self.lr_pid.step(self.lr_err.err, self.prev_time - curr_time)
                mode.y_i += self.fb_pid.step(self.fb_err.err, self.prev_time - curr_time)
                #mode.x_velocity = mode.x_i
                #mode.y_velocity = mode.y_i
                # jgo XXX LOLOL constant velocity controller 
                first_counter = first_counter + 1
                max_first_counter = max(first_counter, max_first_counter)
                cvc_norm = np.sqrt(mode.x_i * mode.x_i + mode.y_i * mode.y_i)
                if cvc_norm <= 0.01:
                    cvc_norm = 1.0


                # XXX
                cvc_vel = 1.00#0.15#0.25

                if shouldi_set_velocity:
                    replan_vel_x = mode.x_i * replan_scale / max(replan_until_deadline, 0.1)
                    replan_vel_y = mode.y_i * replan_scale / max(replan_until_deadline, 0.1)
                    replan_vel_x = min(replan_vel_x, 1.0)
                    replan_vel_y = min(replan_vel_y, 1.0)
                # XXX coast if first frame found but still do PID update to
                # integrate!
                #mode.x_velocity = 0
                #mode.y_velocity = 0
                mode.x_velocity = cvc_vel * mode.x_i 
                mode.y_velocity = cvc_vel * mode.y_i 
                #mode.x_velocity = cvc_vel * mode.x_i / cvc_norm
                #mode.y_velocity = cvc_vel * mode.y_i / cvc_norm
                #mode.x_velocity = replan_vel_x
                #mode.y_velocity = replan_vel_y
                self.iacc_yaw += yaw * self.ki_yaw

                yaw_kpi_term = np.sign(yaw) * yaw * yaw * self.kpi_yaw
                if abs(yaw_kpi_term) < self.kpi_max_yaw:
                    self.iacc_yaw += yaw_kpi_term
                else:
                    self.iacc_yaw += np.sign(yaw) * self.kpi_max_yaw

                mode.yaw_velocity = yaw * self.kp_yaw + self.iacc_yaw
                print "yaw iacc: ", self.iacc_yaw
                self.pospub.publish(mode)
                print "first", max_first_counter, first_counter
            elif corr_int is not None:
                time_since_first = curr_time - self.last_first_time
                print "integrated", time_since_first
                print "max_first_counter: ", max_first_counter
                int_displacement = [corr_int[0, 2] / 320., corr_int[1, 2] / 240.]
                scalex = np.linalg.norm(corr_int[:, 0])
                scalez = np.linalg.norm(corr_int[:, 1])
                corr_int[:, 0] /= scalex
                corr_int[:, 1] /= scalez
                yaw = math.atan2(corr_int[1, 0], corr_int[0, 0])
                print int_displacement, yaw
                self.pos[0] += int_displacement[0] * self.z
                self.pos[1] += int_displacement[1] * self.z
                #self.pos[2] = yaw
                #vel_average = (1.0 - vel_alpha) * vel_average[0] + (vel_alpha) * self.pos[0]
                #vel_average = (1.0 - vel_alpha) * vel_average[1] + (vel_alpha) * self.pos[1]
                vel_average_time = (1.0 - vel_alpha) * vel_average_time + (vel_alpha) * curr_time
                #print "times: ", vel_average_time, curr_time, curr_time - vel_average_time
                #self.lr_err.err = vel_average[0] + self.target_x
                #self.fb_err.err = vel_average[1] + self.target_y
                self.lr_err.err = self.pos[0] + self.target_x
                self.fb_err.err = self.pos[1] + self.target_y
                print "ERR", self.lr_err.err, self.fb_err.err
                mode = Mode()
                mode.mode = 5
                mode.x_i += self.lr_pid.step(self.lr_err.err, self.prev_time - curr_time)
                mode.y_i += self.fb_pid.step(self.fb_err.err, self.prev_time - curr_time)
                # jgo XXX LOLOL constant velocity controller 
                first_counter = 0
                cvc_norm = np.sqrt(mode.x_i * mode.x_i + mode.y_i * mode.y_i)
                if cvc_norm <= 0.01:
                    cvc_norm = 1.0
                cvc_vel = 3.0#0.25 #1.0 

                if shouldi_set_velocity:
                    #replan_vel_x = mode.x_i * replan_scale# * cvc_vel
                    #replan_vel_y = mode.y_i * replan_scale# * cvc_vel
                    replan_vel_x = mode.x_i * replan_scale / max(replan_until_deadline, 0.1)
                    replan_vel_y = mode.y_i * replan_scale / max(replan_until_deadline, 0.1)
                    replan_vel_x = min(replan_vel_x, 1.0)
                    replan_vel_y = min(replan_vel_y, 1.0)
                #cvc_vel = max(min(time_since_first * 0.1, 1.0), 0.0)
                mode.x_velocity = cvc_vel * mode.x_i 
                mode.y_velocity = cvc_vel * mode.y_i 
                #mode.x_velocity = cvc_vel * mode.x_i / cvc_norm
                #mode.y_velocity = cvc_vel * mode.y_i / cvc_norm
                #mode.x_velocity = replan_vel_x
                #mode.y_velocity = replan_vel_y
                #mode.yaw_velocity = yaw * self.kp_yaw
                # yaw i term only
                mode.yaw_velocity = self.iacc_yaw
                print "yaw iacc: ", self.iacc_yaw
                self.pospub.publish(mode)
            else:
                print "LOST"

        else:
            #print "Not transforming"
            #curr_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
            curr_img = img
            corr_first = cv2.estimateRigidTransform(self.first_img, curr_img, False)
            if corr_first is not None:
                #print "first"
                self.last_first_time = rospy.get_time()
                if curr_time - self.last_first_time > 2:
                    self.first = True
            else:
                print "no first", curr_time - self.last_first_time
            self.prev_img = curr_img
            self.prev_rostime = curr_rostime
            self.prev_time = curr_time
            
        prev_time = curr_time
        #print "smoothed yaw: ", self.smoothed_yaw
        self.br.sendTransform((self.pos[0] * 0.01, self.pos[1] * 0.01, self.pos[2] * 0.01),
                              tf.transformations.quaternion_from_euler(0, 0, self.yaw_observed),
                              rospy.Time.now(),
                              "base",
                              "world")
Esempio n. 7
0
 def disarming(self):
     mode = Mode()
     mode.mode = "DISARMED"
     return mode
Esempio n. 8
0
def arm():
    print "arm"
    mode = Mode()
    mode.mode = "ARMED"
    return mode
Esempio n. 9
0
 def arm(self):
     mode = Mode()
     mode.mode = "ARMED"
     return mode
import rospy
from sensor_msgs.msg import Joy
import numpy as np
import os
from geometry_msgs.msg import Pose, Twist
from pidrone_pkg.msg import Mode, RC
from std_msgs.msg import Float32, Empty, Bool

z_total_steps = 24
#z_counter = (z_total_steps / 4) - 1
z_counter = -1
z_step = 5  # cm
scalar = 15
modeMsg = Mode()
modeMsg.mode = 'DISARMED'
positionMsg = Bool()
positionMsg.data = True
poseMsg = Pose()
twistMsg = Twist()

modepub = rospy.Publisher('desired/mode', Mode, queue_size=1)

#positionPub = rospy.Publisher('position_control', Bool, queue_size=1)
#positionControlPub = rospy.Publisher('desired/pose', Pose, queue_size=1)

#elocityControlPub = rospy.Publisher('desired/twist', Twist, queue_size=1)

#mappub = rospy.Publisher('map', Empty, queue_size=1)

#resetpub = rospy.Publisher('reset_transform', Empty, queue_size=1)
Esempio n. 11
0
import rospy
from pidrone_pkg.msg import Mode
from geometry_msgs.msg import PoseStamped
import numpy as np
from copy import deepcopy

mode = Mode()
mode.mode = 5
modepub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
max_accel = np.array([25., 25., 1, 1])  # cm/s/s
movements = []
pub_frequency = 10.


def sp_callback(data):
    global movements
    movement = np.array([data.pose.position.x, data.pose.position.y, 0, 0])
    movements.append(movement)


if __name__ == "__main__":
    rospy.init_node("acceleration_controller")
    rospy.Subscriber("/pidrone/pos_setpoint", PoseStamped, sp_callback)
    global max_accel, movements, pub_frequency
    while not rospy.is_shutdown():
        if len(movements) > 0:
            # movement is the desired diplacement vector
            movement = movements.pop(0)
            movement /= 2.
            travel_time = np.max(np.sqrt(2. * np.abs(movement) / max_accel))
            # the desired acceleration in each axes to ramp smoothly
Esempio n. 12
0
def disarming():
    print "disarm"
    mode = Mode()
    mode.mode = "DISARMED"
    return mode
Esempio n. 13
0
def takeoff():
    print "takeoff"
    mode = Mode()
    mode.mode = "FLYING"
    return mode
Esempio n. 14
0
import rospy
from pidrone_pkg.msg import Mode
from sensor_msgs.msg import Joy
from std_msgs.msg import Empty
import numpy as np
import os

z_total_steps = 24
#z_counter = (z_total_steps / 4) - 1
z_counter = -1
z_step = 5 # cm
scalar = 15
mode = Mode()
mode.mode = 4
modepub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
resetpub = rospy.Publisher('/pidrone/reset_transform', Empty, queue_size=1)
togglepub = rospy.Publisher('/pidrone/toggle_transform', Empty, queue_size=1)

def joy_callback(data):
    global scalar
    global modepub
    global mode
    global resetpub
    global z_counter
    global z_step
    global z_total_steps
    print "callback"
    if data.buttons[3] == 1:
        z_counter = (z_counter+1) % z_total_steps
        print 3, "Z Stepping", z_counter
        mode.mode = 5
Esempio n. 15
0
 def takeoff(self):
     mode = Mode()
     mode.mode = "FLYING"
     return mode
Esempio n. 16
0
def main():
    rospy.init_node("key_translation")
    import sys, tty, termios
    fd = sys.stdin.fileno()
    #attr = termios.tcgetattr(sys.stdin.fileno())
    #tty.setraw(sys.stdin.fileno())
    mode = Mode()
    mode.mode = 4
    modepub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
    rate = rospy.Rate(10)
    msg = """
Commands: 
;:  arm
' ' (spacebar):  disarm
t:  takeoff
l:  land
a:  yaw left
d:  yaw right
w:  up
z:  down
i:  forward
k:  backward
j:  left
l:  right
h:  hover
q:  quit
"""
    try:
        print msg
        while not rospy.is_shutdown():
            ch = getch.getch(0.1)
            if ch == None:
                continue

            if ord(ch) == 32:
                # disarm
                print "disarming"
                mode.mode = 4
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == ";":
                # arm
                print "arm"
                mode.mode = 0
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "y":
                # land
                print "land"
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                mode.mode = 3
                modepub.publish(mode)
            elif ch == "h":
                # hover
                print "hover"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "t":
                print "takeoff"
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                mode.mode = 5
                modepub.publish(mode)
            elif ch == "j":
                print "left"
                mode.mode = 5
                mode.x_velocity = -3
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "l":
                print "right"
                mode.mode = 5
                mode.x_velocity = 3
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "k":
                print "backward"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = -3
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "i":
                print "forward"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = 3
                mode.z_velocity = 0
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "a":
                print "yaw left"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = -50
                modepub.publish(mode)
            elif ch == "d":
                print "yaw right"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 0
                mode.yaw_velocity = 50
                modepub.publish(mode)
            elif ch == "w":
                print "up"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = 1
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "s":
                print "down"
                mode.mode = 5
                mode.x_velocity = 0
                mode.y_velocity = 0
                mode.z_velocity = -1
                mode.yaw_velocity = 0
                modepub.publish(mode)
            elif ch == "q":
                break
            elif ord(ch) == 3:  # Ctrl-C
                break
            else:
                print "unknown character: '%d'" % ord(ch)
                pass
            print msg
            rate.sleep()
    finally:
        print "sending disarm"
        mode.mode = 4
        modepub.publish(mode)
        time.sleep(0.25)
Esempio n. 17
0
import rospy
from pidrone_pkg.msg import Mode

if __name__ == '__main__':
    rospy.init_node('terminal_mode_selector')
    modepub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
    valid_modes = [0, 1, 3, 4, 5]
    mode_msg = Mode()
    print 'Valid modes are', valid_modes
    while not rospy.is_shutdown():
        raw_mode = raw_input('Type a mode number and press enter:\t')
        try:
            des_mode = int(raw_mode.strip())
            if des_mode in valid_modes:
                mode_msg.mode = des_mode
                print 'Sending mode {}!'.format(des_mode)
                modepub.publish(mode_msg)
            else:
                print 'Good job! You entered an integer but we dont support it... yet...'
        except Exception as e:
            print 'Bruh. {} is not an integer'.format(raw_mode)