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 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
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 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 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 not self.found_pokemon:
print "driving"
wv.desiredWV_R = 0.05
wv.desiredWV_L = 0.05
else:
print "stopping"
wv.desiredWV_R = 0.0
wv.desiredWV_L = 0.0
self.should_stop = True
self.velcmd_pub.publish(wv)
rospy.sleep(0.1)
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 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 navi_loop(self):
robot_pos2d = [0, 0]
thres_dist = 0.1
thres_angle = 0.2
k = 0.5
wv = WheelVelCmd()
while not rospy.is_shutdown():
object_pose3d = helper.lookupTransformList('/base_link', '/object', self.listener)
if object_pose3d is None:
print 'object not in view'
wv.desiredWV_R = 0
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
continue
object_pos2d = np.array(robot_pose3d[0:2])
dist = np.linalg.norm(object_pos2d - robot_pos2d)
angle = np.arctan2(object_pos2d[1] - robot_pos2d[1], object_pos2d[0] - robot_pos2d[0])
if dist <= thres_dist and abs(angle) <= thres_angle:
print 'Get close enough to the object'
wv.desiredWV_R = 0
wv.desiredWV_L = 0
elif dist > thres_dist and abs(angle) <= thres_angle:
wv.desiredWV_R = 0.5
wv.desiredWV_L = 0.5
elif dist <= thres_dist and abs(angle) > thres_angle:
wv.desiredWV_R = 0.2*np.sign(angle)
wv.desiredWV_L = -0.2*np.sign(angle)
else:
w = k*angle
vel = 0.5
wv.desiredWV_R = (vel+w*b)/r
wv.desiredWV_L = (vel-w*b)/r
self.velcmd_pub.publish(wv)
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 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)
def MoveTime(wvelR, wvelL, duration):
velcmd_pub = rospy.Publisher("/cmdvel", WheelVelCmd, queue_size=1)
rate = rospy.Rate(100) # 100hz
wcv = WheelVelCmd()
t_end = time.time() + duration
print time.time()
print t_end
while time.time() < t_end:
wcv.desiredWV_R = wvelR
wcv.desiredWV_L = wvelL
velcmd_pub.publish(wcv)
rate.sleep()
def MoveJames(robotVel, K, path_dist):
velcmd_pub = rospy.Publisher("/cmdvel", WheelVelCmd, queue_size=1)
rate = rospy.Rate(100) # 100hz
#robotVel,K,path_dist
r = 0.037
b = 0.225
wcv = WheelVelCmd()
numIter = int(round((path_dist / robotVel) * 100, 0))
print(numIter)
for i in range(1, numIter):
#wcv.desiredWV_R = 0.5
#wcv.desiredWV_L = 0.5
wcv.desiredWV_R = (r) * ((robotVel / r) + (K * b * robotVel) / (r))
wcv.desiredWV_L = (r) * ((robotVel / r) - (K * b * robotVel) / (r))
velcmd_pub.publish(wcv)
rate.sleep()
def navi_loop(self):
##
target_pose2d = [0.25, 0, np.pi]
##
wv = WheelVelCmd()
##
arrived = False
arrived_position = False
while not rospy.is_shutdown():
##
# 1. get robot pose
robot_pose3d = helper.lookupTransformList('/map', '/base_link',
self.listener)
if robot_pose3d is None:
print '1. Tag not in view, Stop'
wv.desiredWV_R = 0 # right, left
wv.desiredWV_L = 0
self.velcmd_pub.publish(wv)
continue
robot_position2d = robot_pose3d[0:2]
target_position2d = target_pose2d[0:2]
robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
robot_pose2d = robot_position2d + [robot_yaw]
# 2. navigation policy
# 2.1 if in the target,
# 2.2 else if close to target position, turn to the target orientation
# 2.3 else if in the correct heading, go straight to the target position,
# 2.4 else turn in the direction of the target position
pos_delta = np.array(target_position2d) - np.array(
robot_position2d)
robot_heading_vec = np.array(
[np.cos(robot_yaw), np.sin(robot_yaw)])
heading_err_cross = helper.cross2d(
robot_heading_vec, pos_delta / np.linalg.norm(pos_delta))
# print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
# print 'pos_delta', pos_delta
# print 'robot_yaw', robot_yaw
# print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
# print 'heading_err_cross', heading_err_cross
if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
diffrad(robot_yaw, target_pose2d[2])) < 0.05):
print 'Case 2.1 Stop'
wv.desiredWV_R = 0
wv.desiredWV_L = 0
arrived = True
elif np.linalg.norm(pos_delta) < 0.08:
arrived_position = True
if diffrad(robot_yaw, target_pose2d[2]) > 0:
print 'Case 2.2.1 Turn right slowly'
wv.desiredWV_R = -0.05
wv.desiredWV_L = 0.05
else:
print 'Case 2.2.2 Turn left slowly'
wv.desiredWV_R = 0.05
wv.desiredWV_L = -0.05
elif arrived_position or np.fabs(heading_err_cross) < 0.2:
print 'Case 2.3 Straight forward'
wv.desiredWV_R = 0.1
wv.desiredWV_L = 0.1
else:
if heading_err_cross < 0:
print 'Case 2.4.1 Turn right'
wv.desiredWV_R = -0.1
wv.desiredWV_L = 0.1
else:
print 'Case 2.4.2 Turn left'
wv.desiredWV_R = 0.1
wv.desiredWV_L = -0.1
self.velcmd_pub.publish(wv)
rospy.sleep(0.01)
def navi_loop():
velcmd_pub = rospy.Publisher("/cmdvel", WheelVelCmd, queue_size=1)
target_pose2d = [0.25, 0.80, np.pi]
rate = rospy.Rate(100) # 100hz
wcv = WheelVelCmd()
arrived = False
arrived_position = False
while not rospy.is_shutdown():
# 1. get robot pose
robot_pose3d = lookupTransformList('/map', '/robot_base', lr)
if robot_pose3d is None:
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
velcmd_pub.publish(wcv)
rate.sleep()
continue
robot_position2d = robot_pose3d[0:2]
target_position2d = target_pose2d[0:2]
robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
robot_pose2d = robot_position2d + [robot_yaw]
# 2. navigation policy
# 2.1 if in the target,
# 2.2 else if close to target position, turn to the target orientation
# 2.3 else if in the correct heading, go straight to the target position,
# 2.4 else turn in the direction of the target position
pos_delta = np.array(target_position2d) - np.array(robot_position2d)
robot_heading_vec = np.array([np.cos(robot_yaw), np.sin(robot_yaw)])
heading_err_cross = cross2d(robot_heading_vec,
pos_delta / np.linalg.norm(pos_delta))
heading_err_align = np.dot(pos_delta, robot_heading_vec)
print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
print 'pos_delta', pos_delta
print 'robot_yaw', robot_yaw
print 'norm delta', np.linalg.norm(pos_delta), 'diffrad', diffrad(
robot_yaw, target_pose2d[2])
print 'heading_err_cross', heading_err_cross
if arrived or (np.linalg.norm(pos_delta) < 0.08 and
np.fabs(diffrad(robot_yaw, target_pose2d[2])) < 0.05):
print 'Case 2.1 Stop'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
elif np.linalg.norm(pos_delta) < 0.08:
arrived_position = True
if diffrad(robot_yaw, target_pose2d[2]) > 0:
print 'Case 2.2.1 Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print 'Case 2.2.2 Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
elif arrived_position or np.fabs(heading_err_cross) < 0.2:
if (heading_error_align > 0):
print 'Case 2.3.1 Straight forward'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
print 'Case 2.3.2 Straight backward'
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = -0.1
else:
if heading_err_cross < 0:
print 'Case 2.4.1 Turn right'
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = 0.1
else:
print 'Case 2.4.2 Turn left'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = -0.1
velcmd_pub.publish(wcv)
rate.sleep()
def navi_loop(self):
wv = WheelVelCmd()
arrived = False
arrived_position = False
is_plan = False
robot_pose3d = [0.635, 0.22]
# Auto-start
robot_pose3d = helper.lookupTransformList('/map', '/robot_base',
self.listener)
while robot_pose3d == None:
robot_pose3d = helper.lookupTransformList('/map', '/robot_base',
self.listener)
wv.desiredWV_R = 0 # Zero Velocities
wv.desiredWV_L = 0
print "Left: ", wv.desiredWV_L, "Right: ", wv.desiredWV_R
self.velcmd_pub.publish(wv)
##hand_transition("close_hand")
##rospy.sleep(8)
hand_transition("open_hand")
rospy.sleep(2)
# Loop Start
while not rospy.is_shutdown():
# Update Pose
robot_pose2d, robot_position2d = self.update_pose(robot_pose3d)
# Check if at Goal
if Goal_test(robot_pose2d, targets[0]):
print "Reached Target"
is_plan = False
del targets[0]
if len(targets) == 8:
# Grab tin
# grab_pidgey(robot_pose2d)
hand_transition("close_hand")
wv = WheelVelCmd()
wv.desiredWV_L, wv.desiredWV_R = -0.1, -0.1
self.velcmd_pub.publish(wv)
rospy.sleep(5)
elif len(targets) == 6:
# Drop Tin
hand_transition("open_hand")
elif len(targets) == 5:
hand_transition("close_hand")
wv = WheelVelCmd()
wv.desiredWV_L, wv.desiredWV_R = -0.1, -0.1
self.velcmd_pub.publish(wv)
rospy.sleep(5)
elif len(targets) == 3:
hand_transition("open_hand")
elif len(targets) == 2:
hand_transition("pikachu")
wv = WheelVelCmd()
wv.desiredWV_L, wv.desiredWV_R = -0.1, -0.1
self.velcmd_pub.publish(wv)
rospy.sleep(5)
elif len(targets) == 1:
hand_transition("open_hand")
self.check_obstacles()
# Generate New Plan
if (not is_plan) or (
(.08, 1.47, .17) in obstacle_list) and (tmp == 0):
smoothedPath, is_plan = self.replan(robot_position2d,
targets[0], obstacle_list)
# Waypoint Follow
if distance(robot_pose2d, smoothedPath[0]) < 0.05:
del smoothedPath[0]
# Generate Wheel Velocities
l, r = generateWheelVel(robot_pose2d, smoothedPath[0])
print "Next Point: ", smoothedPath[0]
wv.desiredWV_L, wv.desiredWV_R = l, r
print "Left: ", wv.desiredWV_L, "Right: ", wv.desiredWV_R
self.velcmd_pub.publish(wv)
#print "Obstacles: ", obstacle_list
rospy.sleep(0.01)
def onshutdown(self):
wv = WheelVelCmd()
wv.desiredWV_L, wv.desiredWV_R = 0, 0
self.velcmd_pub.publish(wv)
def navi_loop_ypos(target_pose2d):
global lr, br
velcmd_pub = rospy.Publisher("/cmdvel", WheelVelCmd, queue_size=1)
rate = rospy.Rate(100) # 100hz
wcv = WheelVelCmd()
arrived = False
arrived_position = False
count = 0
while not rospy.is_shutdown():
# 1. get robot pose
robot_pose3d = lookupTransformList('/map', '/robot_base', lr)
if robot_pose3d is None:
print '1. Tag not in view, Stop', '/ Target Pose:', target_pose2d
wcv.desiredWV_R = 0.0 # right, left
wcv.desiredWV_L = 0.0
velcmd_pub.publish(wcv)
rate.sleep()
continue
robot_position2d = robot_pose3d[0:2]
target_position2d = target_pose2d[0:2]
robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
robot_pose2d = robot_position2d + [robot_yaw]
rawDataFilt = [0, 0, 0]
resetVal = [0, 0, 0]
realSig = [0, 0, 0]
# Apply AprilTagNoiseCorr function to each direction
#if count == 0:
#PrevPos = robot_pose2d
#resetVal = robot_pose2d
#realSig = 1
#else:
#[rawDataFilt[0], resetVal[0], realSig[0]] = AprilTagNoiseCorr( currentPos[0], PrevPos[0], resetVal[0], realSig[0] )
#[rawDataFilt[1], resetVal[1], realSig[1]] = AprilTagNoiseCorr( currentPos[1], PrevPos[1], resetVal[1], realSig[1] )
#[rawDataFilt[2], resetVal[2], realSig[2]] = AprilTagNoiseCorr( currentPos[2], PrevPos[2], resetVal[2], realSig[2] )
#robot_pose2d = rawDataFilt;
# 2. navigation policy
# 2.1 if in the target,
# 2.2 else if close to target position, turn to the target orientation
# 2.3 else if in the correct heading, go straight to the target position,
# 2.4 else turn in the direction of the target position
pos_delta = np.array(target_position2d) - np.array(robot_position2d)
robot_heading_vec = np.array([np.cos(robot_yaw), np.sin(robot_yaw)])
heading_err_cross = cross2d(robot_heading_vec,
pos_delta / np.linalg.norm(pos_delta))
#print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
print 'pos_delta', pos_delta
#print 'robot_yaw', robot_yaw
#print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
#print 'heading_err_cross', heading_err_cross
print 'robot pose', robot_pose2d, 'target pose', target_pose2d
if arrived or (np.linalg.norm(pos_delta) < 0.08 and
np.fabs(diffrad(robot_yaw, target_pose2d[2])) < 0.05):
print 'Case 2.1 Stop'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
return
elif np.linalg.norm(pos_delta) < 0.08: # distance is less than 8cm
arrived_position = True
if diffrad(robot_yaw,
target_pose2d[2]) > 0: # radians difference > 0
print 'Case 2.2.1 Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print 'Case 2.2.2 Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
elif arrived_position or np.fabs(
heading_err_cross) < 0.2: # not arrived OR heading
if (pos_delta[1] > 0): # (y_target - y_robot) > 0
print 'Case 2.3.1 Straight forward'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
print 'Case 2.3.2 Straight backward'
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = -0.1
else:
if heading_err_cross < 0: # cross product of robot vec & target vec < 0
print 'Case 2.4.1 Turn right'
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = 0.1
else:
print 'Case 2.4.2 Turn left'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = -0.1
velcmd_pub.publish(wcv)
rate.sleep()