def update_status(self): """Sets up the status message to be published""" msg = robot_status() msg.robot_id = self.robot_id msg.robot_type = self.type msg.x = self.position["x"] msg.y = self.position["y"] msg.theta = self.position["theta"] if len(self._action_queue) > 0: msg.current_action = type(self._action_queue[len(self._action_queue)-1]).__name__ # is there a better way to do this? msg.is_blocked = self.is_blocked() self.status_msg = msg
def update_status(self): """Sets up the status message to be published""" msg = robot_status() msg.robot_id = self.robot_id msg.robot_type = self.type msg.x = self.position["x"] msg.y = self.position["y"] msg.theta = self.position["theta"] if len(self._action_queue) > 0: msg.current_action = type(self._action_queue[ len(self._action_queue) - 1]).__name__ # is there a better way to do this? msg.is_blocked = self.is_blocked() self.status_msg = msg
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): """ robot_id: The robot's name in stage top_speed: The robot's maximum speed x_offset: The x coordinate the robot starts at in stage y_offset: The y coordinate the robot starts at in stage theta_offset: The direction this robot starts facing in stage """ super(Robot, self).__init__() self.robot_id = robot_id self.type = type(self).__name__ self.top_speed = top_speed self.current_speed = self.top_speed self.angular_top_speed = angular_top_speed self.x_offset = x_offset self.y_offset = y_offset self.theta_offset = theta_offset self.odometry = None self.velocity = Twist() self.is_moving = False self.leftLaser = None self.rightLaser = None self._action_queue = [] self.rotation_executing = False self.current_rotation = None self.slave = None self.curr_robot_messages = [None ] * 10 # max ten robots before it breaks def status_handler(data): """Deal with the other robot statuses, stores in an list for use later""" robot_id = data.robot_id rid = int(robot_id[-1:]) self.curr_robot_messages[rid] = data rospy.Subscriber("statuses", robot_status, status_handler) self.status_msg = robot_status() def odometry_handler(data): """ Handles odometry messages from stage """ self.odometry = data rospy.Subscriber("/" + self.robot_id + "/odom", Odometry, odometry_handler) def left_scan_handler(data): """ Handles LaserScan messages from stage for left sensor """ self.leftLaser = data def right_scan_handler(data): """ Handles LaserScan messages from stage for right sensor """ self.rightLaser = data rospy.Subscriber("/" + self.robot_id + "/base_scan_0", LaserScan, left_scan_handler) rospy.Subscriber("/" + self.robot_id + "/base_scan_1", LaserScan, right_scan_handler) # Wait for odometry data while self.odometry is None: # rospy.loginfo("Waiting for odometry information") pass self.position = self.get_position()
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): """ robot_id: The robot's name in stage top_speed: The robot's maximum speed x_offset: The x coordinate the robot starts at in stage y_offset: The y coordinate the robot starts at in stage theta_offset: The direction this robot starts facing in stage """ super(Robot, self).__init__() self.robot_id = robot_id self.type = type(self).__name__ self.top_speed = top_speed self.current_speed = self.top_speed self.angular_top_speed = angular_top_speed self.x_offset = x_offset self.y_offset = y_offset self.theta_offset = theta_offset self.odometry = None self.velocity = Twist() self.is_moving = False self.leftLaser = None self.rightLaser = None self._action_queue = [] self.rotation_executing = False self.current_rotation = None self.slave = None self.curr_robot_messages = [None] * 10 # max ten robots before it breaks def status_handler(data): """Deal with the other robot statuses, stores in an list for use later""" robot_id = data.robot_id rid = int(robot_id[-1:]) self.curr_robot_messages[rid] = data rospy.Subscriber("statuses", robot_status, status_handler) self.status_msg = robot_status() def odometry_handler(data): """ Handles odometry messages from stage """ self.odometry = data rospy.Subscriber("/" + self.robot_id + "/odom", Odometry, odometry_handler) def left_scan_handler(data): """ Handles LaserScan messages from stage for left sensor """ self.leftLaser = data def right_scan_handler(data): """ Handles LaserScan messages from stage for right sensor """ self.rightLaser = data rospy.Subscriber("/" + self.robot_id + "/base_scan_0", LaserScan, left_scan_handler) rospy.Subscriber("/" + self.robot_id + "/base_scan_1", LaserScan, right_scan_handler) # Wait for odometry data while self.odometry is None: # rospy.loginfo("Waiting for odometry information") pass self.position = self.get_position()