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