def do_long_range_message(config, robot1, robot2, motion1, motion2):
"""Potentially transfer long range messages between robots.
Depending on whether the robots are within thresh distance, they will
exchange long range measurements. The long range measurements are sent in
the form of LongRangeMeasurement objects.
Args:
config: Configuration file.
robot1: First Robot object.
robot2: Second Robot object.
motion1: First RobotMotion object.
motion2: Second RobotMotion object.
"""
# If within long range, exchange long range sensor measurements.
if dist(motion1.pos, motion2.pos) < config['long_thresh']:
# Get message data from each robot.
message1to2_data = robot1.get_long_range_message()
message2to1_data = robot2.get_long_range_message()
# Create the sensor model, which will be used to make all measurements.
sm = SensorModel(config)
# Compute the pairwise sensor measurements between each robot.
num_sensors = len(config['sensor_parameters'])
robot1_measurements = [
sm.get_measurement(motion1, i, motion2) for i in range(num_sensors)
]
robot2_measurements = [
sm.get_measurement(motion2, i, motion1) for i in range(num_sensors)
]
logging.debug("Robot 1 Measurements: %s", robot1_measurements)
logging.debug("Robot 2 Measurements: %s", robot2_measurements)
# Stuff the data into the LongRangeMessages
message1to2 = LongRangeMessage(message1to2_data, robot2_measurements)
message2to1 = LongRangeMessage(message2to1_data, robot1_measurements)
# And then transmit both of the messages.
# Do it this way to ensure that receiving a message doesn't
# modify some state inside the robot.
robot1.receive_long_range_message(message2to1)
robot2.receive_long_range_message(message1to2)
# indicate whether these two robots communicated
return True
return False
{
'delta': [1, 0],
'orientation': 0,
'fov': 180,
},
{
'delta': [-1, 0],
'orientation': 180,
'fov': 180,
},
]
}
sm = SensorModel(config)
config1 = {
'start': [0, 0],
'sigma_initial': [[0.1, 0],
[0, 0.1]],
'id': 1
}
motion1 = RobotMotion(config1)
motion1.pos = np.array([0, 0])
motion1.th = math.radians(0)
motion2 = RobotMotion(config1)
motion2.pos = np.array([10, 0])
print(sm.get_measurement(motion1, 0, motion2))
print()
print(sm.get_measurement(motion1, 1, motion2))