Controller package

Some description.

API Reference

Movement controller

Path divisions

Path segment

Path segment for the robot path.

The path segment is a part of the robot path. It contains the path, the robot namespace, the clearance required by the path, and the required resources. Original path is divided into segments and each segment is checked if the robot can enter it. The segments are checked for collisions and deadlocks.

class mmrs_controller.path.path_segment.PathSegment(robot_id: str, path: Path, clearance: float, required_resources: set[UUID] | None = None)

Segment of the robot path.

Represents a segment of the robot path. The segment contains the path, the robot namespace, the clearance required by the path, and the required resources.

Parameters:

robot_id – The namespace of the robot.
path – The segment of the robot path.
clearance – The required clearance for the path.
required_resources – The required resources for the segment (defaults to None).

to_msg() → PathSegment

Convert the path segment to a ROS message.

Returns:: The ROS message of the path segment.

Path divider

Path divider for the controller of the multi-mobile robot system.

Divide a path into segments. The path is represented by a list of poses. Each pose is a point in 2D space with orientation. The path is divided into segments and each segment is represented by a list of poses. Each segment has a unique identifier. The segments are used by the controller to plan the movement of the robot.

class mmrs_controller.path.dividers.path_divider.PathDivider

Base class for path dividers.

_assign_resources(segments: Mapping[str, Sequence[PathSegment]]) → Mapping[str, Sequence[PathSegment]]

Assign required resources to each segment.

Parameters:: segments – The segments to assign resources to.
Returns:: The segments with assigned resources.

static _lines_too_close(first: tuple[float, LineString], second: tuple[float, LineString]) → bool

Check if two lines are too close to each other.

Parameters:

first – The first line with its required clearance.
second – The second line with its required clearance.

Returns:

True if the lines are too close to each other, False otherwise.

add_robot_clearence(robot_namespace: str, clearance: float) → None

Add required clearance for the robot.

Parameters:

robot_namespace – The robot namespace.
clearance – The required clearance.

divide(robots_paths: Mapping[str, Path]) → Mapping[str, Sequence[PathSegment]]

Divide all defined paths into segments.

Parameters:: robots_paths – The paths to divide.
Returns:: The paths divided into segments for each robot.

abstract divide_single(path: Path, robot_namespace: str, other_paths: Mapping[str, Path]) → Sequence[PathSegment]

Divide a single path into segments.

Parameters:

path – The path to divide.
robot_namespace – The robot namespace.
other_paths – Other robots paths.

Returns:

The path divided into segments.

remove_robot_clearance(robot_namespace: str) → None

Remove required clearance for the robot.

Parameters:: robot_namespace – The robot namespace.

Equal length divider

Equal length path divider for the controller of the multi-mobile robot system.

Divide a path into equal length segments. The length is computed as the euclidean distance between two consecutive points in the path. The path is segmented into equal length segments. Each segment is represented by a list of poses. Each segment has a unique identifier.

class mmrs_controller.path.dividers.equal_length_divider.EqualLengthPathDivider(segment_length: float)

Equal length path divider.

divide_single(path: Path, robot_namespace: str, other_paths: Mapping[str, Path]) → Sequence[PathSegment]

Divides a single path into segments.

Parameters:

path – The path to divide.
robot_namespace – The namespace of the robot.
other_paths – The paths of other robots. Not used in this implementation.

Returns:

The path divided into segments.

Optimal length divider

Optimal length path divider for the controller of the multi-mobile robot system.

Divide a path into optimal length segments. Iterate over the path and check the clearance of the path to other paths. If the clearance is too small, create a segment and start a new segment. The clearance is calculated using the Euclidean distance. If the clearance come back to normal, end the segment and start a new one. The segments are used by the controller to plan the movement of the robot.

class mmrs_controller.path.dividers.optimal_length_divider.OptimalLengthPathDivider

Optimal length path divider.

divide_single(path: Path, robot_namespace: str, other_paths: Mapping[str, Path]) → Sequence[PathSegment]

Divide a single path into segments.

Parameters:

path – The path to divide.
robot_namespace – The robot namespace.
other_paths – Other robots paths.

Returns:

The path divided into segments.

Grid cells divider

Grid cell based path divider for the controller of the multi-mobile robot system.

Divide a path based on the grid of the 2D motion space. The grid is represented by the cells. Additionaly cells are subdivided into squares that can tell whether the robot can fit into one cell or requires more. For this the corners of cells have circles dfefined that represenst that all four cells are required for the robot to fit. The path is divided into segments and each segment is represented by a list of poses. Each segment has a unique identifier.

class mmrs_controller.path.dividers.grid_cells_divider.GridCellsPathDivider(size_m: float, grid_size_m: float, circle_radius_m: float)

Grid cells path divider.

_generate_boxes_polygons(ticks: ndarray, clearance: float) → PolygonsData

Generate the boxes for the grid.

Parameters:

ticks – Thje ticks for the grid.
clearance – The clearance for the boxes.

Returns:

The generated polygons for the boxes.

