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 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 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 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)