|
| 1 | +import logging |
| 2 | +import os |
| 3 | +import shutil |
| 4 | +import subprocess |
| 5 | +import time |
| 6 | +import unittest |
| 7 | + |
| 8 | +import casadi.casadi as cs |
| 9 | +import opengen as og |
| 10 | + |
| 11 | + |
| 12 | +class Ros2BuildTestCase(unittest.TestCase): |
| 13 | + """Integration tests for auto-generated ROS2 packages.""" |
| 14 | + |
| 15 | + TEST_DIR = ".python_test_build" |
| 16 | + OPTIMIZER_NAME = "rosenbrock_ros2" |
| 17 | + PACKAGE_NAME = "parametric_optimizer_ros2" |
| 18 | + NODE_NAME = "open_node_ros2" |
| 19 | + |
| 20 | + @staticmethod |
| 21 | + def get_open_local_absolute_path(): |
| 22 | + """Return the absolute path to the local OpEn repository root.""" |
| 23 | + cwd = os.getcwd() |
| 24 | + return cwd.split('open-codegen')[0] |
| 25 | + |
| 26 | + @classmethod |
| 27 | + def solverConfig(cls): |
| 28 | + """Return a solver configuration shared by the ROS2 tests.""" |
| 29 | + return og.config.SolverConfiguration() \ |
| 30 | + .with_lbfgs_memory(15) \ |
| 31 | + .with_tolerance(1e-4) \ |
| 32 | + .with_initial_tolerance(1e-4) \ |
| 33 | + .with_delta_tolerance(1e-4) \ |
| 34 | + .with_initial_penalty(15.0) \ |
| 35 | + .with_penalty_weight_update_factor(10.0) \ |
| 36 | + .with_max_inner_iterations(155) \ |
| 37 | + .with_max_duration_micros(1e8) \ |
| 38 | + .with_max_outer_iterations(50) \ |
| 39 | + .with_sufficient_decrease_coefficient(0.05) \ |
| 40 | + .with_cbfgs_parameters(1.5, 1e-10, 1e-12) \ |
| 41 | + .with_preconditioning(False) |
| 42 | + |
| 43 | + @classmethod |
| 44 | + def setUpRos2PackageGeneration(cls): |
| 45 | + """Generate the ROS2 package used by the ROS2 integration tests.""" |
| 46 | + u = cs.MX.sym("u", 5) |
| 47 | + p = cs.MX.sym("p", 2) |
| 48 | + phi = og.functions.rosenbrock(u, p) |
| 49 | + c = cs.vertcat(1.5 * u[0] - u[1], |
| 50 | + cs.fmax(0.0, u[2] - u[3] + 0.1)) |
| 51 | + bounds = og.constraints.Ball2(None, 1.5) |
| 52 | + meta = og.config.OptimizerMeta() \ |
| 53 | + .with_optimizer_name(cls.OPTIMIZER_NAME) |
| 54 | + problem = og.builder.Problem(u, p, phi) \ |
| 55 | + .with_constraints(bounds) \ |
| 56 | + .with_penalty_constraints(c) |
| 57 | + ros_config = og.config.RosConfiguration() \ |
| 58 | + .with_package_name(cls.PACKAGE_NAME) \ |
| 59 | + .with_node_name(cls.NODE_NAME) \ |
| 60 | + .with_rate(35) \ |
| 61 | + .with_description("really cool ROS2 node") |
| 62 | + build_config = og.config.BuildConfiguration() \ |
| 63 | + .with_open_version(local_path=cls.get_open_local_absolute_path()) \ |
| 64 | + .with_build_directory(cls.TEST_DIR) \ |
| 65 | + .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \ |
| 66 | + .with_build_c_bindings() \ |
| 67 | + .with_ros2(ros_config) |
| 68 | + og.builder.OpEnOptimizerBuilder(problem, |
| 69 | + metadata=meta, |
| 70 | + build_configuration=build_config, |
| 71 | + solver_configuration=cls.solverConfig()) \ |
| 72 | + .build() |
| 73 | + |
| 74 | + @classmethod |
| 75 | + def setUpClass(cls): |
| 76 | + """Generate the ROS2 package once before all tests run.""" |
| 77 | + if shutil.which("ros2") is None or shutil.which("colcon") is None: |
| 78 | + raise unittest.SkipTest("ROS2 CLI tools are not available in PATH") |
| 79 | + cls.setUpRos2PackageGeneration() |
| 80 | + |
| 81 | + @classmethod |
| 82 | + def ros2_package_dir(cls): |
| 83 | + """Return the filesystem path to the generated ROS2 package.""" |
| 84 | + return os.path.join( |
| 85 | + cls.TEST_DIR, |
| 86 | + cls.OPTIMIZER_NAME, |
| 87 | + cls.PACKAGE_NAME) |
| 88 | + |
| 89 | + @classmethod |
| 90 | + def ros2_test_env(cls): |
| 91 | + """Return the subprocess environment used by ROS2 integration tests.""" |
| 92 | + env = os.environ.copy() |
| 93 | + ros2_dir = cls.ros2_package_dir() |
| 94 | + os.makedirs(os.path.join(ros2_dir, ".ros_log"), exist_ok=True) |
| 95 | + env["ROS_LOG_DIR"] = os.path.join(ros2_dir, ".ros_log") |
| 96 | + env.setdefault("RMW_IMPLEMENTATION", "rmw_fastrtps_cpp") |
| 97 | + env.pop("ROS_LOCALHOST_ONLY", None) |
| 98 | + return env |
| 99 | + |
| 100 | + @staticmethod |
| 101 | + def _bash(command, cwd, env=None, timeout=180, check=True): |
| 102 | + """Run a bash command and return the completed process.""" |
| 103 | + return subprocess.run( |
| 104 | + ["/bin/bash", "-lc", command], |
| 105 | + cwd=cwd, |
| 106 | + env=env, |
| 107 | + text=True, |
| 108 | + capture_output=True, |
| 109 | + timeout=timeout, |
| 110 | + check=check) |
| 111 | + |
| 112 | + def test_ros2_package_generation(self): |
| 113 | + """Verify the ROS2 package files are generated.""" |
| 114 | + ros2_dir = self.ros2_package_dir() |
| 115 | + self.assertTrue(os.path.isfile(os.path.join(ros2_dir, "package.xml"))) |
| 116 | + self.assertTrue(os.path.isfile(os.path.join(ros2_dir, "CMakeLists.txt"))) |
| 117 | + self.assertTrue(os.path.isfile( |
| 118 | + os.path.join(ros2_dir, "launch", "open_optimizer.launch.py"))) |
| 119 | + |
| 120 | + def test_generated_ros2_package_works(self): |
| 121 | + """Build, run, and call the generated ROS2 package.""" |
| 122 | + ros2_dir = self.ros2_package_dir() |
| 123 | + env = self.ros2_test_env() |
| 124 | + |
| 125 | + self._bash( |
| 126 | + f"source install/setup.bash >/dev/null 2>&1 || true; " |
| 127 | + f"colcon build --packages-select {self.PACKAGE_NAME}", |
| 128 | + cwd=ros2_dir, |
| 129 | + env=env, |
| 130 | + timeout=600) |
| 131 | + |
| 132 | + node_process = subprocess.Popen( |
| 133 | + [ |
| 134 | + "/bin/bash", |
| 135 | + "-lc", |
| 136 | + f"source install/setup.bash && " |
| 137 | + f"ros2 run {self.PACKAGE_NAME} {self.NODE_NAME}" |
| 138 | + ], |
| 139 | + cwd=ros2_dir, |
| 140 | + env=env, |
| 141 | + text=True, |
| 142 | + stdout=subprocess.PIPE, |
| 143 | + stderr=subprocess.STDOUT) |
| 144 | + |
| 145 | + try: |
| 146 | + node_seen = False |
| 147 | + topics_seen = False |
| 148 | + for _ in range(6): |
| 149 | + node_result = self._bash( |
| 150 | + "source install/setup.bash && " |
| 151 | + "ros2 node list --no-daemon --spin-time 5", |
| 152 | + cwd=ros2_dir, |
| 153 | + env=env, |
| 154 | + timeout=30, |
| 155 | + check=False) |
| 156 | + topic_result = self._bash( |
| 157 | + "source install/setup.bash && " |
| 158 | + "ros2 topic list --no-daemon --spin-time 5", |
| 159 | + cwd=ros2_dir, |
| 160 | + env=env, |
| 161 | + timeout=30, |
| 162 | + check=False) |
| 163 | + node_seen = f"/{self.NODE_NAME}" in node_result.stdout |
| 164 | + topics_seen = "/parameters" in topic_result.stdout and "/result" in topic_result.stdout |
| 165 | + if node_seen and topics_seen: |
| 166 | + break |
| 167 | + time.sleep(1) |
| 168 | + |
| 169 | + if not (node_seen and topics_seen): |
| 170 | + process_output = "" |
| 171 | + if node_process.poll() is None: |
| 172 | + node_process.terminate() |
| 173 | + try: |
| 174 | + node_process.wait(timeout=10) |
| 175 | + except subprocess.TimeoutExpired: |
| 176 | + node_process.kill() |
| 177 | + node_process.wait(timeout=10) |
| 178 | + if node_process.stdout is not None: |
| 179 | + process_output = node_process.stdout.read() |
| 180 | + self.fail( |
| 181 | + "Generated ROS2 node did not become discoverable.\n" |
| 182 | + f"ros2 node list output:\n{node_result.stdout}\n" |
| 183 | + f"ros2 topic list output:\n{topic_result.stdout}\n" |
| 184 | + f"node process output:\n{process_output}") |
| 185 | + |
| 186 | + echo_process = subprocess.Popen( |
| 187 | + [ |
| 188 | + "/bin/bash", |
| 189 | + "-lc", |
| 190 | + "source install/setup.bash && " |
| 191 | + "ros2 topic echo /result --once" |
| 192 | + ], |
| 193 | + cwd=ros2_dir, |
| 194 | + env=env, |
| 195 | + text=True, |
| 196 | + stdout=subprocess.PIPE, |
| 197 | + stderr=subprocess.STDOUT) |
| 198 | + |
| 199 | + try: |
| 200 | + time.sleep(1) |
| 201 | + self._bash( |
| 202 | + "source install/setup.bash && " |
| 203 | + "ros2 topic pub --once /parameters " |
| 204 | + f"{self.PACKAGE_NAME}/msg/OptimizationParameters " |
| 205 | + "'{parameter: [1.0, 2.0], initial_guess: [0.0, 0.0, 0.0, 0.0, 0.0], initial_y: [], initial_penalty: 15.0}'", |
| 206 | + cwd=ros2_dir, |
| 207 | + env=env, |
| 208 | + timeout=60) |
| 209 | + echo_stdout, _ = echo_process.communicate(timeout=60) |
| 210 | + finally: |
| 211 | + if echo_process.poll() is None: |
| 212 | + echo_process.terminate() |
| 213 | + echo_process.wait(timeout=10) |
| 214 | + |
| 215 | + self.assertIn("solution", echo_stdout) |
| 216 | + self.assertIn("solve_time_ms", echo_stdout) |
| 217 | + finally: |
| 218 | + if node_process.poll() is None: |
| 219 | + node_process.terminate() |
| 220 | + try: |
| 221 | + node_process.wait(timeout=10) |
| 222 | + except subprocess.TimeoutExpired: |
| 223 | + node_process.kill() |
| 224 | + node_process.wait(timeout=10) |
| 225 | + |
| 226 | + |
| 227 | +if __name__ == '__main__': |
| 228 | + logging.getLogger('retry').setLevel(logging.ERROR) |
| 229 | + unittest.main() |
0 commit comments