def arrive_vector(self, unused_start, end, current):
# axis direction
axis_direction = minimize_angle(quaternion_to_heading(end.pose.pose.orientation))
# correction to move away from axis
errors = calc_errors(current, end)
off_axis = errors[1]
heading_correction = minimize_angle(math.atan(-1.5*off_axis))
final_direction = minimize_angle(axis_direction+heading_correction)
# rospy.loginfo('avr: axis '+str(axis_direction)[0:4]+' corr '+str(heading_correction)[0:4]+' off '+str(off_axis)[0:5]+' fina '+str(final_direction)[0:5])
return (math.cos(final_direction), math.sin(final_direction), 0,)
def heading_error(self, location, goal):
"""
return difference in heading between location and goal
"""
loc_head = quaternion_to_heading(location.pose.pose.orientation)
goal_head = quaternion_to_heading(goal.pose.pose.orientation)
return minimize_angle(loc_head - goal_head)
def common_path_gen(self, start, end):
"""
generate a new path forwards
"""
# don't destroy previous work
# self.targets = []
self.targets.append(start)
# pylint: disable=invalid-name
# dt, v, w, are accurately describing what I want in this case
dt = .3
next_ = start
errors = calc_errors(next_, end)
along = errors[0]
count = 2
while along < -0.01 and not rospy.is_shutdown():
if count > 500:
rospy.loginfo('path overflow')
break
current = StateModel(next_)
force_vector = self.get_force_vector(start, end, next_)
force_heading = math.atan2(force_vector[1], force_vector[0])
# rospy.loginfo('fhead: '+str(force_heading)[0:5])
heading_err = minimize_angle(force_heading - current.theta)
# rospy.loginfo('headerr: '+str(heading_err)[0:5])
# pylint: disable=invalid-name
# v, w, are accurately describing what I want in this case
w = heading_err/dt*0.75
v = start.twist.twist.linear.x
# TODO(buckbaskin): calculate something smart based on vel
# profile from start to end
# possibly do vel profile based on distance, slow down proportional to
# heading error relative to force field
# rospy.loginfo('gnxt: '+str(force_heading)+' ... '+str(current.theta))
count += 1
next_ = current.sample_motion_model2(v, w, dt)
# rospy.loginfo('leader req vel: '+str(next_.twist.twist.linear.x))
# rospy.loginfo('lead vel: '+str(next_.twist.twist.linear.x))
# odom_next = self.convert_to_odom(next_)
self.targets.append(next_)
errors = calc_errors(next_, end)
along = errors[0]