|
| 1 | +#!/usr/bin/env python3 |
| 2 | +# SPDX-FileCopyrightText: Alliander N. V. |
| 3 | +# |
| 4 | +# SPDX-License-Identifier: Apache-2.0 |
| 5 | + |
| 6 | +import re |
| 7 | +import subprocess |
| 8 | + |
| 9 | +import numpy as np |
| 10 | +import rclpy |
| 11 | +from rclpy.node import Node |
| 12 | +from scipy.spatial.transform import RigidTransform, Rotation |
| 13 | + |
| 14 | + |
| 15 | +class SpawnPlatform(Node): |
| 16 | + """Node to spawn platforms in the Gazebo simulation.""" |
| 17 | + |
| 18 | + def __init__(self): |
| 19 | + """Initialize the node.""" |
| 20 | + super().__init__("spawn_platforms") |
| 21 | + self.declare_parameter("platforms", "") |
| 22 | + self.declare_parameter("positions", "") |
| 23 | + self.declare_parameter("orientations", "") |
| 24 | + self.declare_parameter("parents", "") |
| 25 | + self.declare_parameter("parent_links", "") |
| 26 | + platforms = ( |
| 27 | + self.get_parameter("platforms").get_parameter_value().string_value.split() |
| 28 | + ) |
| 29 | + positions = ( |
| 30 | + self.get_parameter("positions").get_parameter_value().string_value.split() |
| 31 | + ) |
| 32 | + orientations = ( |
| 33 | + self.get_parameter("orientations") |
| 34 | + .get_parameter_value() |
| 35 | + .string_value.split() |
| 36 | + ) |
| 37 | + parents = ( |
| 38 | + self.get_parameter("parents").get_parameter_value().string_value.split() |
| 39 | + ) |
| 40 | + parent_links = ( |
| 41 | + self.get_parameter("parent_links") |
| 42 | + .get_parameter_value() |
| 43 | + .string_value.split() |
| 44 | + ) |
| 45 | + |
| 46 | + self.get_logger().info("Spawning platforms...") |
| 47 | + self.spawn_platforms(platforms, positions, orientations, parents, parent_links) |
| 48 | + self.get_logger().info("All platforms spawned!") |
| 49 | + |
| 50 | + def spawn_platforms( |
| 51 | + self, |
| 52 | + platforms: list[str], |
| 53 | + positions: list[str], |
| 54 | + orientations: list[str], |
| 55 | + parents: list[str], |
| 56 | + parent_links: list[str], |
| 57 | + ) -> None: |
| 58 | + """Spawn platforms in the Gazebo simulation at specified positions. |
| 59 | +
|
| 60 | + Args: |
| 61 | + platforms (list[str]): List of the platforms. |
| 62 | + positions (list[str]): List of positions in the format "x,y,z". |
| 63 | + orientations (list[str]): List of orientations in the format "roll,pitch,yaw". |
| 64 | + parents (list[str]): List of parent names or "none" when the platform has no parent. |
| 65 | + parent_links (list[str]): List of parent link names or "none" when the platform has no parent. |
| 66 | +
|
| 67 | + """ |
| 68 | + for n in range(len(platforms)): |
| 69 | + platform = platforms[n] |
| 70 | + position = np.array(list(map(float, positions[n].split(",")))) |
| 71 | + orientation = np.array(list(map(float, orientations[n].split(",")))) |
| 72 | + parent = parents[n] |
| 73 | + parent_link = parent_links[n] |
| 74 | + |
| 75 | + if parent != "none": |
| 76 | + # First define the transform from world to model: |
| 77 | + model_pose = get_pose(parent) |
| 78 | + model_tf = RigidTransform.from_components( |
| 79 | + model_pose["position"], |
| 80 | + Rotation.from_euler("xyz", model_pose["orientation"]), |
| 81 | + ) |
| 82 | + |
| 83 | + # Next define the transform from model to link: |
| 84 | + link_pose = get_pose(parent, parent_link) |
| 85 | + link_tf = RigidTransform.from_components( |
| 86 | + link_pose["position"], |
| 87 | + Rotation.from_euler("xyz", link_pose["orientation"]), |
| 88 | + ) |
| 89 | + |
| 90 | + # Finally, combine the transforms and apply the given position and orientation: |
| 91 | + tf = model_tf * link_tf |
| 92 | + position = tf.apply(position) |
| 93 | + rotation = tf.rotation * Rotation.from_euler("xyz", orientation) |
| 94 | + orientation = rotation.as_euler("xyz") |
| 95 | + |
| 96 | + self.spawn_platform(platform, position, orientation) |
| 97 | + |
| 98 | + def spawn_platform( |
| 99 | + self, namespace: str, position: np.ndarray, orientation: np.ndarray |
| 100 | + ) -> None: |
| 101 | + """Spawn a platform in the Gazebo simulation with a specified position and orientation. |
| 102 | +
|
| 103 | + Args: |
| 104 | + namespace (str): The namespace of the platform. |
| 105 | + position (np.ndarray): The position [x, y, z] of the platform. |
| 106 | + orientation (np.ndarray): The orientation [roll, pitch, yaw] of the platform. |
| 107 | + """ |
| 108 | + self.get_logger().info(f"Spawn: {namespace} {position} {orientation}") |
| 109 | + x, y, z = position |
| 110 | + roll, pitch, yaw = orientation |
| 111 | + subprocess.run( |
| 112 | + [ |
| 113 | + "ros2", |
| 114 | + "run", |
| 115 | + "ros_gz_sim", |
| 116 | + "create", |
| 117 | + "-topic", |
| 118 | + f"/{namespace}/robot_description", |
| 119 | + "-name", |
| 120 | + namespace, |
| 121 | + "-x", |
| 122 | + str(x), |
| 123 | + "-y", |
| 124 | + str(y), |
| 125 | + "-z", |
| 126 | + str(z), |
| 127 | + "-R", |
| 128 | + str(roll), |
| 129 | + "-P", |
| 130 | + str(pitch), |
| 131 | + "-Y", |
| 132 | + str(yaw), |
| 133 | + ], |
| 134 | + check=True, |
| 135 | + ) |
| 136 | + |
| 137 | + |
| 138 | +def get_pose(model: str, link: str | None = None) -> dict: |
| 139 | + """Get the pose of a model or a specific link of a model in the Gazebo simulation. |
| 140 | +
|
| 141 | + Args: |
| 142 | + model (str): The name of the model. |
| 143 | + link (str | None): The name of the link. |
| 144 | +
|
| 145 | + Returns: |
| 146 | + dict: A dictionary with the position and orientation of the link. |
| 147 | +
|
| 148 | + Raises: |
| 149 | + RuntimeError: If the link info could not be retrieved or parsed. |
| 150 | + """ |
| 151 | + command = ["gz", "model", "-m", model] |
| 152 | + if link: |
| 153 | + command.extend(["-l", link]) |
| 154 | + message = subprocess.check_output(command, stderr=subprocess.DEVNULL).decode( |
| 155 | + "utf-8" |
| 156 | + ) |
| 157 | + |
| 158 | + lines = message.splitlines() |
| 159 | + line_of_interest = None |
| 160 | + for n, line in enumerate(lines): |
| 161 | + if line == " - Pose [ XYZ (m) ] [ RPY (rad) ]:": |
| 162 | + if line_of_interest is not None: |
| 163 | + raise RuntimeError("Found multiple lines containing pose information.") |
| 164 | + line_of_interest = n |
| 165 | + if line_of_interest is None: |
| 166 | + raise RuntimeError(f"Could not find pose information for {model} - {link}.") |
| 167 | + position = lines[line_of_interest + 1] |
| 168 | + orientation = lines[line_of_interest + 2] |
| 169 | + return { |
| 170 | + "position": process_string(position), |
| 171 | + "orientation": process_string(orientation), |
| 172 | + } |
| 173 | + |
| 174 | + |
| 175 | +def process_string(info_string: str) -> np.ndarray: |
| 176 | + """Process the info string to extract the position or orientation values. |
| 177 | +
|
| 178 | + Args: |
| 179 | + info_string (str): The info string. |
| 180 | +
|
| 181 | + Returns: |
| 182 | + np.ndarray: An array of float values extracted from the info string. |
| 183 | +
|
| 184 | + Raises: |
| 185 | + RuntimeError: If the info string is not in the expected format. |
| 186 | + """ |
| 187 | + values_string = re.search(r"\[(.*?)\]", info_string) |
| 188 | + |
| 189 | + if not values_string: |
| 190 | + raise RuntimeError("Could not parse message.") |
| 191 | + |
| 192 | + group = values_string.group(1) |
| 193 | + return np.array(list(map(float, group.split()))) |
| 194 | + |
| 195 | + |
| 196 | +def main(args: list | None = None) -> None: |
| 197 | + """Main function to initialize the ROS 2 node and set the thresholds. |
| 198 | +
|
| 199 | + Args: |
| 200 | + args (list | None): Command line arguments, defaults to None. |
| 201 | + """ |
| 202 | + rclpy.init(args=args) |
| 203 | + spawn_platform = SpawnPlatform() |
| 204 | + spawn_platform.destroy_node() |
| 205 | + rclpy.shutdown() |
| 206 | + |
| 207 | + |
| 208 | +if __name__ == "__main__": |
| 209 | + main() |
0 commit comments