def ping(self, sensor_name: str, angle: float):
"""Return single sensor reading in a given direction"""
if sensor_name not in self.sensors:
error_msg = "ERROR - Ping failed. Sensor '{}' not found".format(sensor_name)
logger.error(error_msg)
return None
logger.debug("Ping sensor '{}' at angle {}°".format(sensor_name, angle))
return self.sensors[sensor_name].ping(angle)
def move(self, distance: float):
"""
Place the vehicle at the end of the segment of length 'distance'
along the direction defined by its orientation.
:Arguments:
:param dir: direction of the desired movement. Could take only two values forward and backward
:type dir: Vehicle constants FORWARD, FWD, BACKWARD, BACK
:param distance: distance at which the vehicle will stop. Negative values will be transformed in positive.
:type distance: float in length unit defined for the overall simulation.
"""
# logger.debug("before move: {}".format(self.__str__()))
logger.debug("move: {}".format(distance))
# Calculate the cartesian absolute coordinates of the destination point
abs_dist = np.abs(distance)
x_move = abs_dist * np.cos(self.orientation)
y_move = abs_dist * np.sin(self.orientation)
# Calculate the actual point
if distance < 0:
x_move = -x_move
y_move = -y_move
# Now traslate vehicle at that point
x_dest = self.position.x + x_move
y_dest = self.position.y + y_move
self.position = Point(x_dest, y_dest)
self._draw_vehicle_shape()
# Now reposition all onboard sensors
for sensor_id in self.sensors:
self.sensors[sensor_id].update_placement(self.position,
self.orientation)
# Save data path
self._save_datapath()
# Perform light tracing if required
if self.tracing is True:
self.light_plot()
logger.debug("after move: {}".format(self.__str__()))
def turn(self, angle: float):
"""
Turn the vehicle.
After turn all sensors mounted on the vehicle will have their position
and orientation updated accordingly with the new vehicle orientation.
Parameters
----------
angle : float
Rotation angle in degrees units.
If angle > 0 turn direction LEFT
Otherwise turn direction RIGHT
Return
------
None
"""
# logger.debug("before_turn: {}".format(self.__str__()))
logger.debug("turn: {}°".format(angle))
# Update chassis orientation and orient its shape
self.orientation = self.orientation + np.deg2rad(angle)
self._draw_vehicle_shape()
# Update sensor orientation
for sensor_id in self.sensors:
self.sensors[sensor_id].update_placement(self.position,
self.orientation)
# Save data path
self._save_datapath()
# Perform light tracing if required
if self.tracing is True:
self.light_plot()
# Trace vehicle pose and orientation
logger.debug("after turn: {}".format(self.__str__()))
def scan(self,
sensor_name: str,
angle_from: float = -90,
angle_to: float = 90,
angle_step: float = 1):
"""
Perform a sensor scan of the virtual environment loaded.
It is possible to scan all sensors or a single sensor by name.
The same angle ranges will be applaied to all sensors.
A dictionry of readings is returned with key the name of the sensor
Parameters
----------
sensor_name: str
Name of the sensoe to read
angle_from, angle_to, angle_step : float
See description of Sensor.scan() method
Return
------
{} empty dict in error case
{"sensor1": [(rho1, phi1), (rho2, phi2), ...], ...}
"""
scan_data = dict()
if sensor_name == "all":
logger.debug("Scanning 'all' sensors")
for s_name in self.sensors:
logger.debug("---Scanning sensor '{}'".format(s_name))
scan_data[s_name] = self.sensors[s_name].scan(
angle_from, angle_to, angle_step)
elif sensor_name in self.sensors:
logger.debug("Scan sensor '{}'".format(sensor_name))
scan_data[sensor_name] = self.sensors[sensor_name].scan(
angle_from, angle_to, angle_step)
else:
error_msg = "ERROR - Scan failed. Sensor '{}' not found".format(
sensor_name)
logger.error(error_msg)
return scan_data
# File: flatland.py
# Date: 21-09-2019
# Author: Saruccio Culmone
#
"""
Description
A FlatLand is an ensamble of compond objects, shapes and one or more
sensors that can be used to measure the distance among objects.
Sensors can be moved and rotated into the land acquiring _dynamic_ range
measures that can be used to compose a map or to test a navigation algorithm
"""
from trace import logger
logger.debug("FlatLand")
import geom_utils as geom
class FlatLand():
"""
A FlatLand is an ensamble of CompoundShape objects, Shapes and
one or more Sensors that che cam be used to measure the distancea
among objects.
All objects are referred to the same _global_ coordinate system: the one
of the virtulal plane.
"""
def __init__(self, name: str = ""):
self.name = name
self.venv = [] # The virtual environment is a list of compounds
self.sensors = dict() # Sensor dictionary