diff --git a/.openads-dev-environment b/.openads-dev-environment index 9795160..3f9828c 160000 --- a/.openads-dev-environment +++ b/.openads-dev-environment @@ -1 +1 @@ -Subproject commit 97951603333e7f5a55592e7a492d1f97facf3465 +Subproject commit 3f9828c4f933d223f29105228a71a7e32f0013ac diff --git a/README.md b/README.md index d78d1c6..1cc3b1b 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # trajectory_optimization

- + @@ -32,7 +32,7 @@ The ROS 2 node uses the open-source ROS 2 message definitions [perception_interf > [!IMPORTANT] -> This repository is part of [***OpenADS***](https://github.com/openads-project), the *Open Automated Driving Stack*. *OpenADS* and its modules have been initiated and are currently being maintained by the [**Institute for Automotive Engineering (ika) at RWTH Aachen University**](https://www.ika.rwth-aachen.de/de/). +> This repository is part of [***OpenADS***](https://openads-project.github.io/), the *Open Automated Driving Systems* project. *OpenADS* and its modules have been initiated and are currently being maintained by the [**Institute for Automotive Engineering (ika) at RWTH Aachen University**](https://www.ika.rwth-aachen.de/de/). ## 🚀 Quick Start diff --git a/benchmarking/.gitignore b/benchmarking/.gitignore new file mode 100644 index 0000000..16f2dc5 --- /dev/null +++ b/benchmarking/.gitignore @@ -0,0 +1 @@ +*.csv \ No newline at end of file diff --git a/benchmarking/README.md b/benchmarking/README.md new file mode 100644 index 0000000..adffeae --- /dev/null +++ b/benchmarking/README.md @@ -0,0 +1,62 @@ +# Trajectory optimization benchmarking + +This directory contains standalone tooling for extracting and comparing trajectory optimizer performance measurements. The tools are intentionally not installed as part of the ROS package. + +## Recording a new run + +Enable `performance_logging` in the optimizer configuration. The node creates a timestamped file such as +`trajectory_optimization_ackermann_node_20260715T142355_123Z.csv` and buffers up to 100 records before flushing. + +By default, files are written to `/tmp/trajectory_optimization_benchmarks`. To place them in this directory, set the output directory before starting the node: + +```bash +export TRAJECTORY_OPTIMIZATION_BENCHMARK_DIR="$(pwd)/benchmarking" +``` + +The filename identifies the node and start time, so no run ID or output path is needed in the ROS parameter file. Rename the completed CSV if a descriptive name such as `acados-0.5.5-warmstart-0.csv` is more useful. Alternatively, fill the initially empty `run_id` column after the run; avoid editing measurement values. + +For comparable runs, use the same optimizer configuration, rosbag playback rate, warm-up removal, and deadline. Keep `verbose` and `debug_visualization` disabled. + +### Recorded values + +The runtime CSV deliberately contains only values needed to compare solver behavior or explain a regression: + +- Context: schema version, source, optional run ID, cycle, record timestamp, reference-point count, and object count. +- Outcome: ACADOS status and whether a trajectory was published. +- Runtime: complete planning-cycle wall time split into preprocessing, `acados_solve()`, and postprocessing, plus ACADOS' internal total, linearization, simulation, QP, QP-solver, condensing, regularization, globalization, preparation, and feedback times. The three top-level phases add up to the complete cycle; CSV writing happens afterwards and is excluded. +- Work and quality: SQP/QP iterations, QP status, cost, KKT norm, aggregate NLP residual, and stationarity, equality, inequality, and complementarity residuals. + +Timers for input transformation, initial-guess construction, boundary preparation, individual parameter updates, solution reading, diagnostics, and message output are intentionally not recorded. They required instrumentation throughout the planning code but are not needed for the initial ACADOS version and option comparisons. They can be profiled separately if a later result points at non-solver overhead. + +## Extracting a legacy rosout baseline + +The extractor needs the ROS Python environment, including `rosbag2_py`, `rclpy`, and `rcl_interfaces`. These modules come from the ROS installation and are not available as ordinary PyPI dependencies. + +```bash +python3 benchmarking/extract_rosout_performance.py \ + optimization-testing benchmarking/acados-0.5.1.csv +``` + +The resulting CSV only contains values actually present in the old logs: status, publication outcome, ACADOS total time, iterations, KKT, cost, and NLP residual. Missing values remain empty rather than being interpreted as zero. + +## Analyzing and comparing runs + +Analyze one run: + +```bash +python3 benchmarking/analyze_performance.py \ + benchmarking/acados-0.5.5.csv --skip 10 --deadline-ms 100 +``` + +Compare a candidate with a baseline: + +```bash +python3 benchmarking/analyze_performance.py \ + benchmarking/acados-0.5.5.csv \ + --compare benchmarking/acados-0.5.1.csv \ + --skip 10 --deadline-ms 100 +``` + +The report contains status and publication rates, deadline compliance, consecutive failure streaks, and timing and quality distributions. A comparison prints the deltas and a threshold-based `BETTER`, `WORSE`, or `MIXED / NO MATERIAL CHANGE` verdict. + +Output is colored automatically when stdout is a terminal. Use `--color always` to preserve colors in a compatible log viewer, `--color never` to disable them, or set the conventional `NO_COLOR` environment variable. diff --git a/benchmarking/analyze_performance.py b/benchmarking/analyze_performance.py new file mode 100755 index 0000000..2877366 --- /dev/null +++ b/benchmarking/analyze_performance.py @@ -0,0 +1,303 @@ +#!/usr/bin/env python3 + +# Copyright Institute for Automotive Engineering (ika), RWTH Aachen University +# SPDX-License-Identifier: Apache-2.0 + +"""Summarize one optimizer CSV and optionally compare it with a baseline run.""" + +import argparse +import csv +import math +import os +import statistics +import sys +from collections import Counter +from pathlib import Path + +TIMING_METRICS = [ + "acados_total_ms", + "solve_wall_ms", + "cycle_ms", + "preprocessing_ms", + "postprocessing_ms", + "acados_qp_ms", + "acados_qp_solver_ms", + "acados_lin_ms", + "acados_preparation_ms", + "acados_feedback_ms", +] +QUALITY_METRICS = ["sqp_iter", "qp_iter", "kkt", "nlp_res", "cost"] + + +class Color: + """ANSI colors used for terminal summaries.""" + + RESET = "\033[0m" + BOLD = "\033[1m" + RED = "\033[31m" + GREEN = "\033[32m" + YELLOW = "\033[33m" + CYAN = "\033[36m" + DIM = "\033[2m" + + +use_color = False + + +def paint(text, color): + """Apply an ANSI color when colored output is enabled.""" + return f"{color}{text}{Color.RESET}" if use_color else text + + +def higher_rate_color(value): + """Color rates where values close to 100% are desirable.""" + if value is None or value < 0.95: + return Color.RED + return Color.GREEN if value >= 0.99 else Color.YELLOW + + +def lower_rate_color(value): + """Color rates where values close to 0% are desirable.""" + if value is None or value > 0.01: + return Color.RED + return Color.GREEN if value == 0.0 else Color.YELLOW + + +def number(value): + """Convert a CSV value to a finite float or return None.""" + try: + result = float(value) + except (TypeError, ValueError): + return None + return result if math.isfinite(result) else None + + +def percentile(values, fraction): + """Return a linearly interpolated percentile.""" + if not values: + return None + values = sorted(values) + if len(values) == 1: + return values[0] + position = (len(values) - 1) * fraction + lower = math.floor(position) + upper = math.ceil(position) + if lower == upper: + return values[lower] + return values[lower] + (values[upper] - values[lower]) * (position - lower) + + +def values_for(records, key): + """Collect finite numeric values for one field.""" + values = [] + for record in records: + value = number(record.get(key)) + if key == "nlp_res": + residuals = [number(record.get(field)) for field in ("res_stat", "res_eq", "res_ineq", "res_comp")] + finite_residuals = [residual for residual in residuals if residual is not None] + if finite_residuals: + value = max(finite_residuals) + if value is not None: + values.append(value) + return values + + +def longest_streak(statuses, predicate): + """Return the maximum number of consecutive statuses matching predicate.""" + longest = 0 + current = 0 + for status in statuses: + current = current + 1 if predicate(status) else 0 + longest = max(longest, current) + return longest + + +def load_csv(path, skip): + """Load canonical performance records and discard warm-up rows.""" + with path.open(encoding="utf-8", newline="") as source: + records = list(csv.DictReader(source)) + records = records[skip:] + if not records: + raise SystemExit(f"No records remain in {path} after skipping {skip} rows.") + if "status" not in records[0]: + raise SystemExit(f"{path} is not a performance CSV (missing status column).") + return records + + +def summarize(records, deadline_ms): + """Calculate the stability, deadline, and distribution scorecard.""" + statuses = [int(value) for record in records if (value := number(record.get("status"))) is not None] + counts = Counter(statuses) + known = len(statuses) + published_values = values_for(records, "published") + hard_failure = lambda status: status not in (0, 2, 7) # noqa: E731 + + timing_key = "acados_total_ms" + timing_values = values_for(records, timing_key) + if not timing_values: + timing_key = "solve_wall_ms" + timing_values = values_for(records, timing_key) + + return { + "records": len(records), + "status_counts": counts, + "success_rate": counts[0] / known if known else None, + "timeout_rate": counts[7] / known if known else None, + "hard_failure_rate": sum(count for status, count in counts.items() if hard_failure(status)) / known if known else None, + "published_rate": statistics.mean(published_values) if published_values else None, + "deadline_rate": (sum(value <= deadline_ms for value in timing_values) / len(timing_values) if timing_values else None), + "timing_key": timing_key, + "timing_p50": percentile(timing_values, 0.50), + "timing_p95": percentile(timing_values, 0.95), + "timing_p99": percentile(timing_values, 0.99), + "max_timeout_streak": longest_streak(statuses, lambda status: status == 7), + "max_status4_streak": longest_streak(statuses, lambda status: status == 4), + "max_hard_failure_streak": longest_streak(statuses, hard_failure), + } + + +def format_rate(value): + """Format an optional fraction as percentage.""" + return "n/a" if value is None else f"{100.0 * value:.2f}%" + + +def print_run(path, records, summary, deadline_ms): + """Print the scorecard and useful metric distributions for one run.""" + run_ids = sorted({record.get("run_id", "") for record in records if record.get("run_id")}) + suffix = f" run_id={','.join(run_ids)}" if run_ids else "" + print(paint(f"\nRUN {path}{suffix}", Color.BOLD + Color.CYAN)) + status_parts = [] + for status, count in sorted(summary["status_counts"].items()): + color = Color.GREEN if status == 0 else Color.YELLOW if status in (2, 7) else Color.RED + status_parts.append(paint(f"{status}: {count}", color)) + print(f"records={summary['records']} status_counts={{{', '.join(status_parts)}}}") + print( + f"success={paint(format_rate(summary['success_rate']), higher_rate_color(summary['success_rate']))} " + f"timeout={paint(format_rate(summary['timeout_rate']), lower_rate_color(summary['timeout_rate']))} " + f"hard_failure={paint(format_rate(summary['hard_failure_rate']), lower_rate_color(summary['hard_failure_rate']))} " + f"published={paint(format_rate(summary['published_rate']), higher_rate_color(summary['published_rate']))} " + f"within_{deadline_ms:g}ms=" + f"{paint(format_rate(summary['deadline_rate']), higher_rate_color(summary['deadline_rate']))}" + ) + print( + f"max_streaks: timeout=" + f"{paint(str(summary['max_timeout_streak']), Color.GREEN if summary['max_timeout_streak'] == 0 else Color.YELLOW)} " + f"status4={paint(str(summary['max_status4_streak']), Color.GREEN if summary['max_status4_streak'] == 0 else Color.RED)} " + f"hard_failure=" + f"{paint(str(summary['max_hard_failure_streak']), Color.GREEN if summary['max_hard_failure_streak'] == 0 else Color.RED)}" + ) + print(paint(f"{'metric':24} {'count':>7} {'mean':>11} {'p50':>11} {'p95':>11} {'p99':>11} {'max':>11}", Color.BOLD)) + for key in TIMING_METRICS + QUALITY_METRICS: + values = values_for(records, key) + if not values: + continue + print( + f"{key:24} {len(values):7d} {statistics.mean(values):11.4g} " + f"{percentile(values, 0.50):11.4g} {percentile(values, 0.95):11.4g} " + f"{percentile(values, 0.99):11.4g} {max(values):11.4g}" + ) + + +def relative_delta(candidate, baseline): + """Return relative candidate change, or None where it is undefined.""" + if candidate is None or baseline in (None, 0): + return None + return (candidate - baseline) / baseline + + +def compare(candidate, baseline): + """Classify candidate using explicit stability and latency thresholds.""" + stability_regressions = [] + if candidate["hard_failure_rate"] is not None and baseline["hard_failure_rate"] is not None: + if candidate["hard_failure_rate"] - baseline["hard_failure_rate"] > 0.01: + stability_regressions.append("hard failures increased by >1 percentage point") + if candidate["published_rate"] is not None and baseline["published_rate"] is not None: + if baseline["published_rate"] - candidate["published_rate"] > 0.01: + stability_regressions.append("published rate decreased by >1 percentage point") + if candidate["deadline_rate"] is not None and baseline["deadline_rate"] is not None: + if baseline["deadline_rate"] - candidate["deadline_rate"] > 0.02: + stability_regressions.append("deadline rate decreased by >2 percentage points") + + improvements = [] + if candidate["deadline_rate"] is not None and baseline["deadline_rate"] is not None: + if candidate["deadline_rate"] - baseline["deadline_rate"] > 0.02: + improvements.append("deadline rate increased by >2 percentage points") + p95_delta = relative_delta(candidate["timing_p95"], baseline["timing_p95"]) + if p95_delta is not None and p95_delta < -0.05: + improvements.append("p95 solver time decreased by >5%") + if candidate["success_rate"] is not None and baseline["success_rate"] is not None: + if candidate["success_rate"] - baseline["success_rate"] > 0.02: + improvements.append("success rate increased by >2 percentage points") + + if stability_regressions: + return "WORSE", stability_regressions + if improvements: + return "BETTER", improvements + return "MIXED / NO MATERIAL CHANGE", ["no configured material-change threshold was crossed"] + + +def print_comparison(candidate, baseline): + """Print deltas and the threshold-based overall verdict.""" + print(paint("\nCOMPARISON (candidate relative to baseline)", Color.BOLD + Color.CYAN)) + rows = [ + ("success rate", candidate["success_rate"], baseline["success_rate"], "rate", True), + ("timeout rate", candidate["timeout_rate"], baseline["timeout_rate"], "rate", False), + ("hard failure rate", candidate["hard_failure_rate"], baseline["hard_failure_rate"], "rate", False), + ("published rate", candidate["published_rate"], baseline["published_rate"], "rate", True), + ("deadline rate", candidate["deadline_rate"], baseline["deadline_rate"], "rate", True), + ("solver p50 [ms]", candidate["timing_p50"], baseline["timing_p50"], "number", False), + ("solver p95 [ms]", candidate["timing_p95"], baseline["timing_p95"], "number", False), + ("solver p99 [ms]", candidate["timing_p99"], baseline["timing_p99"], "number", False), + ] + print(paint(f"{'metric':24} {'candidate':>12} {'baseline':>12} {'delta':>12}", Color.BOLD)) + for label, candidate_value, baseline_value, value_type, higher_is_better in rows: + if candidate_value is None or baseline_value is None: + print(f"{label:24} {'n/a':>12} {'n/a':>12} {'n/a':>12}") + continue + if value_type == "rate": + delta = 100.0 * (candidate_value - baseline_value) + delta_text = f"{delta:+11.2f}pp" + else: + delta = relative_delta(candidate_value, baseline_value) + delta_text = "n/a" if delta is None else f"{100.0 * delta:+.2f}%" + raw_delta = candidate_value - baseline_value + delta_color = Color.DIM if raw_delta == 0 else Color.GREEN if (raw_delta > 0) == higher_is_better else Color.RED + candidate_text = format_rate(candidate_value) if value_type == "rate" else f"{candidate_value:.4g}" + baseline_text = format_rate(baseline_value) if value_type == "rate" else f"{baseline_value:.4g}" + print(f"{label:24} {candidate_text:>12} {baseline_text:>12} {paint(f'{delta_text:>12}', delta_color)}") + + verdict, reasons = compare(candidate, baseline) + verdict_color = Color.GREEN if verdict == "BETTER" else Color.RED if verdict == "WORSE" else Color.YELLOW + print(f"{paint('VERDICT:', Color.BOLD)} {paint(verdict, Color.BOLD + verdict_color)} ({'; '.join(reasons)})") + + +def main(): + """Parse arguments and report one run plus an optional baseline comparison.""" + global use_color + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("run", type=Path, help="candidate performance CSV") + parser.add_argument("--compare", type=Path, metavar="BASELINE", help="baseline CSV") + parser.add_argument("--skip", type=int, default=0, help="discard this many warm-up rows from both runs") + parser.add_argument("--deadline-ms", type=float, default=100.0, help="solver deadline used for the deadline rate") + parser.add_argument( + "--color", + choices=("auto", "always", "never"), + default="auto", + help="colorize output (default: auto when stdout is a terminal)", + ) + args = parser.parse_args() + use_color = args.color == "always" or (args.color == "auto" and sys.stdout.isatty() and "NO_COLOR" not in os.environ) + + records = load_csv(args.run, args.skip) + summary = summarize(records, args.deadline_ms) + print_run(args.run, records, summary, args.deadline_ms) + + if args.compare: + baseline_records = load_csv(args.compare, args.skip) + baseline_summary = summarize(baseline_records, args.deadline_ms) + print_run(args.compare, baseline_records, baseline_summary, args.deadline_ms) + print_comparison(summary, baseline_summary) + + +if __name__ == "__main__": + main() diff --git a/benchmarking/extract_rosout_performance.py b/benchmarking/extract_rosout_performance.py new file mode 100755 index 0000000..e77c67d --- /dev/null +++ b/benchmarking/extract_rosout_performance.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 + +# Copyright Institute for Automotive Engineering (ika), RWTH Aachen University +# SPDX-License-Identifier: Apache-2.0 + +"""Extract legacy trajectory optimizer measurements from a rosbag /rosout topic.""" + +import argparse +import csv +import re +from pathlib import Path + +import rosbag2_py +from rcl_interfaces.msg import Log +from rclpy.serialization import deserialize_message + +CSV_FIELDS = [ + "schema_version", + "source", + "run_id", + "cycle", + "record_stamp_ns", + "status", + "published", + "ref_points", + "objects", + "sqp_iter", + "qp_iter", + "qp_status", + "cycle_ms", + "preprocessing_ms", + "solve_wall_ms", + "postprocessing_ms", + "acados_total_ms", + "acados_lin_ms", + "acados_sim_ms", + "acados_qp_ms", + "acados_qp_solver_ms", + "acados_qp_xcond_ms", + "acados_reg_ms", + "acados_glob_ms", + "acados_preparation_ms", + "acados_feedback_ms", + "cost", + "kkt", + "nlp_res", + "res_stat", + "res_eq", + "res_ineq", + "res_comp", +] + +ANSI_ESCAPE = re.compile(r"\x1b\[[0-?]*[ -/]*[@-~]") +STATUS = re.compile(r"Optimization failed with status\s+(\d+)") +SOLVER_STATUS = re.compile(r"acados_solve\(\) failed with status\s+(\d+)") +DURATION = re.compile( + r"Optimization took\s+([+\-\d.eE]+)\s+ms\.?\s+" r"\(SQP iter:\s*(\d+)(?:;\s*QP iter:\s*(\d+))?;\s*KKT:\s*([+\-\d.eE]+)\)" +) +COST = re.compile(r"cost_value:\s*([+\-\d.eE]+);\s*nlp_res:\s*([+\-\d.eE]+)") +RESIDUALS = re.compile( + r"cost_value:\s*([+\-\d.eE]+);\s*residuals:\s*" + r"stat=([+\-\d.eE]+)\s+eq=([+\-\d.eE]+)\s+" + r"ineq=([+\-\d.eE]+)\s+comp=([+\-\d.eE]+)" +) + + +def stamp_to_nanoseconds(message: Log, bag_timestamp: int) -> int: + """Prefer the ROS log timestamp and fall back to the bag record timestamp.""" + stamp = message.stamp + timestamp = stamp.sec * 1_000_000_000 + stamp.nanosec + return timestamp or bag_timestamp + + +def main(): + """Read /rosout, reconstruct solve records, and write the canonical CSV.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("bag", type=Path, help="rosbag directory containing /rosout") + parser.add_argument("output", type=Path, help="destination CSV") + parser.add_argument( + "--node", + default="trajectory_optimization", + help="only use rosout records whose node name contains this value", + ) + args = parser.parse_args() + + reader = rosbag2_py.SequentialReader() + reader.open( + rosbag2_py.StorageOptions(uri=str(args.bag), storage_id=""), + rosbag2_py.ConverterOptions(input_serialization_format="cdr", output_serialization_format="cdr"), + ) + + args.output.parent.mkdir(parents=True, exist_ok=True) + records = [] + pending = None + cycle = 0 + + def new_record(timestamp): + nonlocal cycle + cycle += 1 + return { + "schema_version": 1, + "source": "rosout", + "cycle": cycle, + "record_stamp_ns": timestamp, + "published": 0, + } + + def finish_pending(): + nonlocal pending + if pending is not None: + records.append(pending) + pending = None + + while reader.has_next(): + topic, serialized, bag_timestamp = reader.read_next() + if topic != "/rosout": + continue + message = deserialize_message(serialized, Log) + if args.node and args.node not in message.name: + continue + text = ANSI_ESCAPE.sub("", message.msg) + timestamp = stamp_to_nanoseconds(message, bag_timestamp) + + status_match = STATUS.search(text) or SOLVER_STATUS.search(text) + if "Optimization: SUCCESS!" in text or status_match: + finish_pending() + pending = new_record(timestamp) + pending["status"] = int(status_match.group(1)) if status_match else 0 + continue + + duration_match = DURATION.search(text) + if duration_match: + if pending is None: + pending = new_record(timestamp) + pending["status"] = 0 + pending["record_stamp_ns"] = timestamp + pending["acados_total_ms"] = float(duration_match.group(1)) + pending["sqp_iter"] = int(duration_match.group(2)) + if duration_match.group(3) is not None: + pending["qp_iter"] = int(duration_match.group(3)) + pending["kkt"] = float(duration_match.group(4)) + continue + + cost_match = COST.search(text) + if cost_match and pending is not None: + pending["cost"] = float(cost_match.group(1)) + pending["nlp_res"] = float(cost_match.group(2)) + continue + + residual_match = RESIDUALS.search(text) + if residual_match and pending is not None: + pending["cost"] = float(residual_match.group(1)) + pending["res_stat"] = float(residual_match.group(2)) + pending["res_eq"] = float(residual_match.group(3)) + pending["res_ineq"] = float(residual_match.group(4)) + pending["res_comp"] = float(residual_match.group(5)) + continue + + if "Published trajectory" in text and pending is not None: + pending["published"] = 1 + finish_pending() + + finish_pending() + if not records: + raise SystemExit("No optimizer performance records found on /rosout.") + + with args.output.open("w", encoding="utf-8", newline="") as output: + writer = csv.DictWriter(output, fieldnames=CSV_FIELDS, extrasaction="ignore") + writer.writeheader() + writer.writerows(records) + + print(f"Wrote {len(records)} records to {args.output}") + + +if __name__ == "__main__": + main() diff --git a/docker/compose/docker-compose.yml b/docker/compose/docker-compose.yml index d6ba934..e35b799 100644 --- a/docker/compose/docker-compose.yml +++ b/docker/compose/docker-compose.yml @@ -1,7 +1,7 @@ services: trajectory-optimization: - image: ghcr.io/openads-project/trajectory_optimization:v1.2.0 + image: ghcr.io/openads-project/trajectory_optimization:v1.3.1 environment: # --- name ------ NAMESPACE: / diff --git a/docker/custom.sh b/docker/custom.sh index 02e5a24..1d4ecd6 100755 --- a/docker/custom.sh +++ b/docker/custom.sh @@ -1,7 +1,7 @@ # clone acados repo and build it git clone --recurse-submodules https://github.com/acados/acados.git /opt/acados cd /opt/acados -git checkout v0.5.1 +git checkout v0.5.5 git submodule update --init --recursive mkdir -p /opt/acados/build cd /opt/acados/build @@ -13,8 +13,7 @@ pip install -e /opt/acados/interfaces/acados_template --ignore-installed # install t_renderer rm -f /opt/acados/bin/t_renderer -curl -L -o /opt/acados/bin/t_renderer https://github.com/acados/tera_renderer/releases/download/v0.0.34/t_renderer-v0.0.34-linux -chmod +x /opt/acados/bin/t_renderer +ACADOS_SOURCE_DIR=/opt/acados python -c "from acados_template import get_tera; get_tera(force_download=True)" # write necessary environment variables to .bashrc echo "export ACADOS_SOURCE_DIR=/opt/acados" >> /root/.bashrc @@ -32,4 +31,4 @@ git clone --branch jazzy-ika https://github.com/RaphvK/ros2_tracing.git src/ros2 rosdep update && rosdep install -y -i --from-paths src/ros2_tracing/tracetools src/ros2_tracing/tracetools_launch source /opt/ros/${ROS_DISTRO}/setup.bash colcon build --packages-up-to tracetools tracetools_launch --allow-overriding tracetools --allow-overriding tracetools_launch -rm -r src/ros2_tracing log build \ No newline at end of file +rm -r src/ros2_tracing log build diff --git a/dummy_input_generation/package.xml b/dummy_input_generation/package.xml index 2b88900..0f9d95b 100644 --- a/dummy_input_generation/package.xml +++ b/dummy_input_generation/package.xml @@ -3,7 +3,7 @@ dummy_input_generation - 1.2.0 + 1.3.1 Generates and publishes dummy input data for testing purposes of the trajectory optimization node. Jean-Pierre Busch diff --git a/trajectory_optimization/CMakeLists.txt b/trajectory_optimization/CMakeLists.txt index df0d3ca..dced009 100644 --- a/trajectory_optimization/CMakeLists.txt +++ b/trajectory_optimization/CMakeLists.txt @@ -28,6 +28,7 @@ set(TARGET_NAME trajectory_optimization_node_component) add_library(${TARGET_NAME} SHARED src/callbacks.cpp + src/performance_logger.cpp src/trajectory_optimization_node.cpp src/trajectory_optimization_ackermann.cpp src/trajectory_optimization_rws.cpp diff --git a/trajectory_optimization/README.md b/trajectory_optimization/README.md index 0d52783..b28c2c9 100644 --- a/trajectory_optimization/README.md +++ b/trajectory_optimization/README.md @@ -51,6 +51,7 @@ flowchart LR | `n_shots` | `int` | `50` | Number of shooting intervals in optimization horizon | | `optimization_horizon` | `float` | `1.0` | Optimization Horizon in seconds | | `verbose` | `bool` | `false` | Print solver statistics | +| `performance_logging` | `bool` | `false` | Write one CSV record for every completed solver run | | `debug_visualization` | `bool` | `false` | Publish debug visualization markers (e.g. obstacle circles) | | `run_as_callback` | `bool` | `false` | Run OCP once for each received reference trajectory (true) or on a timer (false) | | `cost_weights` | `float[]` | `std::vector(12, 1.0)` | Cost function weights | @@ -69,7 +70,6 @@ flowchart LR | `bi_level_dA` | `float` | `2.0` | Threshold for bi-level stabilization: maximum acceleration difference [m/s^2] | | `bi_level_dY` | `float` | `0.1` | Threshold for bi-level stabilization: maximum y-offset [m] | | `bi_level_dYaw` | `float` | `5.0` | Threshold for bi-level stabilization: maximum yaw difference [degree] | -| `init_as_ref` | `bool` | `false` | Boolean that enables initialization of trajectory states as reference states under certain set of conditions | ## Launch Files diff --git a/trajectory_optimization/config/example_params_ackermann.yml b/trajectory_optimization/config/example_params_ackermann.yml index 4ee94f4..1e857e4 100644 --- a/trajectory_optimization/config/example_params_ackermann.yml +++ b/trajectory_optimization/config/example_params_ackermann.yml @@ -6,14 +6,14 @@ fixed_over_time_frame_id: map # Frame ID of frame that is fixed over time for finding temporal transforms ego_data_timeout: 1.0 # Time after which a received ego vehicle data is considered invalid [s]. Optimization will not be run if ego data is invalid. optimization_frequency: 10.0 # frequency of the optimization loop [Hz] - init_as_ref: False # initialize ocp solution using reference trajectory when no valid last solution is available (e.g. standstill situation) - standstill_threshold: 0.3 # threshold for standstill detection [m/s]. If the velocities of all states are below this threshold, publish standstill trajectory + standstill_threshold: 0.3 # threshold for standstill detection [m/s]. If all state velocities are below this threshold, publish standstill trajectory high_level_stabilization: False # init first trajectory point with: current EgoData (True); interpolation of last trajectory -> bi-level (False) add_x_init_to_ref: False # add initial state of OCP to beginning of reference trajectory if this starts in front of ego vehicle consider_objects: 2 # consider objects in optimization: 0 = no, 1 = static (no prediction), 2 = dynamic (with prediction) min_prediction_probability: 0.0 # consider only predictions with probability > this threshold consider_boundaries: 1 # consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space verbose: False # print debug infromation and solver statistics + performance_logging: False # write one CSV record for each completed solver run debug_visualization: True # publish debug visualization markers (e.g. obstacle circles) run_as_callback: False # run OCP once for each received reference trajectory (true) or on a timer (false) @@ -32,9 +32,9 @@ #### cost weights cost_weights: - 0.3 # dlat (0) - - 0.0 # psi (1) (considered if RWS model is used) - - 0.3 # overspeed (2) - - 0.1 # underspeed (3) + - 0.0 # psi (1) (considered if RWS model is used) + - 0.3 # overspeed (2) + - 0.1 # underspeed (3) - 0.2 # a_lat (4) - 0.2 # a_lon_pos (5) - 0.05 # a_lon_neg (6) diff --git a/trajectory_optimization/config/example_params_rws.yml b/trajectory_optimization/config/example_params_rws.yml index b360f7f..2194f3d 100644 --- a/trajectory_optimization/config/example_params_rws.yml +++ b/trajectory_optimization/config/example_params_rws.yml @@ -6,7 +6,6 @@ fixed_over_time_frame_id: map # Frame ID of frame that is fixed over time for finding temporal transforms ego_data_timeout: 1.0 # Time after which a received ego vehicle data is considered invalid [s]. Optimization will not be run if ego data is invalid. optimization_frequency: 10.0 # frequency of the optimization loop [Hz] - init_as_ref: False # initialize ocp solution using reference trajectory when no valid last solution is available (e.g. standstill situation) standstill_threshold: 0.3 # threshold for standstill detection [m/s]. If the velocities of all states are below this threshold, publish standstill trajectory high_level_stabilization: False # init first trajectory point with: current EgoData (True); interpolation of last trajectory -> bi-level (False) add_x_init_to_ref: False # add initial state of OCP to beginning of reference trajectory if this starts in front of ego vehicle @@ -14,6 +13,7 @@ min_prediction_probability: 0.0 # consider only predictions with probability > this threshold consider_boundaries: 1 # consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space verbose: False # print debug infromation and solver statistics + performance_logging: False # write one CSV record for each completed solver run debug_visualization: True # publish debug visualization markers (e.g. obstacle circles) run_as_callback: False # run OCP once for each received reference trajectory (true) or on a timer (false) diff --git a/trajectory_optimization/include/trajectory_optimization/ocp_model_handler.hpp b/trajectory_optimization/include/trajectory_optimization/ocp_model_handler.hpp index 9dedf10..a4b9446 100644 --- a/trajectory_optimization/include/trajectory_optimization/ocp_model_handler.hpp +++ b/trajectory_optimization/include/trajectory_optimization/ocp_model_handler.hpp @@ -3,6 +3,8 @@ #pragma once +#include + // NOLINTBEGIN(clang-diagnostic-gnu-zero-variadic-macro-arguments) // acados @@ -90,6 +92,44 @@ inline int acados_free(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_fre */ inline int acados_solve(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_solve); } +/** + * @brief Evaluates the explicit model dynamics for a shooting stage. + * + * @param[in] capsule Variant holding the model-specific acados capsule. + * @param[in] stage Shooting stage whose dynamics function is evaluated. + * @param[in] x State vector at which to evaluate the dynamics. + * @param[in] u Control vector at which to evaluate the dynamics. + * @param[out] x_dot Evaluated state derivative. + */ +inline void acados_evaluate_dynamics(ocp_model_capsule_t capsule, int stage, double* x, double* u, double* x_dot) { + std::array input_types = {COLMAJ, COLMAJ}; + std::array inputs = {x, u}; + std::array output_types = {COLMAJ}; + std::array outputs = {x_dot}; + + external_function_external_param_casadi* function = nullptr; + + if (std::holds_alternative(capsule)) { + function = &std::get(capsule)->expl_ode_fun[stage]; // NOLINT + } else if (std::holds_alternative(capsule)) { + function = &std::get(capsule)->expl_ode_fun[stage]; // NOLINT + } else { + throw std::invalid_argument("Invalid capsule type."); + } + function->evaluate(function, input_types.data(), inputs.data(), output_types.data(), outputs.data()); +} + +/** + * @brief Resets selected solver memory without freeing and recreating the capsule. + */ +inline int acados_reset(ocp_model_capsule_t capsule, + int reset_qp_solver_mem, + int reset_numerical_values, + int reset_solver_options, + int reset_x_to_x0_bar) { + ACADOS_DISPATCH(acados_reset, reset_qp_solver_mem, reset_numerical_values, reset_solver_options, reset_x_to_x0_bar); +} + /** * @brief Wrapper around the generated sparse parameter update function. * diff --git a/trajectory_optimization/include/trajectory_optimization/performance_logger.hpp b/trajectory_optimization/include/trajectory_optimization/performance_logger.hpp new file mode 100644 index 0000000..9f56817 --- /dev/null +++ b/trajectory_optimization/include/trajectory_optimization/performance_logger.hpp @@ -0,0 +1,129 @@ +// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +#include + +namespace trajectory_optimization { + +struct PerformanceMetrics { + uint64_t cycle = 0; + int64_t ego_stamp_ns = 0; + int64_t reference_stamp_ns = 0; + int64_t route_stamp_ns = 0; + int status = 0; + int sqp_iter = 0; + int qp_iter = 0; + int qp_status = 0; + int reference_points = 0; + int objects = 0; + bool published = false; + + double cycle_ms = 0.0; + double preprocessing_ms = 0.0; + double solve_wall_ms = 0.0; + double postprocessing_ms = 0.0; + double acados_total_ms = 0.0; + double acados_lin_ms = 0.0; + double acados_sim_ms = 0.0; + double acados_qp_ms = 0.0; + double acados_qp_solver_ms = 0.0; + double acados_qp_xcond_ms = 0.0; + double acados_reg_ms = 0.0; + double acados_glob_ms = 0.0; + double acados_preparation_ms = 0.0; + double acados_feedback_ms = 0.0; + + double cost_value = 0.0; + double nlp_res = 0.0; + double kkt_norm_inf = 0.0; + double res_stat = 0.0; + double res_eq = 0.0; + double res_ineq = 0.0; + double res_comp = 0.0; +}; + +class PerformanceLogger { + public: + /** + * @brief Creates a CSV performance log for the given node. + * + * @param[in] node_name Node name used as part of the log file name. + */ + explicit PerformanceLogger(const std::string& node_name); + + /** + * @brief Flushes and closes the performance log. + */ + ~PerformanceLogger(); + + /** + * @brief Copy construction is disabled because the logger owns a file stream. + */ + PerformanceLogger(const PerformanceLogger&) = delete; + + /** + * @brief Copy assignment is disabled because the logger owns a file stream. + * + * @return Reference to this logger. The operator is deleted. + */ + PerformanceLogger& operator=(const PerformanceLogger&) = delete; + + /** + * @brief Move construction is disabled to keep the log stream bound to one logger. + */ + PerformanceLogger(PerformanceLogger&&) = delete; + + /** + * @brief Move assignment is disabled to keep the log stream bound to one logger. + * + * @return Reference to this logger. The operator is deleted. + */ + PerformanceLogger& operator=(PerformanceLogger&&) = delete; + + /** + * @brief Appends one set of performance metrics to the CSV log. + * + * @param[in] metrics Metrics collected for one optimization cycle. + */ + void write(const PerformanceMetrics& metrics); + + /** + * @brief Reads timing, iteration, cost, and residual statistics from acados. + * + * @param[out] metrics Metrics structure populated with the solver statistics. + * @param[in] solver acados NLP solver instance. + * @param[in] config acados NLP configuration. + * @param[in] dims acados NLP dimensions. + * @param[in] input acados NLP input. + * @param[in] output acados NLP output. + */ + static void collectSolverStatistics(PerformanceMetrics& metrics, + ocp_nlp_solver* solver, + ocp_nlp_config* config, + ocp_nlp_dims* dims, + ocp_nlp_in* input, + ocp_nlp_out* output); + + /** + * @brief Returns the path of the CSV performance log. + * + * @return Path of the active performance log file. + */ + const std::filesystem::path& path() const { return path_; } + + private: + static constexpr uint64_t FLUSH_INTERVAL = 100; + + std::filesystem::path path_; + std::ofstream stream_; + uint64_t records_since_flush_ = 0; +}; + +} // namespace trajectory_optimization diff --git a/trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp b/trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp index f3b1650..4c40cef 100644 --- a/trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp +++ b/trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp @@ -31,6 +31,7 @@ // acados #include +#include namespace trajectory_optimization { @@ -126,10 +127,19 @@ class TrajectoryOptimizationNode : public rclcpp::Node { void setup(); /** - * @brief Recreates the optimizer after releasing the current solver instance. + * @brief Resets the optimizer memory while retaining the generated solver instance. */ void resetSolver(); + /** + * @brief Builds and sets a dynamically consistent NLP initial guess from the current state and cached controls. + * + * @param[in] x_init Hard initial state of the OCP. + * @param[in] stamp Absolute time corresponding to x_init. + * @return `true` if the state rollout succeeded. + */ + bool setInitialGuess(const std::vector& x_init, const rclcpp::Time& stamp); + /** * @brief Creates the acados solver instance and initializes its state buffers. */ @@ -141,11 +151,18 @@ class TrajectoryOptimizationNode : public rclcpp::Node { void freeSolver(); /** - * @brief Logs solver status, timing and optional debug statistics for the last optimization run. + * @brief Logs solver status and optional debug statistics for the last optimization run. * - * @param[in] status acados solver status code. + * @param[in] metrics Performance metrics collected for the optimization run. */ - void printSolution(int status); + void printSolution(const PerformanceMetrics& metrics); + + /** + * @brief Emits one machine-readable performance record when performance logging is enabled. + * + * @param[in] metrics Performance metrics to write to the log. + */ + void logPerformance(const PerformanceMetrics& metrics); /** * @brief Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required. @@ -296,15 +313,13 @@ class TrajectoryOptimizationNode : public rclcpp::Node { void vizEgoCircles(const std::vector& x_trajectory, const std::string& model_name); /** - * @brief Publishes boundary or boundary-intersection points for debugging. + * @brief Publishes the boundary intersections corresponding to the distances passed to the OCP. * * @param[in] left_boundary_points Points on the left side. * @param[in] right_boundary_points Points on the right side. - * @param[in] is_intersection Whether the supplied points represent normal intersections instead of raw boundaries. */ void vizBoundaryPoints(const std::vector& left_boundary_points, - const std::vector& right_boundary_points, - bool is_intersection = false); + const std::vector& right_boundary_points); // virtual functions need to be implemented in derived classes @@ -380,13 +395,13 @@ class TrajectoryOptimizationNode : public rclcpp::Node { int n_shots_ = 50; double optimization_horizon_ = 1.0; bool verbose_ = false; + bool performance_logging_ = false; bool debug_viz_ = false; double standstill_threshold_ = 0.45; bool high_level_stabilization_ = false; bool add_x_init_to_ref_ = false; uint8_t consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS; uint8_t consider_boundaries_ = CONSIDER_BOUNDARIES::SUGGESTED_LANE; - bool init_as_ref_ = false; bool run_as_callback_ = false; // common bi-level thresholds @@ -398,6 +413,10 @@ class TrajectoryOptimizationNode : public rclcpp::Node { // latest valid trajectory trajectory_planning_msgs::msg::Trajectory latest_valid_trajectory_; + // controls of the latest accepted solution; an empty vector denotes that no warm start is available + std::vector control_guess_; + rclcpp::Time control_guess_stamp_{0, 0, RCL_ROS_TIME}; + // visualization std::vector viz_circles_; @@ -427,6 +446,8 @@ class TrajectoryOptimizationNode : public rclcpp::Node { std::vector xtraj_; std::vector utraj_; + uint64_t logging_cycle_ = 0; + std::unique_ptr performance_logger_; }; } // namespace trajectory_optimization diff --git a/trajectory_optimization/package.xml b/trajectory_optimization/package.xml index 1681c43..c69fa87 100644 --- a/trajectory_optimization/package.xml +++ b/trajectory_optimization/package.xml @@ -3,7 +3,7 @@ trajectory_optimization - 1.2.0 + 1.3.1 Periodically solves a nonlinear OCP to generate optimized trajectories for automated driving. Jean-Pierre Busch diff --git a/trajectory_optimization/src/performance_logger.cpp b/trajectory_optimization/src/performance_logger.cpp new file mode 100644 index 0000000..fa7d97d --- /dev/null +++ b/trajectory_optimization/src/performance_logger.cpp @@ -0,0 +1,122 @@ +// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace trajectory_optimization { + +namespace { + +std::string timestamp() { + const auto now = std::chrono::system_clock::now(); + const auto time = std::chrono::system_clock::to_time_t(now); + std::tm utc_time{}; + gmtime_r(&time, &utc_time); + const auto milliseconds = std::chrono::duration_cast(now.time_since_epoch()).count() % 1000; + + std::ostringstream value; + value << std::put_time(&utc_time, "%Y%m%dT%H%M%S") << '_' << std::setfill('0') << std::setw(3) << milliseconds << 'Z'; + return value.str(); +} + +int64_t nowNanoseconds() { + return std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); +} + +} // namespace + +PerformanceLogger::PerformanceLogger(const std::string& node_name) { + const char* configured_directory = std::getenv("TRAJECTORY_OPTIMIZATION_BENCHMARK_DIR"); + const std::filesystem::path directory = configured_directory != nullptr && *configured_directory != '\0' + ? std::filesystem::path(configured_directory) + : std::filesystem::temp_directory_path() / "trajectory_optimization_benchmarks"; + path_ = directory / (node_name + '_' + timestamp() + ".csv"); + + std::error_code error; + std::filesystem::create_directories(directory, error); + if (error) { + throw std::runtime_error("could not create directory '" + directory.string() + "': " + error.message()); + } + + stream_.open(path_, std::ios::out | std::ios::trunc); + if (!stream_) { + throw std::runtime_error("could not open '" + path_.string() + "'"); + } + stream_ << "schema_version,source,run_id,cycle,record_stamp_ns,ego_stamp_ns,reference_stamp_ns,route_stamp_ns,status," + "published,ref_points,objects,sqp_iter,qp_iter," + "qp_status,cycle_ms,preprocessing_ms,solve_wall_ms,postprocessing_ms,acados_total_ms,acados_lin_ms," + "acados_sim_ms,acados_qp_ms," + "acados_qp_solver_ms,acados_qp_xcond_ms,acados_reg_ms,acados_glob_ms,acados_preparation_ms," + "acados_feedback_ms,cost,kkt,nlp_res,res_stat,res_eq,res_ineq,res_comp\n"; + stream_.flush(); +} + +PerformanceLogger::~PerformanceLogger() { + stream_.flush(); + stream_.close(); +} + +void PerformanceLogger::collectSolverStatistics(PerformanceMetrics& metrics, + ocp_nlp_solver* solver, + ocp_nlp_config* config, + ocp_nlp_dims* dims, + ocp_nlp_in* input, + ocp_nlp_out* output) { + auto readTime = [&](const char* field, double& destination_ms) { + double time_seconds = 0.0; + ocp_nlp_get(solver, field, &time_seconds); + destination_ms = time_seconds * 1000.0; + }; + + readTime("time_tot", metrics.acados_total_ms); + readTime("time_lin", metrics.acados_lin_ms); + readTime("time_sim", metrics.acados_sim_ms); + readTime("time_qp", metrics.acados_qp_ms); + readTime("time_qp_solver_call", metrics.acados_qp_solver_ms); + readTime("time_qp_xcond", metrics.acados_qp_xcond_ms); + readTime("time_reg", metrics.acados_reg_ms); + readTime("time_glob", metrics.acados_glob_ms); + readTime("time_preparation", metrics.acados_preparation_ms); + readTime("time_feedback", metrics.acados_feedback_ms); + ocp_nlp_get(solver, "sqp_iter", &metrics.sqp_iter); + ocp_nlp_get(solver, "qp_iter", &metrics.qp_iter); + ocp_nlp_get(solver, "qp_status", &metrics.qp_status); + ocp_nlp_out_get(config, dims, output, 0, "kkt_norm_inf", &metrics.kkt_norm_inf); + + ocp_nlp_eval_cost(solver, input, output); + ocp_nlp_eval_residuals(solver, input, output); + ocp_nlp_get(solver, "cost_value", &metrics.cost_value); + ocp_nlp_get(solver, "res_stat", &metrics.res_stat); + ocp_nlp_get(solver, "res_eq", &metrics.res_eq); + ocp_nlp_get(solver, "res_ineq", &metrics.res_ineq); + ocp_nlp_get(solver, "res_comp", &metrics.res_comp); + metrics.nlp_res = std::max({metrics.res_stat, metrics.res_eq, metrics.res_ineq, metrics.res_comp}); +} + +void PerformanceLogger::write(const PerformanceMetrics& metrics) { + stream_ << std::setprecision(17) << 4 << ",runtime,," << metrics.cycle << ',' << nowNanoseconds() << ',' << metrics.ego_stamp_ns + << ',' << metrics.reference_stamp_ns << ',' << metrics.route_stamp_ns << ',' << metrics.status << ',' + << (metrics.published ? 1 : 0) << ',' << metrics.reference_points << ',' << metrics.objects << ',' << metrics.sqp_iter + << ',' << metrics.qp_iter << ',' << metrics.qp_status << ',' << metrics.cycle_ms << ',' << metrics.preprocessing_ms + << ',' << metrics.solve_wall_ms << ',' << metrics.postprocessing_ms << ',' << metrics.acados_total_ms << ',' + << metrics.acados_lin_ms << ',' << metrics.acados_sim_ms << ',' << metrics.acados_qp_ms << ',' + << metrics.acados_qp_solver_ms << ',' << metrics.acados_qp_xcond_ms << ',' << metrics.acados_reg_ms << ',' + << metrics.acados_glob_ms << ',' << metrics.acados_preparation_ms << ',' << metrics.acados_feedback_ms << ',' + << metrics.cost_value << ',' << metrics.kkt_norm_inf << ',' << metrics.nlp_res << ',' << metrics.res_stat << ',' + << metrics.res_eq << ',' << metrics.res_ineq << ',' << metrics.res_comp << '\n'; + + if (++records_since_flush_ >= FLUSH_INTERVAL) { + stream_.flush(); + records_since_flush_ = 0; + } +} + +} // namespace trajectory_optimization diff --git a/trajectory_optimization/src/trajectory_optimization_node.cpp b/trajectory_optimization/src/trajectory_optimization_node.cpp index 5ca6508..422ae20 100644 --- a/trajectory_optimization/src/trajectory_optimization_node.cpp +++ b/trajectory_optimization/src/trajectory_optimization_node.cpp @@ -14,6 +14,19 @@ */ namespace trajectory_optimization { +namespace { +using SteadyClock = std::chrono::steady_clock; + +double elapsedMilliseconds(const SteadyClock::time_point& start) { + return std::chrono::duration(SteadyClock::now() - start).count(); +} + +double elapsedMilliseconds(const SteadyClock::time_point& start, const SteadyClock::time_point& end) { + return std::chrono::duration(end - start).count(); +} + +} // namespace + TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_name, const rclcpp::NodeOptions& options) : rclcpp::Node(node_name, options) { // declare and load node parameters @@ -31,6 +44,8 @@ TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_na this->declareAndLoadParameter("n_shots", n_shots_, "Number of shooting intervals in optimization horizon"); this->declareAndLoadParameter("optimization_horizon", optimization_horizon_, "Optimization Horizon in seconds"); this->declareAndLoadParameter("verbose", verbose_, "Print solver statistics"); + this->declareAndLoadParameter("performance_logging", performance_logging_, + "Write one CSV record for every completed solver run", false, false, true); this->declareAndLoadParameter("debug_visualization", debug_viz_, "Publish debug visualization markers (e.g. obstacle circles)"); this->declareAndLoadParameter("run_as_callback", run_as_callback_, "Run OCP once for each received reference trajectory (true) or on a timer (false)"); @@ -66,9 +81,14 @@ TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_na this->declareAndLoadParameter("bi_level_dY", bi_level_dY_, "Threshold for bi-level stabilization: maximum y-offset [m]"); this->declareAndLoadParameter("bi_level_dYaw", bi_level_dYaw_, "Threshold for bi-level stabilization: maximum yaw difference [degree]"); - this->declareAndLoadParameter( - "init_as_ref", init_as_ref_, - "Boolean that enables initialization of trajectory states as reference states under certain set of conditions"); + if (performance_logging_) { + try { + performance_logger_ = std::make_unique(get_name()); + RCLCPP_INFO(get_logger(), "Writing benchmark logs to '%s'.", performance_logger_->path().c_str()); + } catch (const std::exception& error) { + RCLCPP_ERROR(get_logger(), "Could not initialize benchmark logging: %s", error.what()); + } + } this->setup(); } @@ -249,23 +269,21 @@ void TrajectoryOptimizationNode::setup() { } void TrajectoryOptimizationNode::setupSolver() { - // setup acados solver - ocp_capsule_ = trajectory_optimization::acados_create_capsule(model_name_); - int status = trajectory_optimization::acados_create(ocp_capsule_); - nlp_dims_ = trajectory_optimization::acados_get_nlp_dims(ocp_capsule_); if (n_shots_ <= 0) { RCLCPP_FATAL(this->get_logger(), "n_shots must be > 0, got %d", n_shots_); exit(1); - } else if (n_shots_ != nlp_dims_->N) { - std::vector new_time_steps(n_shots_, optimization_horizon_ / n_shots_); - RCLCPP_INFO(this->get_logger(), "Recreate OCP with: horizon = %f, n_shots = %d, dt = %f", optimization_horizon_, n_shots_, - new_time_steps.front()); - status = trajectory_optimization::acados_create_with_discretization(ocp_capsule_, n_shots_, new_time_steps.data()); } - if (status != 0) { - RCLCPP_INFO(this->get_logger(), "%s_acados_create_with_discretization() returned status %d. Exiting.", model_name_.c_str(), - status); + // Create exactly once. This also supports a runtime horizon that differs from the generated default. + ocp_capsule_ = trajectory_optimization::acados_create_capsule(model_name_); + std::vector new_time_steps(n_shots_, optimization_horizon_ / n_shots_); + RCLCPP_INFO(this->get_logger(), "Create OCP with: horizon = %f, n_shots = %d, dt = %f", optimization_horizon_, n_shots_, + new_time_steps.front()); + int status = trajectory_optimization::acados_create_with_discretization(ocp_capsule_, n_shots_, new_time_steps.data()); + + if (status != ACADOS_SUCCESS) { + RCLCPP_FATAL(this->get_logger(), "%s_acados_create_with_discretization() returned status %d. Exiting.", model_name_.c_str(), + status); exit(1); } @@ -275,20 +293,14 @@ void TrajectoryOptimizationNode::setupSolver() { nlp_out_ = trajectory_optimization::acados_get_nlp_out(ocp_capsule_); nlp_solver_ = trajectory_optimization::acados_get_nlp_solver(ocp_capsule_); nlp_opts_ = trajectory_optimization::acados_get_nlp_opts(ocp_capsule_); - - // initialization of state and control values; set all to zero - std::vector x_init(*nlp_dims_->nx, 0.0); - std::vector u_init(*nlp_dims_->nu, 0.0); - - // initialize solution - for (int i = 0; i < n_shots_; ++i) { - ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, i, "x", x_init.data()); - ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, i, "u", u_init.data()); + if (nlp_dims_->N != n_shots_) { + RCLCPP_FATAL(this->get_logger(), "Created solver has N=%d, expected n_shots=%d. Exiting.", nlp_dims_->N, n_shots_); + exit(1); } - ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, n_shots_, "x", x_init.data()); xtraj_.resize(*nlp_dims_->nx * (n_shots_ + 1)); utraj_.resize(*nlp_dims_->nu * n_shots_); + control_guess_.clear(); } void TrajectoryOptimizationNode::freeSolver() { @@ -305,11 +317,76 @@ void TrajectoryOptimizationNode::freeSolver() { } void TrajectoryOptimizationNode::resetSolver() { - freeSolver(); - setupSolver(); + const int status = trajectory_optimization::acados_reset(ocp_capsule_, 1, 0, 0, 0); + if (status != ACADOS_SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "%s_acados_reset() returned status %d. Recreating solver.", model_name_.c_str(), status); + freeSolver(); + setupSolver(); + } +} + +bool TrajectoryOptimizationNode::setInitialGuess(const std::vector& x_init, const rclcpp::Time& stamp) { + constexpr double MAX_CONTROL_GUESS_AGE_FACTOR = 0.5; + const int nx = *nlp_dims_->nx; + const int nu = *nlp_dims_->nu; + const double time_step = optimization_horizon_ / n_shots_; + const size_t expected_control_size = static_cast(nu * n_shots_); + if (x_init.size() != static_cast(nx)) { + RCLCPP_ERROR(get_logger(), "Initial state has size %zu, expected %d.", x_init.size(), nx); + return false; + } + + // Fall back to zero controls if no sufficiently recent accepted solution is available. + std::vector controls(expected_control_size, 0.0); + if (control_guess_.size() == expected_control_size) { + const double elapsed = (stamp - control_guess_stamp_).seconds(); + if (elapsed >= 0.0 && elapsed < MAX_CONTROL_GUESS_AGE_FACTOR * optimization_horizon_) { + // Shift the previous controls to the current planning time and interpolate between shooting nodes. + for (int stage = 0; stage < n_shots_; ++stage) { + const double previous_stage = (elapsed + stage * time_step) / time_step; + if (previous_stage >= n_shots_) break; + + const int lower_stage = static_cast(std::floor(previous_stage)); + const double interpolation_factor = previous_stage - lower_stage; + for (int control = 0; control < nu; ++control) { + const double lower_value = control_guess_[lower_stage * nu + control]; + const double upper_value = lower_stage + 1 < n_shots_ ? control_guess_[(lower_stage + 1) * nu + control] : 0.0; + controls[stage * nu + control] = lower_value + interpolation_factor * (upper_value - lower_value); + } + } + } + } + + ocp_nlp_out_set_values_to_zero(nlp_config_, nlp_dims_, nlp_out_); + std::vector rollout_state = x_init; + std::vector intermediate_state(nx); + std::vector k1(nx), k2(nx), k3(nx), k4(nx); + const double integration_step = time_step / 2.0; + for (int stage = 0; stage < n_shots_; ++stage) { + double* control = &controls[stage * nu]; + ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "x", rollout_state.data()); + ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "u", control); + + // Match sim_method_num_stages=4 and sim_method_num_steps=2 from the generated OCP. + for (int integration = 0; integration < 2; ++integration) { + trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, rollout_state.data(), control, k1.data()); + for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k1[i]; + trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k2.data()); + for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k2[i]; + trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k3.data()); + for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + integration_step * k3[i]; + trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k4.data()); + for (int i = 0; i < nx; ++i) { + rollout_state[i] += integration_step / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]); + } + } + } + ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, n_shots_, "x", rollout_state.data()); + return true; } void TrajectoryOptimizationNode::planningCycle() { + const auto cycle_start = SteadyClock::now(); if (debug_viz_) viz_circles_.clear(); if (rclcpp::Time(this->now()) - rclcpp::Time(ego_data_.header.stamp) > rclcpp::Duration::from_seconds(ego_data_timeout_)) { RCLCPP_WARN(this->get_logger(), "EgoData outdated. Skipping planning cycle."); @@ -335,10 +412,27 @@ void TrajectoryOptimizationNode::planningCycle() { } trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, true); trajectory_pub_->publish(std::move(trajectory)); - resetSolver(); + // Invalidate the warm start and reset the solver once when entering standstill. + if (!control_guess_.empty()) { + control_guess_.clear(); + resetSolver(); + } return; } + PerformanceMetrics metrics; + metrics.cycle = ++logging_cycle_; + metrics.ego_stamp_ns = rclcpp::Time(ego_data_.header.stamp).nanoseconds(); + metrics.reference_stamp_ns = rclcpp::Time(reference_trajectory_.header.stamp).nanoseconds(); + metrics.route_stamp_ns = rclcpp::Time(route_.header.stamp).nanoseconds(); + metrics.reference_points = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory_); + metrics.objects = static_cast(object_list_.objects.size()); + auto logCompletedCycle = [&]() { + metrics.cycle_ms = elapsedMilliseconds(cycle_start); + metrics.postprocessing_ms = metrics.cycle_ms - metrics.preprocessing_ms - metrics.solve_wall_ms; + logPerformance(metrics); + }; + // set initial state std::vector x_init(*nlp_dims_->nx, 0.0); if (!trajectory_planning_msgs::trajectory_access::getStandstill(latest_valid_trajectory_)) { @@ -364,8 +458,27 @@ void TrajectoryOptimizationNode::planningCycle() { return; } + if (!setInitialGuess(x_init, rclcpp::Time(ego_data_.header.stamp))) { + control_guess_.clear(); + return; + } + // solve the optimization problem - int status = trajectory_optimization::acados_solve(ocp_capsule_); + const auto solve_start = SteadyClock::now(); + metrics.preprocessing_ms = elapsedMilliseconds(cycle_start, solve_start); + metrics.status = trajectory_optimization::acados_solve(ocp_capsule_); + const auto solve_end = SteadyClock::now(); + metrics.solve_wall_ms = elapsedMilliseconds(solve_start, solve_end); + + PerformanceLogger::collectSolverStatistics(metrics, nlp_solver_, nlp_config_, nlp_dims_, nlp_in_, nlp_out_); + + if (metrics.status == ACADOS_NAN_DETECTED || metrics.status == ACADOS_MINSTEP || metrics.status == ACADOS_QP_FAILURE) { + printSolution(metrics); + // Keep the last accepted controls; the failed solver output is not added to the cache. + resetSolver(); + logCompletedCycle(); + return; + } // get solution for (int ii = 0; ii <= nlp_dims_->N; ++ii) { @@ -374,19 +487,15 @@ void TrajectoryOptimizationNode::planningCycle() { for (int ii = 0; ii < nlp_dims_->N; ++ii) { ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, ii, "u", &utraj_[ii * *nlp_dims_->nu]); } + control_guess_ = utraj_; + control_guess_stamp_ = rclcpp::Time(ego_data_.header.stamp); - printSolution(status); + printSolution(metrics); if (debug_viz_) { vizCircles(viz_circles_); vizEgoCircles(xtraj_, model_name_); } - if (status == 1 || status == 3 || status == 4) { - RCLCPP_ERROR(this->get_logger(), "Solver failed with status %d.", status); - resetSolver(); - return; - } - // convert output into trajectory message convertToTrajectoryMsg(*trajectory); @@ -395,18 +504,19 @@ void TrajectoryOptimizationNode::planningCycle() { if (trajectory_planning_msgs::trajectory_access::getV(*trajectory, i) > standstill_threshold_) standstill = false; } trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, standstill); - if (standstill) { - resetSolver(); - } // transform trajectory to output frame if (!trajectory2outputFrame(*trajectory)) { + logCompletedCycle(); return; } latest_valid_trajectory_ = *trajectory; trajectory_pub_->publish(std::move(trajectory)); - RCLCPP_INFO(this->get_logger(), "Published trajectory"); + metrics.published = true; + logCompletedCycle(); + const char* cycle_time_color = metrics.cycle_ms <= 100.0 ? "\x1b[32m" : "\x1b[31m"; + RCLCPP_INFO(this->get_logger(), "Published trajectory (cycle: %s%.2f ms\x1b[0m)", cycle_time_color, metrics.cycle_ms); } bool TrajectoryOptimizationNode::updateOcpInputs(const perception_msgs::msg::EgoData& ego_data, @@ -448,17 +558,9 @@ bool TrajectoryOptimizationNode::updateOcpInputs(const perception_msgs::msg::Ego return false; } - if (init_as_ref_ && trajectory_planning_msgs::trajectory_access::getStandstill(latest_valid_trajectory_)) { - // set initial guess - std::vector initial_guess(*nlp_dims_->nx, 0.0); - for (int i = 0; i <= n_shots_; ++i) { - int idx = std::min(i, trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_reference_trajectory) - 1); - initial_guess[0] = trajectory_planning_msgs::trajectory_access::getX(tf_reference_trajectory, idx); - initial_guess[1] = trajectory_planning_msgs::trajectory_access::getY(tf_reference_trajectory, idx); - initial_guess[3] = trajectory_planning_msgs::trajectory_access::getV(tf_reference_trajectory, idx); - initial_guess[5] = trajectory_planning_msgs::trajectory_access::getTheta(tf_reference_trajectory, idx); - ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, i, "x", initial_guess.data()); - } + if (trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_reference_trajectory) <= 0) { + RCLCPP_ERROR(this->get_logger(), "Reference trajectory contains no sample points."); + return false; } // update ocp parameters @@ -511,9 +613,13 @@ void TrajectoryOptimizationNode::setOcpGlobalParameters(const std::vector::difference_type>(n_ref_states)); } else { - // TODO: what to do here? Currently just copy the whole reference trajectory and rest is filled with infinity // NOLINT(google-readability-todo) + // Repeat the final valid state. Infinity padding can propagate NaNs through closest-point calculations. global_params.insert(global_params.end(), ref.begin(), ref.end()); - global_params.insert(global_params.end(), n_ref_states - ref.size(), std::numeric_limits::infinity()); + const size_t state_width = static_cast(p_ref_path_shape_[1]); + while (global_params.size() < expected_cost_weights_size + 4 + n_ref_states) { + global_params.insert(global_params.end(), ref.end() - static_cast::difference_type>(state_width), + ref.end()); + } } if (global_params.size() != static_cast(nlp_dims_->np_global)) { @@ -521,8 +627,11 @@ void TrajectoryOptimizationNode::setOcpGlobalParameters(const std::vectornp_global); throw std::runtime_error("Size of global parameters does not match expected size."); } - trajectory_optimization::acados_set_p_global_and_precompute_dependencies(ocp_capsule_, global_params.data(), - static_cast(global_params.size())); + const int status = trajectory_optimization::acados_set_p_global_and_precompute_dependencies( + ocp_capsule_, global_params.data(), static_cast(global_params.size())); + if (status != ACADOS_SUCCESS) { + throw std::runtime_error("acados global parameter update failed with status " + std::to_string(status)); + } const auto elapsed_ms = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); RCLCPP_DEBUG(this->get_logger(), "setOcpGlobalParameters duration: %.3f ms", elapsed_ms); } @@ -540,7 +649,11 @@ void TrajectoryOptimizationNode::setOcpParameters(const perception_msgs::msg::Eg std::vector idx_dynamic_weight(n); // fill vector with values from idx to idx + n std::iota(idx_dynamic_weight.begin(), idx_dynamic_weight.end(), idx); - trajectory_optimization::acados_update_params_sparse(ocp_capsule_, i, idx_dynamic_weight.data(), &floating_dynamic_weight, n); + int status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, i, idx_dynamic_weight.data(), + &floating_dynamic_weight, n); + if (status != ACADOS_SUCCESS) { + throw std::runtime_error("acados dynamic-weight update failed with status " + std::to_string(status)); + } floating_dynamic_weight *= dynamic_weight_; // obstacles @@ -551,7 +664,7 @@ void TrajectoryOptimizationNode::setOcpParameters(const perception_msgs::msg::Eg for (size_t j = 0; j < object_list.objects.size(); ++j) { std::vector TIME, X, Y, YAW; std::vector> target_states; - // TODO: should not be done for each shooting interval. Could be improved. // NOLINT(google-readability-todo) + // TODO(ika): Build prediction arrays once per object outside the shooting-interval loop. TIME.push_back(static_cast(rclcpp::Time(object_list.header.stamp).nanoseconds()) / 1e9); X.push_back(perception_msgs::object_access::getX(object_list.objects[j])); Y.push_back(perception_msgs::object_access::getY(object_list.objects[j])); @@ -649,7 +762,10 @@ void TrajectoryOptimizationNode::setOcpParameters(const perception_msgs::msg::Eg std::vector idx_obstacles(n); // fill vector with values from idx to idx + n std::iota(idx_obstacles.begin(), idx_obstacles.end(), idx); - trajectory_optimization::acados_update_params_sparse(ocp_capsule_, i, idx_obstacles.data(), circles.data(), n); + status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, i, idx_obstacles.data(), circles.data(), n); + if (status != ACADOS_SUCCESS) { + throw std::runtime_error("acados obstacle update failed with status " + std::to_string(status)); + } } const auto elapsed_ms = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); RCLCPP_DEBUG(this->get_logger(), "setOcpParameters duration: %.3f ms", elapsed_ms); diff --git a/trajectory_optimization/src/utils.cpp b/trajectory_optimization/src/utils.cpp index 0344e4d..35857f2 100644 --- a/trajectory_optimization/src/utils.cpp +++ b/trajectory_optimization/src/utils.cpp @@ -1,6 +1,7 @@ // Copyright Institute for Automotive Engineering (ika), RWTH Aachen University // SPDX-License-Identifier: Apache-2.0 +#include #include #include @@ -153,11 +154,14 @@ std::vector TrajectoryOptimizationNode::discretizeBB2Circles( std::vector> TrajectoryOptimizationNode::normalBoundaryDistance( const trajectory_planning_msgs::msg::Trajectory& reference_trajectory, const route_planning_msgs::msg::Route& route) { const double NO_BOUNDARY_DISTANCE = 1e6; // should be smaller than MAX_BOUNDARY_CONSTRAINT from ocp + constexpr double MAX_ROUTE_S_DIFFERENCE = 20.0; struct Boundaries { std::vector> min_normal_distances; std::vector left_boundary_points; std::vector right_boundary_points; + std::vector left_boundary_route_s; + std::vector right_boundary_route_s; std::vector left_boundary_intersections; std::vector right_boundary_intersections; }; @@ -167,12 +171,16 @@ std::vector> TrajectoryOptimizationNode::normalBoundar const int ref_sample_size = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory); boundaries.left_boundary_points.reserve(remaining_route.size()); boundaries.right_boundary_points.reserve(remaining_route.size()); + boundaries.left_boundary_route_s.reserve(remaining_route.size()); + boundaries.right_boundary_route_s.reserve(remaining_route.size()); boundaries.min_normal_distances.reserve(ref_sample_size); boundaries.left_boundary_intersections.reserve(ref_sample_size); boundaries.right_boundary_intersections.reserve(ref_sample_size); if (remaining_route.empty()) { - RCLCPP_WARN(get_logger(), "Remaining route is empty. Do not constrain boundaries."); + if (consider_boundaries_ != CONSIDER_BOUNDARIES::NO_BOUNDS) { + RCLCPP_WARN(get_logger(), "Remaining route is empty. Do not constrain boundaries."); + } for (int i = 0; i < ref_sample_size; ++i) { boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE); } @@ -187,6 +195,8 @@ std::vector> TrajectoryOptimizationNode::normalBoundar boundaries.left_boundary_points.emplace_back(suggested_lane.left_boundary.point.x, suggested_lane.left_boundary.point.y); boundaries.right_boundary_points.emplace_back(suggested_lane.right_boundary.point.x, suggested_lane.right_boundary.point.y); + boundaries.left_boundary_route_s.push_back(route_element.s); + boundaries.right_boundary_route_s.push_back(route_element.s); } else if (consider_boundaries_ == CONSIDER_BOUNDARIES::INCLUDING_ADJACENT) { const auto& lane_elements = route_element.lane_elements; if (!lane_elements.empty()) { @@ -194,21 +204,26 @@ std::vector> TrajectoryOptimizationNode::normalBoundar lane_elements.front().left_boundary.point.y); boundaries.right_boundary_points.emplace_back(lane_elements.back().right_boundary.point.x, lane_elements.back().right_boundary.point.y); + boundaries.left_boundary_route_s.push_back(route_element.s); + boundaries.right_boundary_route_s.push_back(route_element.s); } } else if (consider_boundaries_ == CONSIDER_BOUNDARIES::DRIVABLE_SPACE) { boundaries.left_boundary_points.emplace_back(route_element.left_boundary.x, route_element.left_boundary.y); boundaries.right_boundary_points.emplace_back(route_element.right_boundary.x, route_element.right_boundary.y); + boundaries.left_boundary_route_s.push_back(route_element.s); + boundaries.right_boundary_route_s.push_back(route_element.s); } } } // Helper lambda to find intersection - auto findIntersection = [](const Eigen::Vector2d& ref_pos, double sin_yaw, double cos_yaw, - const std::vector& boundary_points, - bool isLeft) -> std::pair { + auto findIntersection = [&](const Eigen::Vector2d& ref_pos, double sin_yaw, double cos_yaw, double expected_route_s, + const std::vector& boundary_points, const std::vector& boundary_route_s, + bool isLeft) -> std::pair { std::pair intersection_result = { std::numeric_limits::infinity(), Eigen::Vector2d(std::numeric_limits::infinity(), std::numeric_limits::infinity())}; + double best_route_s_difference = std::numeric_limits::infinity(); const Eigen::Vector2d normal_dir = isLeft ? Eigen::Vector2d(-sin_yaw, cos_yaw) : Eigen::Vector2d(sin_yaw, -cos_yaw); const auto cross2d = [](const Eigen::Vector2d& u, const Eigen::Vector2d& v) { return u.x() * v.y() - u.y() * v.x(); }; @@ -229,10 +244,15 @@ std::vector> TrajectoryOptimizationNode::normalBoundar if (s >= 0.0 && s <= 1.0 && t >= 0.0) { Eigen::Vector2d intersection = a + s * seg; - double euklidean_distance = (ref_pos - intersection).norm(); - if (euklidean_distance < intersection_result.first) { - intersection_result.first = euklidean_distance; - intersection_result.second = intersection; + double euclidean_distance = (ref_pos - intersection).norm(); + const double intersection_route_s = boundary_route_s[i] + s * (boundary_route_s[i + 1] - boundary_route_s[i]); + const double route_s_difference = std::abs(intersection_route_s - expected_route_s); + + if (route_s_difference <= MAX_ROUTE_S_DIFFERENCE && + (route_s_difference < best_route_s_difference || + (std::abs(route_s_difference - best_route_s_difference) < 1e-9 && euclidean_distance < intersection_result.first))) { + best_route_s_difference = route_s_difference; + intersection_result = {euclidean_distance, intersection}; } } } @@ -240,15 +260,24 @@ std::vector> TrajectoryOptimizationNode::normalBoundar }; // Loop over trajectory points and compute intersections + double reference_progress = 0.0; + Eigen::Vector2d previous_ref_pos = Eigen::Vector2d::Zero(); + const double current_route_s = remaining_route.front().s; for (int i = 0; i < ref_sample_size; ++i) { Eigen::Vector2d ref_pos(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i), trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i)); + reference_progress += (ref_pos - previous_ref_pos).norm(); + previous_ref_pos = ref_pos; + const double expected_route_s = current_route_s + reference_progress; + double yaw = trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i); const double sin_yaw = std::sin(yaw); const double cos_yaw = std::cos(yaw); - auto left_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, boundaries.left_boundary_points, true); - auto right_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, boundaries.right_boundary_points, false); + auto left_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.left_boundary_points, + boundaries.left_boundary_route_s, true); + auto right_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.right_boundary_points, + boundaries.right_boundary_route_s, false); if (left_intersection.first != std::numeric_limits::infinity() && right_intersection.first != std::numeric_limits::infinity()) { boundaries.min_normal_distances.emplace_back(left_intersection.first, right_intersection.first); @@ -263,16 +292,14 @@ std::vector> TrajectoryOptimizationNode::normalBoundar } } if (debug_viz_) { - vizBoundaryPoints(boundaries.left_boundary_points, boundaries.right_boundary_points, false); - vizBoundaryPoints(boundaries.left_boundary_intersections, boundaries.right_boundary_intersections, true); + vizBoundaryPoints(boundaries.left_boundary_intersections, boundaries.right_boundary_intersections); } return boundaries.min_normal_distances; } void TrajectoryOptimizationNode::vizBoundaryPoints(const std::vector& left_boundary_points, - const std::vector& right_boundary_points, - bool is_intersection) { + const std::vector& right_boundary_points) { visualization_msgs::msg::MarkerArray marker_array; int id = 0; @@ -302,13 +329,8 @@ void TrajectoryOptimizationNode::vizBoundaryPoints(const std::vectorpublish(marker_array); } @@ -346,7 +368,7 @@ void TrajectoryOptimizationNode::vizEgoCircles(const std::vector& x_traj // define vehicle geometry based on model name (should match the OCP definition) if (model_name == "karl") { ego_length = 5.173; - ego_width = 2.252; + ego_width = 1.94; ego_offset2geocenter = {1.4895, 0.0}; n_ego_circles = 5; } else if (model_name == "shuttle") { @@ -416,7 +438,7 @@ void TrajectoryOptimizationNode::vizEgoCircles(const std::vector& x_traj ego_circles_pub_->publish(marker_array); } -void TrajectoryOptimizationNode::printSolution(int status) { +void TrajectoryOptimizationNode::printSolution(const PerformanceMetrics& metrics) { // Status codes: // 0: Success (ACADOS_SUCCESS) // 1: NaN detected (ACADOS_NAN_DETECTED) @@ -426,34 +448,22 @@ void TrajectoryOptimizationNode::printSolution(int status) { // 5: Solver created (ACADOS_READY) // 6: Problem unbounded (ACADOS_UNBOUNDED) // 7: Solver timeout (ACADOS_TIMEOUT) - if (status == ACADOS_SUCCESS) { + if (metrics.status == ACADOS_SUCCESS && verbose_) { RCLCPP_INFO(get_logger(), "\033[1;32mOptimization: SUCCESS!\033[0m"); - } else if (status == ACADOS_MAXITER) { - RCLCPP_WARN(get_logger(), "Optimization failed with status %d (max iterations).", status); - } else if (status == ACADOS_TIMEOUT) { - RCLCPP_WARN(get_logger(), "\033[38;5;214mOptimization failed with status %d (timeout).\033[0m", status); - } else { - RCLCPP_ERROR(get_logger(), "%s_acados_solve() failed with status %d.", model_name_.c_str(), status); + } else if (metrics.status == ACADOS_MAXITER) { + RCLCPP_WARN(get_logger(), "Optimization failed with status %d (max iterations).", metrics.status); + } else if (metrics.status == ACADOS_TIMEOUT) { + RCLCPP_WARN(get_logger(), "\033[38;5;214mOptimization failed with status %d (timeout).\033[0m", metrics.status); + } else if (metrics.status != ACADOS_SUCCESS) { + RCLCPP_ERROR(get_logger(), "%s_acados_solve() failed with status %d.", model_name_.c_str(), metrics.status); } - // print duration, KKT, and number of SQP iterations - double elapsed_time = 0.0, kkt_norm_inf = 0.0; - int sqp_iter = 0; - ocp_nlp_get(nlp_solver_, "time_tot", &elapsed_time); - ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, 0, "kkt_norm_inf", &kkt_norm_inf); - ocp_nlp_get(nlp_solver_, "sqp_iter", &sqp_iter); - RCLCPP_INFO(get_logger(), "Optimization took \033[1m%f ms.\033[0m (SQP iter: \033[1m%2d\033[0m; KKT: \033[1m%e\033[0m)", - elapsed_time * 1000, sqp_iter, kkt_norm_inf); - - // print cost value and residuals - double cost_value = 0.0, nlp_res = 0.0; - ocp_nlp_eval_cost(nlp_solver_, nlp_in_, nlp_out_); - ocp_nlp_eval_residuals(nlp_solver_, nlp_in_, nlp_out_); - ocp_nlp_get(nlp_solver_, "cost_value", &cost_value); - ocp_nlp_get(nlp_solver_, "nlp_res", &nlp_res); - RCLCPP_INFO(get_logger(), "cost_value: \033[1m%f\033[0m; nlp_res: \033[1m%f\033[0m", cost_value, nlp_res); - if (verbose_) { + RCLCPP_INFO(get_logger(), "Optimization took %.3f ms (SQP iter: %d; QP iter: %d; KKT: %e)", metrics.acados_total_ms, + metrics.sqp_iter, metrics.qp_iter, metrics.kkt_norm_inf); + RCLCPP_INFO(get_logger(), "cost_value: %f; residuals: stat=%e eq=%e ineq=%e comp=%e", metrics.cost_value, metrics.res_stat, + metrics.res_eq, metrics.res_ineq, metrics.res_comp); + std::fputs("\n--- xtraj ---\n", stdout); d_print_exp_tran_mat(*nlp_dims_->nx, n_shots_ + 1, xtraj_.data(), *nlp_dims_->nx); std::fputs("\n--- utraj ---\n", stdout); @@ -462,4 +472,10 @@ void TrajectoryOptimizationNode::printSolution(int status) { } } +void TrajectoryOptimizationNode::logPerformance(const PerformanceMetrics& metrics) { + if (performance_logger_) { + performance_logger_->write(metrics); + } +} + } // namespace trajectory_optimization diff --git a/trajectory_optimization_ocp/package.xml b/trajectory_optimization_ocp/package.xml index af1a909..988be11 100644 --- a/trajectory_optimization_ocp/package.xml +++ b/trajectory_optimization_ocp/package.xml @@ -3,7 +3,7 @@ trajectory_optimization_ocp - 1.2.0 + 1.3.1 Defines the OCP for trajectory optimization and generates the corresponding C code headers/libraries, which are then used by `trajectory_optimization`. Jean-Pierre Busch diff --git a/trajectory_optimization_ocp/trajectory_optimization_ocp/constraints.py b/trajectory_optimization_ocp/trajectory_optimization_ocp/constraints.py index 059d2f6..29398f9 100644 --- a/trajectory_optimization_ocp/trajectory_optimization_ocp/constraints.py +++ b/trajectory_optimization_ocp/trajectory_optimization_ocp/constraints.py @@ -118,12 +118,6 @@ def set_constraints(ocp: AcadosOcp, config): ) normal_vec = ca.vertcat(-ca.sin(ref_inter["psi"]), ca.cos(ref_inter["psi"])) - # calc signed lateral offset from interpolated reference path to current position - vec_x = ocp.model.x[STATE_INDEX_X] - ref_inter["x"] - vec_y = ocp.model.x[STATE_INDEX_Y] - ref_inter["y"] - ref_diff = ca.vertcat(vec_x, vec_y) - d_normal = ca.dot(ref_diff, normal_vec) - # calc offset to boundaries for each ego circle MAX_BOUNDARY_CONSTRAINT = 1e9 # limit the requested extra clearance to what the current lane geometry allows @@ -131,11 +125,9 @@ def set_constraints(ocp: AcadosOcp, config): right_margin_required = ca.fmin(d_min_boundary_lat, ca.fmax(ref_inter["d_right_boundary"] - ego_radius, 0.0)) for i in range(n_ego_circles): - # circle offset from current position - circle_offset = ca.vertcat(ego_approximation["x_offset"][i], ego_approximation["y_offset"][i]) - - # distance to reference from each ego circle center - d_circle_center_ref_path = d_normal + ca.dot(circle_offset, normal_vec) + # signed lateral distance from the reference path to the actual circle center + circle_ref_diff = ca.vertcat(ego_approximation["x"][i] - ref_inter["x"], ego_approximation["y"][i] - ref_inter["y"]) + d_circle_center_ref_path = ca.dot(circle_ref_diff, normal_vec) # boundary constraint: circl_center_to_ref + ego_radius + margin < d_boundary (note: offset to right is negative!) left_constraint = d_circle_center_ref_path + ego_radius + left_margin_required - ref_inter["d_left_boundary"] diff --git a/trajectory_optimization_ocp/trajectory_optimization_ocp/dims.py b/trajectory_optimization_ocp/trajectory_optimization_ocp/dims.py deleted file mode 100644 index 5f8ef49..0000000 --- a/trajectory_optimization_ocp/trajectory_optimization_ocp/dims.py +++ /dev/null @@ -1,22 +0,0 @@ -# Copyright Institute for Automotive Engineering (ika), RWTH Aachen University -# SPDX-License-Identifier: Apache-2.0 - -from acados_template import AcadosOcpDims - - -def set_dims(ocp, config): - """Set dimensions for the OCP problem based on configuration. - - Args: - ocp: The ACADOS OCP object to configure. - config: Configuration dictionary containing optimization parameters. - """ - dims = AcadosOcpDims() - - dims.N = config["n_shots"] # number of shooting intervals - dims.nx = ocp.model.x.rows() # number of states - dims.nu = ocp.model.u.rows() # number of inputs/controls - dims.np = ocp.model.p.rows() # number of model parameters - dims.np_global = ocp.model.p_global.rows() # number of model global parameters - - ocp.dims = dims diff --git a/trajectory_optimization_ocp/trajectory_optimization_ocp/generate_ocp.py b/trajectory_optimization_ocp/trajectory_optimization_ocp/generate_ocp.py index cc15d80..fbda8e3 100644 --- a/trajectory_optimization_ocp/trajectory_optimization_ocp/generate_ocp.py +++ b/trajectory_optimization_ocp/trajectory_optimization_ocp/generate_ocp.py @@ -7,7 +7,7 @@ import sys import yaml -from acados_template import AcadosOcp, AcadosOcpSolver, builders +from acados_template import AcadosOcp, AcadosOcpSolver, ocp_get_default_cmake_builder CURRENT_DIR_PATH = os.path.dirname(os.path.abspath(__file__)) MODELS_DIR_PATH = os.path.join(CURRENT_DIR_PATH, "models") @@ -17,7 +17,6 @@ constraints = importlib.import_module("constraints") costs = importlib.import_module("costs") -dims = importlib.import_module("dims") model_Ackermann = importlib.import_module("model_Ackermann") model_RWS = importlib.import_module("model_RWS") opts = importlib.import_module("opts") @@ -63,16 +62,14 @@ def main(): raise ValueError("Unknown model type. Choose between 'Ackermann' or 'RWS'.") costs.set_costs(ocp, parameters) constraints.set_constraints(ocp, parameters) # Set constraints AFTER costs as soft constraints need to modify cost - dims.set_dims(ocp, parameters) opts.set_opts(ocp, parameters) - ocp.code_export_directory = os.path.join(CURRENT_DIR_PATH, "c_generated_code") + ocp.code_gen_options.code_export_directory = os.path.join(CURRENT_DIR_PATH, "c_generated_code") + ocp.code_gen_options.json_file = f"{parameters['model_name']}.json" - builder = builders.CMakeBuilder() - builder.options_on = ["BUILD_ACADOS_SOLVER_LIB", "BUILD_ACADOS_OCP_SOLVER_LIB"] + builder = ocp_get_default_cmake_builder() + builder.options_on.append("BUILD_ACADOS_SOLVER_LIB") - _ = AcadosOcpSolver( - ocp, json_file=f"{parameters['model_name']}.json", simulink_opts=None, build=True, generate=True, cmake_builder=builder - ) + _ = AcadosOcpSolver(ocp, build=True, generate=True, cmake_builder=builder) if __name__ == "__main__": diff --git a/trajectory_optimization_ocp/trajectory_optimization_ocp/opts.py b/trajectory_optimization_ocp/trajectory_optimization_ocp/opts.py index afb05a1..4082809 100644 --- a/trajectory_optimization_ocp/trajectory_optimization_ocp/opts.py +++ b/trajectory_optimization_ocp/trajectory_optimization_ocp/opts.py @@ -1,8 +1,6 @@ # Copyright Institute for Automotive Engineering (ika), RWTH Aachen University # SPDX-License-Identifier: Apache-2.0 -from acados_template import AcadosOcpOptions - def set_opts(ocp, config): """Set ACADOS OCP solver options based on configuration. @@ -11,7 +9,7 @@ def set_opts(ocp, config): ocp: ACADOS OCP object to configure. config: Configuration dictionary containing optimization settings. """ - opts = AcadosOcpOptions() + opts = ocp.solver_options # set options opts.qp_solver = "PARTIAL_CONDENSING_HPIPM" @@ -39,7 +37,7 @@ def set_opts(ocp, config): opts.tol = 1e-4 # default 1e-6 opts.qp_solver_iter_max = 50 # default 50 opts.qp_solver_warm_start = ( - 2 # default 0. 1 (warm: Initialize solver primal w/ last it) faster, 2 (hot: also initialize dual) even faster + 0 # default 0. 1 (warm: Initialize solver primal w/ last it) faster, 2 (hot: also initialize dual) even faster ) # opts.qp_solver_tol_stat = 1e-4 # opts.qp_solver_tol_eq = 1e-4 @@ -50,13 +48,14 @@ def set_opts(ocp, config): opts.sim_method_num_steps = 2 # default 1. Not sure what this does, but 2 seems to make it slightly faster opts.globalization = "FIXED_STEP" # default. String in ('FIXED_STEP', 'MERIT_BACKTRACKING'). opts.globalization_use_SOC = 0 # default. 1 could help to solve the problem if 0 fails, but will be slower - opts.line_search_use_sufficient_descent = 0 # default. 1 could help to solve the problem if 0 fails, but will be slower + opts.globalization_line_search_use_sufficient_descent = ( + 0 # default. 1 could help to solve the problem if 0 fails, but will be slower + ) # opts.globalization_line_search_use_sufficient_descent = 1 opts.levenberg_marquardt = 0.05 # default. Larger values could help to solve the problem if 0 fails, but will be slower # opts.nlp_solver_warm_start_first_qp = True # opts.nlp_solver_warm_start_first_qp_from_nlp = True - # set prediction horizon in s + # set prediction horizon + opts.N_horizon = config["n_shots"] opts.tf = config["optimization_horizon"] - - ocp.solver_options = opts diff --git a/trajectory_optimization_ocp/trajectory_optimization_ocp/utils.py b/trajectory_optimization_ocp/trajectory_optimization_ocp/utils.py index 172ac45..cd46da4 100644 --- a/trajectory_optimization_ocp/trajectory_optimization_ocp/utils.py +++ b/trajectory_optimization_ocp/trajectory_optimization_ocp/utils.py @@ -100,7 +100,6 @@ def determine_spacially_matched_ref_path_point(config: dict, p_ref_path: ca.MX, # formulate it as parameter lambda # values [0, 1] for lambda mean the nearest point is on the segment # and the computed distance is perpendicular to the line segment - # note that lambda must be >=0 due to the way we defined the line segment psi1 = psi_ref_path[idx_min] x1 = x_ref_path[idx_min] y1 = y_ref_path[idx_min] @@ -116,18 +115,19 @@ def determine_spacially_matched_ref_path_point(config: dict, p_ref_path: ca.MX, d_right_boundary2 = d_right_boundary_ref_path[next_idx_min] dxy_sq = ca.power(x2 - x1, 2) + ca.power(y2 - y1, 2) - lmd = ca.if_else((dxy_sq == 0), 0, ((x_position - x1) * (x2 - x1) + (y_position - y1) * (y2 - y1)) / dxy_sq) + raw_lmd = ca.if_else((dxy_sq == 0), 0, ((x_position - x1) * (x2 - x1) + (y_position - y1) * (y2 - y1)) / dxy_sq) # allow extrapolation for beginning of reference but not at the end (to penalize overshooting) - lmd = ca.if_else(condition_begin, lmd, ca.fmin(ca.fmax(lmd, 0), 1)) - x_ref_inter = x1 + lmd * (x2 - x1) - y_ref_inter = y1 + lmd * (y2 - y1) - d_left_boundary_inter = d_left_boundary1 + lmd * (d_left_boundary2 - d_left_boundary1) - d_right_boundary_inter = d_right_boundary1 + lmd * (d_right_boundary2 - d_right_boundary1) - - # interpolate psi and v without extrapolation at the beginning - lmd = ca.fmin(ca.fmax(lmd, 0), 1) - psi_ref_inter = psi1 + lmd * wrap_angle(psi2 - psi1) - v_ref_inter = v1 + lmd * (v2 - v1) + position_lmd = ca.if_else(condition_begin, raw_lmd, ca.fmin(ca.fmax(raw_lmd, 0), 1)) + x_ref_inter = x1 + position_lmd * (x2 - x1) + y_ref_inter = y1 + position_lmd * (y2 - y1) + + # Boundary distances, heading, and velocity are only defined at the provided + # reference samples and must not be extrapolated with the reference line. + bounded_lmd = ca.fmin(ca.fmax(position_lmd, 0), 1) + d_left_boundary_inter = d_left_boundary1 + bounded_lmd * (d_left_boundary2 - d_left_boundary1) + d_right_boundary_inter = d_right_boundary1 + bounded_lmd * (d_right_boundary2 - d_right_boundary1) + psi_ref_inter = psi1 + bounded_lmd * wrap_angle(psi2 - psi1) + v_ref_inter = v1 + bounded_lmd * (v2 - v1) interpolated_ref_path_point = { "psi": psi_ref_inter, @@ -148,42 +148,37 @@ def approximate_ego_geometry(ocp: AcadosOcp, config: dict) -> dict: config: Configuration dictionary with parameters like length, width, n_ego_circles, and offset2geocenter. Returns: - A dictionary containing circle positions (x, y), offsets (x_offset, y_offset), and radius. + A dictionary containing circle positions (x, y) and their radius. """ # rectangular ego-vehicle approximation with n_circles circles + psi = ocp.model.x[STATE_INDEX_PSI] + cos_psi = ca.cos(psi) + sin_psi = ca.sin(psi) # currently only working if y-offset (second element in "offset2geocenter") is 0.0 -> TODO: handle y-offset - ego_center_x = ocp.model.x[STATE_INDEX_X] + config["offset2geocenter"][0] * ca.cos(ocp.model.x[STATE_INDEX_PSI]) - ego_center_y = ocp.model.x[STATE_INDEX_Y] + config["offset2geocenter"][0] * ca.sin(ocp.model.x[STATE_INDEX_PSI]) + ego_center_x = ocp.model.x[STATE_INDEX_X] + config["offset2geocenter"][0] * cos_psi + ego_center_y = ocp.model.x[STATE_INDEX_Y] + config["offset2geocenter"][0] * sin_psi # Calculate the radius using symbolic operations radius = ca.sqrt(ca.power(config["length"] / (2 * config["n_ego_circles"]), 2) + ca.power((config["width"] / 2.0), 2)) # Initialize an empty list for circle centers coordinates circle_position_x = [] circle_position_y = [] - circle_offset_x = [] - circle_offset_y = [] if config["n_ego_circles"] == 1: circle_position_x.append(ego_center_x) circle_position_y.append(ego_center_y) - circle_offset_x.append(0.0) - circle_offset_y.append(0.0) else: # Loop to compute the centers of each circle for i in range(config["n_ego_circles"]): lon_offset = -config["length"] / 2 + (2 * i + 1) * config["length"] / (2 * config["n_ego_circles"]) - x_offset = lon_offset * ca.cos(ocp.model.x[STATE_INDEX_PSI]) - y_offset = lon_offset * ca.sin(ocp.model.x[STATE_INDEX_PSI]) + x_offset = lon_offset * cos_psi + y_offset = lon_offset * sin_psi circle_position_x.append(ego_center_x + x_offset) circle_position_y.append(ego_center_y + y_offset) - circle_offset_x.append(x_offset) - circle_offset_y.append(y_offset) return { "x": circle_position_x, "y": circle_position_y, - "x_offset": circle_offset_x, - "y_offset": circle_offset_y, "radius": radius, }