def drive_simple_distance(self, desired_distance):
print "enter drive_simple_distance"
wv = WheelVelCmd()
wv.desiredWrist = 0.0
k = 0.8
distance_remaining = self.apriltag_distance - desired_distance
distance_threshold = 0.01
print "desired_distance", desired_distance, "distance_remaining", distance_remaining
while abs(distance_remaining) > distance_threshold:
print "desired_distance", desired_distance, "distance_remaining", distance_remaining
wv.desiredWV_R = k * distance_remaining
wv.desiredWV_L = k * distance_remaining
distance_remaining = self.apriltag_distance - desired_distance
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
print "drive_simple_distance complete"
def run(self):
wv = WheelVelCmd()
wv.desiredWrist = 0.0
if 0.3 < self.apriltag_distance < 10:
if self.tag_id in self.tags_in_view:
distance_remaining = self.apriltag_distance - self.desired_dist_away
if abs(distance_remaining) > self.distance_threshold:
wv.desiredWV_R = self.k * distance_remaining
wv.desiredWV_L = self.k * distance_remaining
print "distance_remaining", distance_remaining
else:
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.arrived = True
else:
wv.desiredWV_R = 0
wv.desiredWV_L = 0
if self.stall_count > 200:
self.arrived = True
self.stall_count += 1
else:
print "bad data"
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
def run(self):
wv = WheelVelCmd()
wv.desiredWrist = 0
if self.tag_id in self.tags_in_view:
tag_center_x = self.tag_centers[self.tag_id][0]
print "tag_center_x", tag_center_x
if tag_center_x <= self.desired_pos_in_frame - self.pixel_threshold:
wv.desiredWV_R = 0.1
wv.desiredWV_L = 0
elif tag_center_x >= self.desired_pos_in_frame + self.pixel_threshold:
wv.desiredWV_R = 0
wv.desiredWV_L = 0.1
else:
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.arrived = True
else:
if self.turn_direction == self.TURN_LEFT:
wv.desiredWV_R = 0.1
wv.desiredWV_L = 0
elif self.turn_direction == self.TURN_RIGHT:
wv.desiredWV_R = 0
wv.desiredWV_L = 0.1
else:
print "kbai"
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
def move_wrist(self, position):
wv = WheelVelCmd()
wv.desiredWV_R = 0 # don't move...
wv.desiredWV_L = 0
wv.desiredWrist = position
self.velcmd_pub.publish(wv)
return 0
def drive_simple_y(self, desired_delta_y):
print "enter drive_simple_y"
wv = WheelVelCmd()
wv.desiredWrist = 0.0
current_delta_y = 0
prev_enc_count = self.enc_y
start_y = self.current_y
print "start_y", start_y
desired_y = start_y + desired_delta_y
k = 0.6
if self.alpha >= 0:
turn_direction = self.TURN_LEFT
else:
turn_direction = self.TURN_RIGHT
start_enc_count = prev_enc_count
while abs(desired_y - self.current_y) > self.y_threshold:
while not (desired_delta_y - self.y_threshold < current_delta_y <
desired_delta_y + self.y_threshold):
print "self.current_y", self.current_y, "desired_delta_y:", desired_delta_y, "current_delta_y:", current_delta_y
print "current encoder location", self.enc_y
wv.desiredWV_R = k * abs(desired_delta_y - current_delta_y)
wv.desiredWV_L = k * abs(desired_delta_y - current_delta_y)
self.velcmd_pub.publish(wv)
y_inc = abs(self.enc_y - prev_enc_count)
if y_inc <= self.enc_delta_threshold:
current_delta_y = abs(self.enc_y - start_enc_count)
else:
print "ignoring"
rospy.sleep(0.01)
prev_enc_count = self.enc_y
print "current encoder count", prev_enc_count - start_enc_count, "current_delta_y:", current_delta_y
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
self.turn_to_tag(turn_direction)
self.get_current_point()
print "abs(desired_y - self.current_y)", abs(desired_y -
self.current_y)
# resolve current point
est_y = start_y + current_delta_y
obs_y = self.current_y
self.current_y = est_y # always take encoder data
print "check try y again? abs(desired_y - self.current_y)", abs(
desired_y - self.current_y)
print "drive_simple_y complete"
print "current_y:", self.current_y, "current_alpha", self.current_theta
def drive_simple_x(self, desired_delta_x):
print "enter drive_simple_x"
wv = WheelVelCmd()
wv.desiredWrist = 0.0
current_delta_x = 0
prev_enc_count = self.enc_x
start_x = self.current_x
print "start_x", start_x
desired_x = start_x + desired_delta_x
k = 0.6
if self.alpha >= 0:
turn_direction = self.TURN_LEFT
else:
turn_direction = self.TURN_RIGHT
start_enc_count = prev_enc_count
while abs(desired_x - self.current_x) > self.x_threshold:
while not (desired_delta_x - self.x_threshold < current_delta_x <
desired_delta_x + self.x_threshold):
wv.desiredWV_R = k * abs(desired_delta_x - current_delta_x)
wv.desiredWV_L = k * abs(desired_delta_x - current_delta_x)
self.velcmd_pub.publish(wv)
x_inc = abs(self.enc_x - prev_enc_count)
if x_inc <= self.enc_delta_threshold:
current_delta_x = abs(self.enc_x - start_enc_count)
else:
print "ignoring"
rospy.sleep(0.01)
prev_enc_count = self.enc_x
print "desired_delta_x", desired_delta_x, "current_delta_x:", current_delta_x
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
self.turn_to_tag(turn_direction)
self.get_current_point()
print "abs(desired_x - self.current_x)", abs(desired_x -
self.current_x)
# resolve current point
est_x = start_x + current_delta_x
obs_x = self.current_x
self.current_x = est_x # always take encoder data
print "check try x again? abs(desired_x - self.current_x)", abs(
desired_x - self.current_x)
print "drive_simple_x complete"
print "current_x:", self.current_x, "current_alpha", self.current_theta
def stop(self):
wv = WheelVelCmd()
if not rospy.is_shutdown():
print '1. Tag not in view, Stop'
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
wv.desiredWrist = 0.0
self.velcmd_pub.publish(wv)
rospy.sleep(.01)
def turn_to_tag(self, direction):
wv = WheelVelCmd()
while self.current_input not in self.tags_in_view:
if direction == self.TURN_RIGHT:
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
elif direction == self.TURN_LEFT:
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
else:
print "invalid turn direction"
wv.desiredWrist = 0.0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
camera_center_x = 320
camera_x_threshold = 5
tag_x = self.tag_centers[self.current_input][0]
while not camera_center_x - camera_x_threshold < tag_x < camera_center_x + camera_x_threshold:
print "tag_x", tag_x
if direction == self.TURN_RIGHT:
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
elif direction == self.TURN_LEFT:
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
else:
print "invalid turn direction"
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
tag_x = self.tag_centers[self.current_input][0]
wv.desiredWV_R = 0
wv.desiredWV_L = 0
wv.desiredWrist = 0.0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
def turn_alpha(self, target_tag):
print "enter turn_alpha"
wv = WheelVelCmd()
wv.desiredWrist = 0.0
offset = self.tag_angles[target_tag]
self.alpha = -(self.current_theta - offset)
if self.current_input in self.turn_directions:
turn_direction = self.turn_directions[self.current_input]
elif self.alpha <= 0:
turn_direction = self.TURN_LEFT
else:
turn_direction = self.TURN_RIGHT
current_delta_alpha = 0 # how much alpha we have gone, pos if TL/neg if TR
start_enc_theta = self.enc_theta # what is enc theta right now, before moving?
start_enc_x = self.enc_x
start_enc_y = self.enc_y
if turn_direction == self.TURN_LEFT:
turn_angle = np.pi / 2 - (-self.alpha)
elif turn_direction == self.TURN_RIGHT:
turn_angle = self.alpha - np.pi / 2
while not (turn_angle - self.theta_threshold < current_delta_alpha <
turn_angle + self.theta_threshold):
if turn_direction == self.TURN_LEFT:
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
elif turn_direction == self.TURN_RIGHT:
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
else:
print "invalid turn direction"
self.velcmd_pub.publish(wv)
current_delta_alpha = self.enc_theta - start_enc_theta
rospy.sleep(0.01)
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
print "before end of turn_alpha", self.current_x
self.current_x = self.current_x + (self.enc_x - start_enc_x)
self.current_y = self.current_y + (self.enc_y - start_enc_y)
print "end of turn_alpha", self.current_x
print "turn_alpha complete"
def run(self):
wv = WheelVelCmd()
wv.desiredWrist = 0
if self.dist < self.desired_dist:
wv.desiredWV_R = self.speed
wv.desiredWV_L = self.speed
else:
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.arrived = True
self.dist = np.sqrt((self.enc_x - self.start_enc_x)**2 + (self.enc_y - self.start_enc_y)**2)
self.velcmd_pub.publish(wv)
rospy.sleep(.01)
def run(self):
wv = WheelVelCmd()
if self.current_input in self.tags_in_view:
print "found target"
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.found_target = True
elif self.current_input == 2:
wv.desiredWV_R = 0
wv.desiredWV_L = 0
elif self.current_input in self.right_turns:
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
else:
# turn left
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
wv.desiredWrist = 0.0
self.velcmd_pub.publish(wv)
rospy.sleep(.01)
def turn_to_special_tag_normal(self, direction, tag):
wv = WheelVelCmd()
wv.desiredWrist = 0.0
while tag not in self.tags_in_view:
if direction == self.TURN_RIGHT:
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
elif direction == self.TURN_LEFT:
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
else:
print "invalid turn direction"
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
camera_center_x = 320
camera_x_threshold = 5
tag_x = self.tag_centers[tag][0]
while not camera_center_x - camera_x_threshold < tag_x < camera_center_x + camera_x_threshold:
if direction == self.TURN_RIGHT:
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
elif direction == self.TURN_LEFT:
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
else:
print "invalid turn direction"
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)