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)
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)
# 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:
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
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")
def disarming(self): mode = Mode() mode.mode = "DISARMED" return mode
def arm(): print "arm" mode = Mode() mode.mode = "ARMED" return mode
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)
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
def disarming(): print "disarm" mode = Mode() mode.mode = "DISARMED" return mode
def takeoff(): print "takeoff" mode = Mode() mode.mode = "FLYING" return mode
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
def takeoff(self): mode = Mode() mode.mode = "FLYING" return mode
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)
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)