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)
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)
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")
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
adjusted_accel = (2. * movement) / (travel_time**2)
# the number of velocity points to ramp up
num_points = int(np.floor(pub_frequency * travel_time))
# generate the list of desired velocities to ramp up
command_list = []
for i in range(1, num_points + 1):
command_list.append((i / pub_frequency) * adjusted_accel)
command_list_r = deepcopy(command_list)
command_list_r.reverse()
command_list += command_list_r # mirror to ramp back down
command_list.append(np.array([0., 0., 0., 0.]))
for cmd in command_list:
mode.x_velocity = cmd[0]
mode.y_velocity = cmd[1]
mode.z_velocity = cmd[2]
mode.yaw_velocity = cmd[3]
print mode
modepub.publish(mode)
rospy.sleep(1. / pub_frequency)