def diff_get_error_angle(self):
'''
Input:
self.Vy
self.Wz
self.theta
Return ( (ref_ang, error_theta) , (ref_ang, error_theta) )
^ leader ^ follower
'''
# Get refenrence angle
R = self.get_radius_of_rotation(self.Vx, self.Wz)
ref_ang_L = atan2(TOW_CAR_LENGTH / 2.0, abs(R))
if not is_same_sign(R, self.Vx):
ref_ang_L *= -1
# Follower reverse ref_ang
ref_ang_F = normalize_angle(pi - ref_ang_L)
# Get error theta
error_theta_L = normalize_angle(ref_ang_L - self.theta_L)
error_theta_F = normalize_angle(ref_ang_F - self.theta_F)
# In-place rotation, Find the nearest 90 degree to ref.
if abs(R) < INPLACE_ROTATION_R and\
abs(error_theta_L) > pi/2 and\
abs(error_theta_F) > pi/2:
# ref_ang_L and ref_ang_F must have same sign
ref_ang_L *= -1
ref_ang_F *= -1
error_theta_L = normalize_angle(ref_ang_L - self.theta_L)
error_theta_F = normalize_angle(ref_ang_F - self.theta_F)
return ((ref_ang_L, error_theta_L), (ref_ang_F, error_theta_F))
def car2_feedback_cb(self, data):
'''
'''
quaternion = (
data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w)
euler = transformations.euler_from_quaternion(quaternion)
yaw = atan2(data.pose.position.y, data.pose.position.x)
if data.pose.position.x != L * cos(yaw) or data.pose.position.y != L * sin(yaw):
data.pose.position.x = L * cos(yaw)
data.pose.position.y = L * sin(yaw)
self.x = L * cos(yaw)
self.y = L * sin(yaw)
self.theta = normalize_angle (euler[2])
SERVER.setPose( data.marker_name, data.pose )
else:
self.x = data.pose.position.x
self.y = data.pose.position.y
self.theta = normalize_angle (euler[2])
# self.update_markers()
SERVER.applyChanges()
def publish_theta(self):
# Get theta1 or theta2
if ROLE == "leader": # Theta1
self.theta = normalize_angle(-self.shelf_finder_base.center[2])
elif ROLE == "follower": # Theta2
self.theta = normalize_angle(-self.shelf_finder_base.center[2] +
pi)
self.pub_theta.publish(self.theta)
# Send tf
if ROLE == "leader":
send_tf((TOW_CAR_LENGTH / 2.0, 0, self.theta), "carB/base_link",
"car1/base_link")
def crab_get_error_angle(self):
'''
Input:
self.Vx
self.Vy
self.theta
Return ( (ref_ang, error_theta) , (ref_ang, error_theta) , T/F)
^ leader ^ follower
'''
# Get refenrence angle
ref_ang_L = atan2(self.Vy, self.Vx)
ref_ang_F = normalize_angle(ref_ang_L + pi)
# Get error angle
error_theta_L = normalize_angle(ref_ang_L - self.theta_L)
error_theta_F = normalize_angle(ref_ang_F - self.theta_F)
# Go backward or forward?
is_forward = True
if abs(error_theta_L) > pi/2 and\
abs(error_theta_F) > pi/2 and\
(not(self.Vx == 0 and self.Vy == 0)):
is_forward = False
ref_ang_L = normalize_angle(ref_ang_L + pi)
ref_ang_F = normalize_angle(ref_ang_F + pi)
error_theta_L = normalize_angle(ref_ang_L - self.theta_L)
error_theta_F = normalize_angle(ref_ang_F - self.theta_F)
return ((ref_ang_L, error_theta_L), (ref_ang_F, error_theta_F),
is_forward)
def cal_FK_passing_paramters(self, x,y,theta,v,w):
'''
output (x_p,y_p,theta_p,x_c,y_c)
'''
try:
r = v/w # divide by zero
except ZeroDivisionError:
# w = INF_SMALL
r = float('inf')
# --- rotation center ----#
x_c = x - r*sin(theta)
y_c = y + r*cos(theta)
# --- result -----#
x_p = x_c + r*sin(theta + w*DT)
y_p = y_c - r*cos(theta + w*DT)
theta_p = normalize_angle (theta + w*DT)
return (x_p,y_p,theta_p,x_c,y_c,r)
def cal_IK(self):
'''
calculate inverse kinematics
This should be only called by car_1, car_2
Input :
self.x
self.y
self.theta
self.x_p
self.y_p
Output:
self.theta_p
self.v
self.w
self.r
'''
A = 2*(self.x_p - self.x)
B = 2*(self.y_p - self.y)
C = self.x_p**2 + self.y_p**2 - self.x**2 - self.y**2
D = cos(self.theta)
E = sin(self.theta)
F = cos(self.theta) * self.x + sin(self.theta) * self.y
LHS = np.array([[A,B],[D,E]])
RHS = np.array([C,F])
try:
(self.x_c, self.y_c) = np.linalg.solve(LHS,RHS)
except np.linalg.linalg.LinAlgError:
print ("NO SOLUTION!!!!!!!!!!!!!!!!!")
self.r = sqrt( (self.x - self.x_c)**2 + (self.y - self.y_c)**2 )
self.theta_p = normalize_angle(atan2(self.x_c - self.x_p , self.y_p - self.y_c))
#---- theta_p has a constraint that direction must be the same as the votex -----#
v_s = np.cross( [ self.x - self.x_c , self.y - self.y_c ,0 ] , [ cos(self.theta) , sin(self.theta),0] )
v_e = np.cross( [ self.x_p - self.x_c , self.y_p - self.y_c,0] , [ cos(self.theta_p) , sin(self.theta_p),0] )
if v_s[2] * v_e [2] < 0: # different sign
self.theta_p = normalize_angle(self.theta_p + pi) # Switch the direction
# Judge center of rotation is at RHS or LHS
# if self.name == "start":
dtheta = self.theta_p - self.theta
if v_s[2] > 0 :
self.dir_center = "LHS"
# LHS
if dtheta > 0.0:
# print ("Forward")
self.v = 1.0
else:
# print("backward")
self.v = -1.0
else:
self.dir_center = "RHS"
# RHS
if dtheta < 0.0:
# print ("Forward")
self.v = 1.0
else:
# print("backward")
self.v = -1.0
#---- Go forward of Go backward ? ----#
self.w = (self.theta_p - self.theta) / DT
dtheta = self.theta_p - self.theta
if dtheta > pi :
self.w = (self.theta_p - self.theta - 2*pi) / DT
elif dtheta < -pi :
self.w = (self.theta_p - self.theta + 2*pi) / DT
if self.w != 0:
self.v = abs(self.w * self.r) * sign(self.v)
else:
self.v = (self.x_p - self.x) / DT
def run_once(self):
# Check simple goal is already reached
if self.simple_goal == None:
return False
# Update tf # TODO use odom_fuse result
t_big_car = get_tf(self.tfBuffer, MAP_FRAME, BIG_CAR_FRAME)
if t_big_car != None:
self.big_car_xyt = t_big_car
if self.big_car_xyt == None: #tf is invalid
time.sleep(1)
return False
# Get Local goal
self.prune_global_path()
local_goal = self.get_local_goal()
if local_goal == None:
rospy.logdebug("[rap_planner] Can't get local goal from global path.")
return False
# transform local goal to /base_link frame
p_dif = (local_goal[0] - self.big_car_xyt[0],
local_goal[1] - self.big_car_xyt[1])
t = self.big_car_xyt[2]
x_goal = cos(t)*p_dif[0] + sin(t)*p_dif[1]
y_goal =-sin(t)*p_dif[0] + cos(t)*p_dif[1]
self.rho = sqrt((self.simple_goal[0] - self.big_car_xyt[0])**2 + \
(self.simple_goal[1] - self.big_car_xyt[1])**2)
# Check xy_goal reached
if (not self.latch_xy) and self.rho < GOAL_TOLERANCE_XY:
rospy.loginfo("[rap_planner] Goal xy Reached")
if IGNORE_HEADING:
self.reset_plan()
return True
else:
self.latch_xy = True
# Check goal_heading reached
if self.latch_xy:
if abs(normalize_angle(self.simple_goal[2] - self.big_car_xyt[2])) <\
(GOAL_TOLERANCE_T/2.0):
self.reset_plan()
rospy.loginfo("[rap_planner] Goal Heading Reached")
return True
# Get alpha
alpha = atan2(y_goal, x_goal)
# Get beta
'''
# This is Benson's legacy, but it's not very helpful
if (not IGNORE_HEADING) and local_goal[2] != None:
beta = normalize_angle(local_goal[2] - alpha - self.big_car_xyt[2])
if abs(alpha) > pi/2:# Go backward
beta = normalize_angle(beta - pi)
else:
beta = 0
'''
pursu_angle = alpha
self.alpha = alpha
# self.beta = beta
# self.angle = pursu_angle
self.viz_marker.update_marker("local_goal", (x_goal, y_goal) )
# p = (cos(alpha)*LOOK_AHEAD_DIST, sin(alpha)*LOOK_AHEAD_DIST)
# self.viz_marker.update_marker("goal_head", ((0,0), p))
##################
### Get Flags ###
##################
# Check where is the goal
is_aside_goal = False
if abs(abs(alpha) - pi/2) < (ASIDE_GOAL_ANG/2.0):
is_aside_goal = True
# Check is need to consider heading
d_head = normalize_angle(self.simple_goal[2] - self.big_car_xyt[2])
need_consider_heading = False
if (not IGNORE_HEADING) and local_goal[2] != None:
need_consider_heading = True
# Check need to switch to rota
is_need_rota = False # current rota only when heading adjment
if self.latch_xy: #or\
#(USE_CRAB_FOR_HEADING and\
#need_consider_heading and\
#abs(d_head) > (GOAL_TOLERANCE_T/2.0)): # TODO # ROTA, cause occlication
# need to adjust heading
is_need_rota = True
############################
### Finite State Machine ###
############################
# Flags:
# States: crab, diff, rota, tran
if self.mode == "crab":
if self.latch_xy and (not IGNORE_HEADING):
self.set_tran_mode("rota")
else:
if need_consider_heading:
if is_need_rota:
if USE_CRAB_FOR_HEADING:
self.set_tran_mode("rota")
else:
self.set_tran_mode("diff")
else:
pass # Stay crab
else:
if is_aside_goal:
pass # Stay crab
else:
# Transit to diff mode
self.set_tran_mode("diff")
elif self.mode == "diff":
if self.latch_xy and (not IGNORE_HEADING):
self.set_tran_mode("rota")
else:
if is_aside_goal:
self.set_tran_mode("crab")
else:
pass
'''
if need_consider_heading:
if USE_CRAB_FOR_HEADING:
# Current diff can't heading adj
# self.set_tran_mode("rota")
pass
else:
pass # Stay here
else:
if is_aside_goal:
self.set_tran_mode("crab")
else:
pass
'''
elif self.mode == "rota":
if need_consider_heading:
if is_need_rota:
pass# Stay rota
else: # Finish rota
if USE_CRAB_FOR_HEADING:
self.set_tran_mode("crab") # Go to goal
else:
self.set_tran_mode("diff") # Go to goal
else:
self.set_tran_mode("crab") # Leave heading adj
elif self.mode == "tran":
if (not RAP_CTL.is_transit):
rospy.loginfo("[rap_planner] transit finish, switch to " + self.next_mode)
self.mode_latch_counter = MODE_SWITCH_LATCH
self.mode = self.next_mode
else:
rospy.logerr("[rap_planner] Invalid mode " + str(self.mode))
####################
### Execute mode ###
####################
if self.mode == "crab" or (self.mode == "tran" and self.next_mode == "crab"):
self.vx_out = cos(alpha) * CRAB_KP_VEL
self.vy_out = sin(alpha) * CRAB_KP_VEL
self.wz_out = 0.0
elif self.mode == "rota" or (self.mode == "tran" and self.next_mode == "rota"):
self.vx_out = 0.0
self.vy_out = 0.0
self.wz_out =sign(d_head) * ROTA_KP_VEL
elif self.mode == "diff" or (self.mode == "tran" and self.next_mode == "diff"):
# Get R
if self.rho < LOOK_AHEAD_DIST:
R = sqrt((tan(pi/2 - (pursu_angle))*self.rho/2)**2 +
(self.rho/2.0)**2 )
else:
R = sqrt((tan(pi/2 - (pursu_angle))*LOOK_AHEAD_DIST/2)**2 +
(LOOK_AHEAD_DIST/2.0)**2 )
if pursu_angle < 0: # alpha = [0,-pi]
R = -R
self.vx_out = sqrt(x_goal**2 + y_goal**2) * DIFF_KP_VEL
self.vy_out = 0.0
# TODO test this , to avoid near-goal weird things
if self.rho < LOOK_AHEAD_DIST:
self.wz_out = (self.vx_out / R) * (self.rho/LOOK_AHEAD_DIST)
else:
self.wz_out = (self.vx_out / R)
if abs(pursu_angle) > pi/2: # Go backward
self.vx_out *= -1.0
else:
rospy.logerr("[rap_planner] Invalid mode " + str(self.mode))
# Debug marker text
if self.mode == "tran":
text = str(self.previous_mode) + "->" + self.next_mode
else:
text = self.mode
self.viz_marker.update_marker("mode_text", (0,-0.5), text)
# Latch counts
if self.mode_latch_counter > 0:
self.mode_latch_counter -= 1
return True