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