def new_goal(self):
goal = self.moveto_as.accept_new_goal()
self.desired_position = tools.position_from_posetwist(goal.posetwist)
self.desired_orientation = tools.orientation_from_posetwist(goal.posetwist)
#self.linear_tolerance = goal.linear_tolerance
#self.angular_tolerance = goal.angular_tolerance
rospy.loginfo('Desired position:' + str(self.desired_position))
rospy.loginfo('Current position:' + str(self.current_position))
rospy.loginfo('Desired orientation:' + str(self.desired_orientation))
rospy.loginfo('Current orientation:' + str(self.current_orientation))
rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance))
rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance))
# This controller doesn't handle desired twist
#self.desired_twist = goal.posetwist.twist
if (xyz_array(goal.posetwist.twist.linear).any() or
xyz_array(goal.posetwist.twist.angular).any() ):
rospy.logwarn('None zero are not handled by the tank steer trajectory generator. Setting twist to 0')
if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius:
self.line = line(self.current_position, self.desired_position)
self.redraw_line = True
else:
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.current_position)
self.redraw_line = False
def new_goal(self):
goal = self.moveto_as.accept_new_goal()
self.desired_position = tools.position_from_posetwist(goal.posetwist)
self.desired_orientation = tools.orientation_from_posetwist(
goal.posetwist)
#self.linear_tolerance = goal.linear_tolerance
#self.angular_tolerance = goal.angular_tolerance
rospy.loginfo('Desired position:' + str(self.desired_position))
rospy.loginfo('Current position:' + str(self.current_position))
rospy.loginfo('Desired orientation:' + str(self.desired_orientation))
rospy.loginfo('Current orientation:' + str(self.current_orientation))
rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance))
rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance))
# This controller doesn't handle desired twist
#self.desired_twist = goal.posetwist.twist
if (xyz_array(goal.posetwist.twist.linear).any()
or xyz_array(goal.posetwist.twist.angular).any()):
rospy.logwarn(
'None zero are not handled by the tank steer trajectory generator. Setting twist to 0'
)
if np.linalg.norm(self.current_position -
self.desired_position) > self.orientation_radius:
self.line = line(self.current_position, self.desired_position)
self.redraw_line = True
else:
self.line = line(
self.current_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.current_position)
self.redraw_line = False
def new_goal(self):
# Lock on odom cb
#print 'New goal: wait for lock'
self.as_lock.acquire()
#print 'New goal lock'
goal = self.moveto_as.accept_new_goal()
pt = goal.posetwist
self.desired_position = tools.position_from_posetwist(pt)
self.desired_orientation = tools.orientation_from_posetwist(pt)
#self.linear_tolerance = goal.linear_tolerance
#self.angular_tolerance = goal.angular_tolerance
rospy.logdebug('Desired position:' + str(self.desired_position))
rospy.logdebug('Current position:' + str(self.current_position))
rospy.logdebug('Desired orientation:' + str(self.desired_orientation * 180 / np.pi))
rospy.logdebug('Current orientation:' + str(self.current_orientation * 180 / np.pi))
rospy.logdebug('linear_tolerance:' + str(self.linear_tolerance))
rospy.logdebug('angular_tolerance:' + str(self.angular_tolerance))
# Pass the new goal to the trajectory generator
if self.traj_gen is not None:
self.traj_gen.new_goal(pt)
# Release lock
self.as_lock.release()
def new_goal(self, goal):
self.desired_position = tools.position_from_posetwist(goal)
self.desired_orientation = tools.orientation_from_posetwist(goal)
# Twist not supported
if (xyz_array(goal.twist.linear).any() or
xyz_array(goal.twist.angular).any() ):
rospy.logwarn('None zero twist are not handled by the azi goal_pp trajectory generator. Setting twist to 0')
def new_goal(self, goal):
self.desired_position = tools.position_from_posetwist(goal)
self.desired_orientation = tools.orientation_from_posetwist(goal)
# Twist not supported
if (xyz_array(goal.twist.linear).any() or
xyz_array(goal.twist.angular).any() ):
rospy.logwarn('None zero twist are not handled by the azi a_star_rpp trajectory generator. Setting twist to 0')
def start(self, current_pt):
# Set current position and twist
self.current_position = tools.position_from_posetwist(current_pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(current_pt)
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
def start(self, current_pt):
# Set current position and twist
self.current_position = tools.position_from_posetwist(current_pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(current_pt)
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
# Get distance to the goal
if np.array_equal(self.current_position, self.desired_position):
self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
else:
self.line = line(self.current_position, self.desired_position)
self.distance_to_goal = np.linalg.norm(self.line.s)
def start(self, current_pt):
# Set current position and twist
self.current_position = tools.position_from_posetwist(current_pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(current_pt)
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
self.redraw_line = False
def start(self, current_pt):
# Set current position and twist
self.current_position = tools.position_from_posetwist(current_pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(current_pt)
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
self.redraw_line = False
def new_goal(self, goal):
self.desired_position = tools.position_from_posetwist(goal)
self.desired_orientation = tools.orientation_from_posetwist(goal)
# Twist not supported
if (xyz_array(goal.twist.linear).any() or
xyz_array(goal.twist.angular).any() ):
rospy.logwarn('None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0')
if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius:
#print 'Far out dude'
self.line = line(self.current_position, self.desired_position)
else:
#print 'Pretty close'
self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
# Get distance to the goal
if np.array_equal(self.current_position, self.desired_position):
self.line = line(
self.desired_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.desired_position)
else:
self.line = line(self.current_position, self.desired_position)
self.distance_to_goal = np.linalg.norm(self.line.s)
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
# Get distance to the goal
vector_to_goal = self.desired_position - self.current_position
self.distance_to_goal = np.linalg.norm(vector_to_goal)
# overshoot is 1 if behind line drawn perpendicular to the goal line and through the desired position, -1 if on the other
# Side of said line
overshoot = np.dot(vector_to_goal / self.distance_to_goal, self.line.hat)
self.overshoot = overshoot / abs(overshoot)
# In the case that overshoot is 0 or otherwise fails default to not overshoot
if math.isnan(self.overshoot):
self.overshoot = 1
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
# Get distance to the goal
vector_to_goal = self.desired_position - self.current_position
self.distance_to_goal = np.linalg.norm(vector_to_goal)
# overshoot is 1 if behind line drawn perpendicular to the goal line and through the desired position, -1 if on the other
# Side of said line
overshoot = np.dot(vector_to_goal / self.distance_to_goal,
self.line.hat)
self.overshoot = overshoot / abs(overshoot)
# In the case that overshoot is 0 or otherwise fails default to not overshoot
if math.isnan(self.overshoot):
self.overshoot = 1
def new_goal(self, goal):
self.desired_position = tools.position_from_posetwist(goal)
self.desired_orientation = tools.orientation_from_posetwist(goal)
# Twist not supported
if (xyz_array(goal.twist.linear).any()
or xyz_array(goal.twist.angular).any()):
rospy.logwarn(
'None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0'
)
if np.linalg.norm(self.current_position -
self.desired_position) > self.orientation_radius:
#print 'Far out dude'
self.line = line(self.current_position, self.desired_position)
else:
#print 'Pretty close'
self.line = line(
self.desired_position,
tools.normal_vector_from_rotvec(self.desired_orientation) +
self.desired_position)
def feedback(self, pt):
# Update current pt
self.current_position = tools.position_from_posetwist(pt)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_posetwist(pt)
