Skip to content

docs(sensor-models): update and review doc/3_sensor_models #74

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
shivangi306 wants to merge 1 commit into
base: main
Choose a base branch
from
+219 −899
Open

docs(sensor-models): update and review doc/3_sensor_models #74

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
241 changes: 41 additions & 200 deletions doc/3_sensor_models/3_1_obstacle.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,233 +1,74 @@
## 3.1 Obstacle
In this section, Obstacle class and Landmark class are implemented. These class are used for locating the obstacles object in the world. The located obstacles can be observed by an sensor model.
This section explains the obstacle models used by the sensor simulations. The repository provides two modules:

### 3.1.1 Rectangle obstacle
The obstacle class is implemented as follow. [State class implemented in Chapter 2](/doc/2_vehicle_model/2_vehicle_model.md) is given to the constructor for computing the position and pose of the obstacle. And then, [XYArray class](/doc/2_vehicle_model/2_vehicle_model.md) is also imported in the class for representing the shape as rectangle.

[obstacle.py](/src/components/obstacle/obstacle.py)
```python
"""
obstacle.py

Author: Shisato Yano
"""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
from xy_array import XYArray
- [obstacle.py](/src/components/obstacle/obstacle.py): Represents one obstacle (static or dynamic).
- [obstacle_list.py](/src/components/obstacle/obstacle_list.py): Manages multiple obstacles.

### 3.1.1 Rectangle Obstacle
The `Obstacle` class stores its own state and geometric shape. The constructor takes a [State class](/doc/2_vehicle_model/2_vehicle_model.md) instance and shape/motion parameters.

```python
class Obstacle:
"""
Obstacle's data and logic class
"""

def __init__(self, state, accel_mps2=0.0, yaw_rate_rps=0.0,
length_m=2.0, width_m=2.0):
"""
Constructor
state: Obstacle's state object
accel_mps2: acceleration input to move[m/s2]
yaw_rate_rps: yaw rate input to move[rad/s]
length_m: Obstacle's longitudinal size[m]
width_m: Obstacle's half of lateral size[m]
"""

self.state = state
self.accel_mps2 = accel_mps2
self.yaw_rate_rps = yaw_rate_rps

contour = np.array([[length_m, -length_m, -length_m, length_m, length_m],
[width_m, width_m, -width_m, -width_m, width_m]])
self.array = XYArray(contour)
self.length_m = length_m
self.width_m = width_m
```

The given arguments, accel_mps2 and yaw_rate_rps are used for making the obstacle moved as a dynamic obstacle. If these values are default 0.0, the obstacle is represented as a static obstacle. Additionally, length_m and width_m are used for setting the size of the obstacle.
Notes:

The member methods, update, draw and vertex_xy are implemented. The method, update is used to compute the state including position(x, y) and yaw angle of the obstacle.
```python
def update(self, time_s):
"""
Function to update obstacle's state
time_s: Time interval value[sec]
"""

self.state.update(self.accel_mps2, self.yaw_rate_rps, time_s)
```
- `accel_mps2` and `yaw_rate_rps` control obstacle motion.
- When both are `0.0`, the obstacle behaves as a static object.
- `width_m` is treated as half-width in the class contour definition.

The method, draw is used to visualize the obstacle in the world. The vertexes and lines of obstacle are transformed based on the state and visualized.
```python
def draw(self, axes, elems):
"""
Function to draw obstacle
axes: Axes object of figure
elems: List of plot objects
"""

x_m = self.state.get_x_m()
y_m = self.state.get_y_m()
yaw_rad = self.state.get_yaw_rad()

transformed_array = self.array.homogeneous_transformation(x_m, y_m, yaw_rad)
obstacle_plot, = axes.plot(transformed_array.get_x_data(),
transformed_array.get_y_data(),
lw=1.0, color='k', ls='-')
elems.append(obstacle_plot)
```
The update function advances obstacle motion using the state model:

The method, vertex_xy is used to get the vertexes coordinates (x, y) of the obstacle and to generate a point cloud by the sensor models.
```python
def vertex_xy(self):
"""
Function to get obstacle's vertex point coordinates
"""

x_m = self.state.get_x_m()
y_m = self.state.get_y_m()
yaw_rad = self.state.get_yaw_rad()

transformed_array = self.array.homogeneous_transformation(x_m, y_m, yaw_rad)

