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(self):
generic_target_pose2d = [0.4, 0, np.pi]
#target_pose2d = [0.25, 0, np.pi]
rate = rospy.Rate(100) # 100hz
wcv = WheelCmdVel()
arrived = False
arrived_position = False
self.print_id = -1
while not rospy.is_shutdown():
if self.idx >= 10:
break
# 1. get robot pose
robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')
target_pose2d = generic_target_pose2d
if self.idx == 6:
print 'idx =', self.idx
if self.idx6cnt < 4:
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
self.idx6cnt = self.idx6cnt + 1
self.velcmd_pub.publish(wcv)
else:
self.idx = self.idx + 1
rospy.sleep(3.2)
elif self.idx == 7:
# close gripper
self.idx = self.idx + 1
rospy.sleep(5)
elif self.idx == 8:
print 'idx =', self.idx
if self.idx6cnt > 0:
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = -0.1
self.idx6cnt = self.idx6cnt - 1
self.velcmd_pub.publish(wcv)
else:
self.idx = self.idx + 1
rospy.sleep(3.2)
elif self.idx == 9:
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.idx = self.idx + 1
self.velcmd_pub.publish(wcv)
elif self.idx == 5 and self.detection_id == 3:
if robot_pose3d is None:
print_id = 0
if print_id != self.print_id:
self.print_id = print_id
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
self.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, stop
# 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
if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
diffrad(robot_yaw, target_pose2d[2])) < 0.05):
print_id = 21
if print_id != self.print_id:
self.print_id = print_id
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_id = 221
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.2.1 Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print_id = 222
if print_id != self.print_id:
self.print_id = print_id
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:
print_id = 23
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.3 Straight forward'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
if heading_err_cross < 0:
print_id = 241
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.1 Turn right'
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = 0.1
else:
print_id = 242
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.2 Turn left'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = -0.1
self.velcmd_pub.publish(wcv)
if arrived:
print 'old idx', self.idx, 'new idx', self.idx + 1
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(1)
rate.sleep()
def navi_loop():
velcmd_pub = rospy.Publisher("/cmdvel", WheelCmdVel, queue_size=1)
target_pose2d = [0.25, 0, np.pi]
rate = rospy.Rate(100) # 100hz
wcv = WheelCmdVel()
arrived = False
arrived_position = False
while not rospy.is_shutdown():
# 1. get robot pose
robot_pose3d = lookupTransform(lr, '/map', '/robot_base')
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)
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, stop
# 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
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:
print 'Case 2.3 Straight forward'
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_0(self):
first_turn_point_id6 = [0.95, 0.28, -3.00] # apriltag id = 6
stop_first_turn_lo = [2.38, 0.84, -2.75] # apriltag id = 4
stop_first_turn_up = [2.30, 0.86, -3.03] # apriltag id = 4
generic_target_pose2d = [0.4, 0, np.pi]
#target_pose2d = [0.25, 0, np.pi]
rate = rospy.Rate(100) # 100hz
wcv = WheelCmdVel()
arrived = False
arrived_position = False
self.print_id = -1
while not rospy.is_shutdown() :
print 'self.idx',self.idx
if self.idx >= 6:
break
# 0. set target
if self.idx == 1:
target_pose2d = first_turn_point_id6
elif self.idx == 2:
target_pose2d = stop_first_turn_lo
else:
target_pose2d = generic_target_pose2d
# 1. get robot pose
robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')
if self.idx == 1 and self.detection_id == 6:
if robot_pose3d is None:
print_id = 0
if print_id != self.print_id:
self.print_id = print_id
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
self.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]
if robot_pose2d[0] > target_pose2d[0]:
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rate.sleep()
elif (self.idx == 3 and self.detection_id == 4) or (self.idx == 5 and self.detection_id == 3):
if robot_pose3d is None:
print_id = 0
if print_id != self.print_id:
self.print_id = print_id
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
self.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, stop
# 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
if arrived or (np.linalg.norm( pos_delta ) < 0.08 and np.fabs(diffrad(robot_yaw, target_pose2d[2]))<0.05) :
print_id = 21
if print_id != self.print_id:
self.print_id = print_id
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_id = 221
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.2.1 Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print_id = 222
if print_id != self.print_id:
self.print_id = print_id
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:
print_id = 23
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.3 Straight forward'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
if heading_err_cross < 0:
print_id = 241
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.1 Turn right'
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = 0.1
else:
print_id = 242
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.2 Turn left'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = -0.1
self.velcmd_pub.publish(wcv)
if arrived:
print 'old idx', self.idx, 'new idx', self.idx + 1
self.idx = self.idx + 1
arrived = False
arrived_position = False
rate.sleep()
rate.sleep()
elif self.idx == 2:
if robot_pose3d is not None:
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]
if robot_pose3d is None or robot_yaw > stop_first_turn_lo[2] or self.detection_id != 4:
print_id = 2001
if print_id != self.print_id:
self.print_id = print_id
print 'Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
'''
elif robot_yaw < stop_first_turn_up[2]:
print_id = 2002
if print_id != self.print_id:
self.print_id = print_id
print 'Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
'''
else:
print_id = 2003
if print_id != self.print_id:
self.print_id = print_id
print 'idx2 finished'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rate.sleep()
elif self.idx == 4:
if self.detection_id == 3:
print_id = 4002
if print_id != self.print_id:
self.print_id = print_id
print 'idx4 finished'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
else:
print_id = 4001
if print_id != self.print_id:
self.print_id = print_id
print 'Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rate.sleep()
rate.sleep()
def measure(self):
target_pose2d = [0.25, 0, np.pi]
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
# 1. get robot pose
robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')
if robot_pose3d is None:
print '1. Tag not in view'
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]
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
## print '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:
print 'Case 2.3 Straight forward'
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
self.velcmd_pub.publish(wcv)
'''
rate.sleep()
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()
def navi_loop(self):
first_turn_point_id6_route0 = [0.95, 0.28, -3.00] # apriltag id = 6
first_turn_point_id6_route1 = [0.38, 0.09, 3.076] # apriltag id = 6
stop_first_turn_lo_route0 = [2.38, 0.84, -2.75] # apriltag id = 4
stop_first_turn_up_route0 = [2.30, 0.86, -3.03] # apriltag id = 4
stop_first_turn_lo_route1 = [2.14, -0.45, 2.95] # apriltag id = 4
stop_first_turn_up_route1 = [2.01, 0.34, 3.10] # apriltag id = 4
generic_target_pose2d = [0.4, 0, np.pi]
#target_pose2d = [0.25, 0, np.pi]
rate = rospy.Rate(100) # 100hz
wcv = WheelCmdVel()
arrived = False
arrived_position = False
self.print_id = -1
self.idx6cnt = 0
while not rospy.is_shutdown():
# print 'self.idx',self.idx
if self.idx >= 7:
break
if self.idx <= 0: # haven't found obstacle
rospy.sleep(2)
continue
# 0. set target
if self.idx == 1:
if self.route_idx == 0:
target_pose2d = first_turn_point_id6_route0
else:
target_pose2d = first_turn_point_id6_route1
elif self.idx == 2:
target_pose2d = stop_first_turn_lo_route0
else:
target_pose2d = generic_target_pose2d
# 1. get robot pose
robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')
if self.idx == 6:
if self.idx6cnt < 4:
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
self.idx6cnt = self.idx6cnt + 1
else:
self.idx = self.idx + 1
# idx == 1, start from initial position
if self.idx == 1 and self.detection_id == 6:
if robot_pose3d is None:
print_id = 0
if print_id != self.print_id:
self.print_id = print_id
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
self.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]
if robot_pose2d[0] > target_pose2d[0]:
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(1)
# idx == 3 or 5, go to human or target
elif (self.idx == 3
and self.detection_id == 4) or (self.idx == 5
and self.detection_id == 3):
if robot_pose3d is None:
print_id = 0
if print_id != self.print_id:
self.print_id = print_id
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
self.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, stop
# 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
if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
diffrad(robot_yaw, target_pose2d[2])) < 0.05):
print_id = 21
if print_id != self.print_id:
self.print_id = print_id
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_id = 221
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.2.1 Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print_id = 222
if print_id != self.print_id:
self.print_id = print_id
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:
print_id = 23
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.3 Straight forward'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
if heading_err_cross < 0:
print_id = 241
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.1 Turn right'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print_id = 242
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.2 Turn left'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
self.velcmd_pub.publish(wcv)
if arrived:
print 'old idx', self.idx, 'new idx', self.idx + 1
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(3)
# idx == 2, turn right and go to human
elif self.idx == 2:
if self.route_idx == 0: # route 0: turn right early, smaller degrees
if robot_pose3d is not None:
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]
if (robot_pose3d is None) or (self.detection_id != 4) or (
self.detection_id == 4
and robot_yaw > stop_first_turn_lo_route0[2]):
print_id = 2001
if print_id != self.print_id:
self.print_id = print_id
print 'Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
'''
elif robot_yaw < stop_first_turn_up[2]:
print_id = 2002
if print_id != self.print_id:
self.print_id = print_id
print 'Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
'''
else:
print_id = 2003
if print_id != self.print_id:
self.print_id = print_id
print 'idx2 finished'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
else: # route 1: turn right late, larger degrees, near 90 degrees.
if robot_pose3d is not None:
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]
if (robot_pose3d is None) or (self.detection_id != 4) or (
self.detection_id == 4 and
(robot_yaw < stop_first_turn_lo_route1[2]
or robot_yaw > stop_first_turn_up_route1[2])):
# make it between 2.95 and 3.10
print_id = 2001
if print_id != self.print_id:
self.print_id = print_id
print 'Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
'''
elif robot_yaw < stop_first_turn_up[2]:
print_id = 2002
if print_id != self.print_id:
self.print_id = print_id
print 'Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
'''
else:
print_id = 2003
if print_id != self.print_id:
self.print_id = print_id
print 'idx2 finished'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(1)
# idx == 4, turn left and go to target
elif self.idx == 4:
if self.detection_id == 3:
print_id = 4002
if print_id != self.print_id:
self.print_id = print_id
print 'idx4 finished'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
else:
print_id = 4001
if print_id != self.print_id:
self.print_id = print_id
print 'Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(1)
rate.sleep()
def navi_loop(self):
first_turn_point_id6_route0 = [0.95, 0.28, -3.00] # apriltag id = 6
first_turn_point_id6_route1 = [0.38, 0.09, 3.076] # apriltag id = 6
stop_first_turn_lo_route0 = [2.38, 0.84, -2.75] # apriltag id = 4
stop_first_turn_up_route0 = [2.30, 0.86, -3.03] # apriltag id = 4
stop_first_turn_lo_route1 = [2.14, -0.45, 2.95] # apriltag id = 4
stop_first_turn_up_route1 = [2.01, 0.34, 3.10] # apriltag id = 4
generic_target_pose2d = [0.4, 0, np.pi]
#target_pose2d = [0.25, 0, np.pi]
rate = rospy.Rate(100) # 100hz
wcv = WheelCmdVel()
wcv.desiredGripperPos = gripperOpen
arrived = False
arrived_position = False
self.print_id = -1
while not rospy.is_shutdown():
print 'self.idx', self.idx
if self.idx >= 11:
break
if self.idx <= 0: # haven't found obstacle
rospy.sleep(2)
continue
# 0. set target
target_pose2d = generic_target_pose2d
# 1. get robot pose
robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')
# idx == 1, start from initial position
if self.idx == 1:
if self.route_idx == 0:
self.idx = self.idx + 1
# go straight
wcv.desiredWV_R = 0.2
wcv.desiredWV_L = 0.2
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
# stop
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
# turn right a few degrees
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.2
self.velcmd_pub.publish(wcv)
rospy.sleep(5.5)
# stop
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
# go straight
wcv.desiredWV_R = 0.5
wcv.desiredWV_L = 0.5
self.velcmd_pub.publish(wcv)
rospy.sleep(4)
# stop
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
elif self.route_idx == 1:
self.idx = self.idx + 1
# go straight
wcv.desiredWV_R = 0.3
wcv.desiredWV_L = 0.3
self.velcmd_pub.publish(wcv)
rospy.sleep(4.5)
# stop
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
# turn right a few degrees
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = 0.1
self.velcmd_pub.publish(wcv)
rospy.sleep(3)
# stop
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
# go straight
wcv.desiredWV_R = 0.5
wcv.desiredWV_L = 0.5
self.velcmd_pub.publish(wcv)
rospy.sleep(3) #was3
# stop
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
elif self.idx == 2:
self.idx = self.idx + 1
# idx == 2 or 5, go to human or target
elif (self.idx == 5 and self.detection_id == 3) \
or (self.idx == 10 and self.detection_id == 4):
if robot_pose3d is None:
print_id = 0
if print_id != self.print_id:
self.print_id = print_id
print '1. Tag not in view, Stop'
wcv.desiredWV_R = 0 # right, left
wcv.desiredWV_L = 0
self.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, stop
# 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
if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
diffrad(robot_yaw, target_pose2d[2])) < 0.05):
print_id = 21
if print_id != self.print_id:
self.print_id = print_id
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_id = 221
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.2.1 Turn right slowly'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print_id = 222
if print_id != self.print_id:
self.print_id = print_id
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:
print_id = 23
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.3 Straight forward'
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
else:
if heading_err_cross < 0:
print_id = 241
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.1 Turn right'
wcv.desiredWV_R = -0.05
wcv.desiredWV_L = 0.05
else:
print_id = 242
if print_id != self.print_id:
self.print_id = print_id
print 'Case 2.4.2 Turn left'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
self.velcmd_pub.publish(wcv)
if arrived:
print 'old idx', self.idx, 'new idx', self.idx + 1
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(3)
# idx == 3, wait for human signal
# apriltag id == 12: left
# apriltag_id == 14: middle
# apriltag_id == 16: right
elif self.idx == 3:
if self.detection_id == 12:
self.bottle = 0
self.idx = self.idx + 1
elif self.detection_id == 14:
self.bottle = 1
self.idx = self.idx + 1
elif self.detection_id == 16:
self.bottle = 2
self.idx = self.idx + 1
# idx == 4, turn left and go to target
elif self.idx == 4:
if self.detection_id == 3:
print_id = 4002
if print_id != self.print_id:
self.print_id = print_id
print 'idx4 finished'
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
arrived = True
else:
print_id = 4001
if print_id != self.print_id:
self.print_id = print_id
print 'Turn left slowly'
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = -0.05
self.velcmd_pub.publish(wcv)
if arrived:
self.idx = self.idx + 1
arrived = False
arrived_position = False
rospy.sleep(1)
# 6 go to bottle
elif self.idx == 6:
print 'idx =', self.idx
if self.bottle == 0: #left
wcv.desiredWV_R = -0.3
wcv.desiredWV_L = -0.1
self.velcmd_pub.publish(wcv)
rospy.sleep(2)
wcv.desiredWV_R = 0.3
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1.8)
'''
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(1)
'''
wcv.desiredWV_R = 0.1
wcv.desiredWV_L = 0.1
self.velcmd_pub.publish(wcv)
rospy.sleep(3.8)
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
elif self.bottle == 1:
# middle
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(3)
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = 0.05
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(3.6)
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.bottle == 2: # right
wcv.desiredWV_R = 0.05
wcv.desiredWV_L = 0.05
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(6)
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
# left
#7 close gripper
elif self.idx == 7:
wcv.desiredGripperPos = girpperClose
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(2)
self.idx6cnt = 0
#8
elif self.idx == 8:
# retreat
print 'idx =', self.idx
if self.bottle == 0: #left
if self.route_idx == 0:
wcv.desiredWV_R = -0.3
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(2)
self.idx = self.idx + 1
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = -0.1
#self.velcmd_pub.publish(wcv)
rospy.sleep(2) # left
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.route_idx == 1:
wcv.desiredWV_R = -0.3
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
rospy.sleep(2)
self.idx = self.idx + 1
wcv.desiredWV_R = -0.1
wcv.desiredWV_L = -0.1
#self.velcmd_pub.publish(wcv)
rospy.sleep(1) # left
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.bottle == 1: #middle
wcv.desiredWV_R = -0.3
wcv.desiredWV_L = -0.1
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(5) #middle
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.bottle == 2: #right
wcv.desiredWV_R = -0.3
wcv.desiredWV_L = -0.1
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(4)
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.idx == 9:
# turn right
print 'idx =', self.idx
if self.bottle == 0: #left
if self.route_idx == 0:
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0.2
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(3) # left
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.route_idx == 1:
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0.2
self.velcmd_pub.publish(wcv)
rospy.sleep(3) # left
if self.detection_id != 4:
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0.05
self.velcmd_pub.publish(wcv)
else:
self.idx = self.idx + 1
elif self.bottle == 1: #middle
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0.2
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(5.5) # middle
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
elif self.bottle == 2: #right
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0.2
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(5)
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
'''
elif self.idx == 10:
# go straight
print 'idx =', self.idx
wcv.desiredWV_R = 0.3
wcv.desiredWV_L = 0.3
self.velcmd_pub.publish(wcv)
self.idx = self.idx + 1
rospy.sleep(4.5)
wcv.desiredWV_R = 0
wcv.desiredWV_L = 0
self.velcmd_pub.publish(wcv)
'''
rate.sleep()
