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]
def depart_vector(self, start, unused_end, current): # axis direction axis_direction = quaternion_to_heading(start.pose.pose.orientation) # correction to move away from axis errors = calc_errors(current, start) off_axis = errors[1] heading_correction = math.atan(1.5*off_axis) final_direction = axis_direction+heading_correction return (math.cos(final_direction), math.sin(final_direction), 0,)
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,)