def __init__(self): # Debugging purposes self.print_velocities = rospy.get_param('print_velocities') # Where and when should you use this? self.stop_robot = False # Create the needed objects self.sonar_aggregation = SonarDataAggregator() self.laser_aggregation = LaserDataAggregator() self.navigation = Navigation() self.linear_velocity = 0 self.angular_velocity = 0 self.previous_turn_dir = None # Check if the robot moves with target or just wanders self.move_with_target = rospy.get_param("calculate_target") # The timer produces events for sending the speeds every 110 ms rospy.Timer(rospy.Duration(0.11), self.publishSpeeds) self.velocity_publisher = rospy.Publisher( rospy.get_param('speeds_pub_topic'), Twist, queue_size=10) # Obstacle Avoidance state self.obstacle_avoidance__active = False self.obstacle_avoidance__remaining_steps = 20
def __init__(self): # Debugging purposes self.print_velocities = rospy.get_param('print_velocities') # Where and when should you use this? self.stop_robot = False # Create the needed objects self.sonar_aggregation = SonarDataAggregator() self.laser_aggregation = LaserDataAggregator() self.navigation = Navigation() self.linear_velocity = 0 self.angular_velocity = 0 # Check if the robot moves with target or just wanders self.move_with_target = rospy.get_param("calculate_target") # The timer produces events for sending the speeds every 110 ms rospy.Timer(rospy.Duration(0.11), self.publishSpeeds) self.velocity_publisher = rospy.Publisher(\ rospy.get_param('speeds_pub_topic'), Twist,\ queue_size = 10) # Read the velocities architecture self.velocity_arch = rospy.get_param("velocities_architecture") print "The selected velocities architecture is " + self.velocity_arch
def __init__(self): self.print_velocities = rospy.get_param('print_velocities') # For Printing Velocity Calculated self.debug = rospy.get_param('debug') # For Debugging self.move_with_target = rospy.get_param("calculate_target") # Robot moves with target or just wanders self.initial_turn = rospy.get_param('initial_turn') # Perform a 360 turn when starting # Where and when should you use this? self.stop_robot = False self.sonar_on = False # Create the needed Objects self.laser_aggregation = LaserDataAggregator() self.navigation = Navigation() if self.sonar_on: self.sonar_aggregation = SonarDataAggregator() # Enable this if Robot has Sonar! # Speed Parameters for Turtlebot self.linear_velocity = 0 self.angular_velocity = 0 self.low_turn_limit = 4.2 self.max_linear_velocity = rospy.get_param('max_linear_speed') self.max_angular_velocity = rospy.get_param('max_angular_speed') # The Timer Produces Events for Sending Speeds Every 110 ms rospy.Timer(rospy.Duration(0.11), self.publishSpeeds) # The Turtlebot Speed Publisher Topic self.velocity_publisher = rospy.Publisher(rospy.get_param('turtlebot_speeds_topic'), Twist, queue_size = 10)
def __init__(self): # Initializations self.robot_perception = RobotPerception() self.path_planning = PathPlanning() # Check if the robot moves with target or just wanders self.move_with_target = rospy.get_param("calculate_target") # Flag to check if the vehicle has a target or not self.target_exists = False self.select_another_target = 0 self.inner_target_exists = False # Container for the current path self.path = [] # Container for the subgoals in the path self.subtargets = [] # Container for the next subtarget. Holds the index of the next subtarget self.next_subtarget = 0 self.count_limit = 200 # 20 sec self.counter_to_next_sub = self.count_limit self.laser_aggregation = LaserDataAggregator() # Check if subgoal is reached via a timer callback rospy.Timer(rospy.Duration(0.10), self.checkTarget) # Read the target function self.target_selector = rospy.get_param("target_selector") print "The selected target function is" + self.target_selector self.target_selection = TargetSelection(self.target_selector) # ROS Publisher for the path self.path_publisher = \ rospy.Publisher(rospy.get_param('path_pub_topic'), \ Path, queue_size = 10) # ROS Publisher for the subtargets self.subtargets_publisher = \ rospy.Publisher(rospy.get_param('subgoals_pub_topic'),\ MarkerArray, queue_size = 10) # ROS Publisher for the current target self.current_target_publisher = \ rospy.Publisher(rospy.get_param('curr_target_pub_topic'),\ Marker, queue_size = 10)
class RobotController: LINEAR_VELOCITY_MAX = 0.3 # in m/s ANGULAR_VELOCITY_MAX = 0.3 # in rad/s OBSTACLE_SEARCH_ANGLE_MIN = -60 # in degrees OBSTACLE_SEARCH_ANGLE_MAX = +60 # in degrees OBSTACLE_DISTANCE_IN_SIGHT = 1.0 # in m (when angular motors should start to activate) OBSTACLE_DISTANCE_MIN = 0.5 # in m # Constructor def __init__(self): # Debugging purposes self.print_velocities = rospy.get_param('print_velocities') # Where and when should you use this? self.stop_robot = False # Create the needed objects self.sonar_aggregation = SonarDataAggregator() self.laser_aggregation = LaserDataAggregator() self.navigation = Navigation() self.linear_velocity = 0 self.angular_velocity = 0 self.previous_turn_dir = None # Check if the robot moves with target or just wanders self.move_with_target = rospy.get_param("calculate_target") # The timer produces events for sending the speeds every 110 ms rospy.Timer(rospy.Duration(0.11), self.publishSpeeds) self.velocity_publisher = rospy.Publisher( rospy.get_param('speeds_pub_topic'), Twist, queue_size=10) # Obstacle Avoidance state self.obstacle_avoidance__active = False self.obstacle_avoidance__remaining_steps = 20 # This function publishes the speeds and moves the robot def publishSpeeds(self, event): # Produce speeds self.produceSpeeds() # Create the commands message twist = Twist() twist.linear.x = self.linear_velocity twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = self.angular_velocity # Send the command self.velocity_publisher.publish(twist) # Print the speeds for debugging purposes if self.print_velocities == True: print "[L,R] = [" + str(twist.linear.x) + " , " + \ str(twist.angular.z) + "]" # def turnDirection(self, from_angle_deg, to_angle_deg): # """ # Check laser scan measurements on the left vs on the right side of the robot and determine # the type of the rotation # :param from_angle_deg: left minimum angle in degrees # :param to_angle_deg: right maximum angle in degrees # :return: -1: right rotation | 1 : left rotation # """ # laser_scan_left = self.laser_aggregation.getScanForAngleDegRange(from_angle_deg, 0) # laser_scan_right = self.laser_aggregation.getScanForAngleDegRange(0, to_angle_deg) # return -1 \ # if (sum(laser_scan_left) - sum(laser_scan_right)) > 2 \ # and max(laser_scan_left) > max(laser_scan_right) else \ # 1 # Produces speeds from the laser def produceSpeedsLaser(self): """ :return: linear 0 to 1 and angular -1 to 1 """ # laser_scan = self.laser_aggregation.getScanForAngleDegRange(self.OBSTACLE_SEARCH_ANGLE_MIN, # self.OBSTACLE_SEARCH_ANGLE_MAX) # """laser_scan contains ~335 measurements from ~[-90, 90]° angles""" # laser_scan_min = min(laser_scan) ############################### NOTE QUESTION ############################ # Check what laser_scan contains and create linear and angular speeds # for obstacle avoidance angular = self.laser_aggregation.getNextAngularVelocity() linear = 1 - abs(angular) ########################################################################## return [linear, angular] # Combines the speeds into one output using a motor schema approach def produceSpeeds(self): # Produce target if not existent if self.move_with_target == True and \ self.navigation.target_exists == False: # Create the commands message twist = Twist() twist.linear.x = 0 twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = 0 # Send the command self.velocity_publisher.publish(twist) self.navigation.selectTarget() # Get the submodule's speeds [l_laser, a_laser] = self.produceSpeedsLaser() # You must fill these self.linear_velocity = 1 self.angular_velocity = 0 if self.move_with_target: [l_goal, a_goal] = self.navigation.velocitiesToNextSubtarget() ############################### NOTE QUESTION ############################ # You must combine the two sets of speeds. You can use motor schema, # subsumption of whatever suits your better. if l_laser == 0 or self.obstacle_avoidance__active: # Enter obstacle avoidance mode if not self.obstacle_avoidance__active: self.obstacle_avoidance__active = True # Hand-break must be done now # ---> Subsumes Path Following <--- self.linear_velocity = self.LINEAR_VELOCITY_MAX * l_laser self.angular_velocity = self.ANGULAR_VELOCITY_MAX * a_laser # Keep obstacle avoidance active (since initiated after hand-break) if self.obstacle_avoidance__remaining_steps == 0: self.obstacle_avoidance__active = False self.obstacle_avoidance__remaining_steps = 20 else: self.obstacle_avoidance__remaining_steps -= 1 else: self.linear_velocity = self.LINEAR_VELOCITY_MAX * l_goal self.angular_velocity = self.ANGULAR_VELOCITY_MAX * a_goal ########################################################################## else: ############################### NOTE QUESTION ############################ # Implement obstacle avoidance here using the laser speeds. # Hint: Subtract them from something constant self.linear_velocity = self.LINEAR_VELOCITY_MAX * l_laser self.angular_velocity = self.ANGULAR_VELOCITY_MAX * a_laser ########################################################################## # Assistive functions def stopRobot(self): self.stop_robot = True def resumeRobot(self): self.stop_robot = False