_generate_circle_polygons(ticks: ndarray, clearance: float) → PolygonsData

Generate circles for the grid.

Parameters:

ticks – The ticks for the grid.
clearance – The clearance for the circles.

Returns:

The generated polygons for the circles.

_generate_polygons(ticks: ndarray, clearance: float) → PolygonsData

Generate polygons for the grid and circles.

Parameters:

ticks – The ticks for the grid.
clearance – The clearances for the cells and circles that are required to fit the robot in one cell.

Returns:

The generated polygons that cover whole 2D motion space.

_generate_subgrid_boxes(subgrid: tuple[float, float, float, float], *, clearance: float) → PolygonsData

Genertate 9 non-overlapping boxes within a main grid cell.

The boxes that are in the corners of the cell are substracted by the circles that are required for the robot to fit into one cell.

Parameters:

subgrid – The coordinates of the one cell of the grid.
clearance – The clearance for the boxes.

Returns:

The generated polygons for the boxes of one cell.

static _generate_ticks(start: float, end: float, grid_spacing: float) → ndarray

Generate ticks for the grid.

Parameters:

start – Start of the grid.
end – End of the grid.
grid_spacing – Spacing of the grid.

Returns:

Ticks for the grid.

_get_cell_index(x: float, y: float) → tuple[int, int]

Get the cell index for a given point.

Parameters:

x – The x coordinate of the point in the global frame.
y – The y coordinate of the point in the global frame.

Raises:

ValueError – If the point is outside of the grid.

Returns:

The cell index for the given point.

divide(robots_paths: Mapping[str, Path]) → Mapping[str, Sequence[PathSegment]]

Divide all defined paths into segments.

Parameters:: robots_paths – The paths to divide.
Returns:: The paths divided into segments for each robot.

divide_single(path: Path, robot_namespace: str, other_paths: Mapping[str, Path]) → Sequence[PathSegment]

Divides a single path into segments.

Parameters:

path – The path to divide.
robot_namespace – The namespace of the robot.
other_paths – The paths of other robots. Not used in this implementation.

Returns:

The path divided into segments.

class mmrs_controller.path.dividers.grid_cells_divider.PolygonsData(polygons: dict[UUID, Polygon], polygons_cells: dict[UUID, list[tuple[int, int]]], cells_polygons: dict[tuple[int, int], list[UUID]])

Polygons generatation data.

Parameters:

polygons – The mapping of polygons id to their geometry.
polygons_cells – The mapping of polygons id to cells that are required for robot to fit.
cells_polygons – The mapping of cells to polygons that are contained in it.

Robot movement

Main controller

Controller node for multi mobile robot system.

class mmrs_controller.controller.Controller

Controller node for multi mobile robot system.

_navigate_action_goal_response_callback(future: Future, *, robot_namespace: str) → None

Navigate action goal handle callback.

Parameters:

future (Future) – The result of the action request. It is a goal handle.
robot_namespace (str) – The namespace of the robot.

_navigate_action_result_callback(future: Future, *, robot_namespace: str) → None

Navigate action result handle callback.

Parameters:

future (Future) – The result of the action request. It is a result of the action.
robot_namespace (str) – The namespace of the robot.

_send_one_navigation_goal(robot_namespace: str, segments: Sequence[PathSegment]) → None

Send navigation goal to robot.

Parameters:

robot_namespace (str) – The namespace of the robot.
segments (Sequence[PathSegment]) – The segments of the path to send to the robot.

alive_msg_callback(msg: RobotMetadata) → None

Callback for the alive message.

Parameters:: msg (RobotMetadata) – The alive message with metadata.

alive_timer_callback() → None: Check if robots are alive.

divide_paths() → None: Send navigation goals to robots.

mission_msg_callback(msg: Mission) → None

Callback for the mission message.

Navigates the robot to pose or through poses.

Parameters:: msg (Mission) – The mission message with goal.

register_calculated_robot_path(future: Future, *, robot_namespace: str) → None

Navigate robot to the goal.

Parameters:

future (Future) – The future of the service call that requests path calculation.
robot_namespace (str) – The namespace of the robot.

register_robot_in_registry(request: Register_Request, response: Register_Response) → Register_Response

Registers robot in registry with metadata. Create all clients for robot control.

Parameters:

request (Register.Request) – The request to register robot.
response (Register.Response) – The response of the registration in the registry.

Returns:

The response of the registration in registry.

Return type:

request_path_through_poses(poses: list[PoseStamped], *, robot_namespace: str) → None

Request a path through poses from the navigator.

Parameters:

robot_namespace (str) – The namespace of the robot.
poses (list[PoseStamped]) – The poses to navigate through.

request_path_to_pose(pose: PoseStamped, *, robot_namespace: str) → None

Request a path to pose from the navigator.

Parameters:

robot_namespace (str) – The namespace of the robot.
pose (PoseStamped) – The pose to navigate to.

mmrs_controller.controller.main(args: list[str] | None = None) → None: Run controller node.