return transformed_array.get_x_data(), transformed_array.get_y_data()
def update(self, time_s):
self.state.update(self.accel_mps2, self.yaw_rate_rps, time_s)
```

### 3.1.2 Obstacles list
ObstacleList class is implemented to execute update and draw the data of multiple obstacles sequencially. An object of this list is given to a visualizer object and a sensor model object.
The draw function transforms the rectangle contour from obstacle coordinates to global coordinates and plots it:

[obstacle_list.py](/src/components/obstacle/obstacle_list.py)
```python
"""
obstacle_list.py
def draw(self, axes, elems):
x_m = self.state.get_x_m()
y_m = self.state.get_y_m()
yaw_rad = self.state.get_yaw_rad()

transformed_array = self.array.homogeneous_transformation(x_m, y_m, yaw_rad)
obstacle_plot, = axes.plot(transformed_array.get_x_data(),
transformed_array.get_y_data(),
lw=1.0, color='k', ls='-')
elems.append(obstacle_plot)
```

Author: Shisato Yano
"""
The `vertex_xy()` method returns transformed corner coordinates for downstream modules such as LiDAR point-cloud generation.

### 3.1.2 Obstacles List
`ObstacleList` is a small container class for multiple obstacles:

```python
class ObstacleList:
"""
Obstacles list class
"""

def __init__(self):
"""
Constructor
"""

self.list = []

def add_obstacle(self, obstacle):
"""
Function to add obstacle object into list
obstacle: Obstacle class's object
"""

self.list.append(obstacle)

def update(self, times_s):
"""
Function to update each obstacle's state in list
time_s: Time interval value[sec]
"""

for obst in self.list: obst.update(times_s)

def draw(self, axes, elems):
"""
Function to draw each obstacles in list
axes: Axes object of figure
elems: List of plot objects
"""

for obst in self.list: obst.draw(axes, elems)

def get_list(self):
"""
Function to get obstacle's list
"""

return self.list
```

This class has a list to store each obstacle objects as a member. Each obstacles can be stored by using a method, add_obstacle. The data of the obstacles can be updated and drawn sequencially by using update and draw methods. Finally, get_list method is implemented to get the list for a sensor model object.
It also provides batch `update()` and `draw()` calls, plus `get_list()` for other modules (for example, LiDAR).

### 3.1.3 Visualization
A sample program for visualizing some obstacles in the simulation world is implemented as follow. A static obstacle and two dynamic obstacles can be drawn by giving an acceleration input and a yaw rate input.
A sample script for obstacle visualization is provided:

[visualize_obstacle.py](/doc/3_sensor_models/visualize_obstacle.py)
```python
"""
visualize_obstacle.py

Author: Shisato Yano
"""

# import path setting
import numpy as np
import sys
from pathlib import Path

abs_dir_path = str(Path(__file__).absolute().parent)
relative_path = "/../../src/components/"

sys.path.append(abs_dir_path + relative_path + "visualization")
sys.path.append(abs_dir_path + relative_path + "state")
sys.path.append(abs_dir_path + relative_path + "vehicle")
sys.path.append(abs_dir_path + relative_path + "obstacle")


# import component modules
from global_xy_visualizer import GlobalXYVisualizer
from min_max import MinMax
from time_parameters import TimeParameters
from vehicle_specification import VehicleSpecification
from state import State
from four_wheels_vehicle import FourWheelsVehicle
from obstacle import Obstacle
from obstacle_list import ObstacleList


# flag to show plot figure
# when executed as unit test, this flag is set as false
show_plot = True


def main():
"""
Main process function
"""

# set simulation parameters
x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30)
vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=20))

# create obstacle instances
obst_list = ObstacleList()
obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0)
obst_list.add_obstacle(obst1)
obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0)
obst_list.add_obstacle(obst2)
obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15))
obst_list.add_obstacle(obst3)
vis.add_object(obst_list)

# create vehicle instance
spec = VehicleSpecification(area_size=30.0) # spec instance
vehicle = FourWheelsVehicle(State(color=spec.color), spec) # set state, spec as arguments
vis.add_object(vehicle)

# plot figure is not shown when executed as unit test
if not show_plot: vis.not_show_plot()

# show plot figure
vis.draw()


# execute main process
if __name__ == "__main__":
main()
```
- [visualize_obstacle.py](/doc/3_sensor_models/visualize_obstacle.py)

This script creates one static obstacle and two dynamic obstacles, then draws them in the simulation world together with a vehicle model.

The output example is:

The following simulation can be visualized by executing the above program.
![](/doc/3_sensor_models/visualize_obstacle.gif)
![](/doc/3_sensor_models/visualize_obstacle.gif)
Loading
Loading