diff --git a/docs/_04_robot_behaviors/index.rst b/docs/_04_robot_behaviors/index.rst index e09f7dff..236a6741 100644 --- a/docs/_04_robot_behaviors/index.rst +++ b/docs/_04_robot_behaviors/index.rst @@ -85,12 +85,12 @@ Trajectory Generation Behaviors * - Name - Description - Parameters - * - Generate Polynomial Trajectory - - Convert a list of waypoints to a trajectory - - | Header: desired frame of the path - | Yaw: desired mode - | Path: desired path with a list of poses with id. - | Max speed: desired speed movement, in meters per second (m/s) + * - :ref:`behaviors_trajectory_generation_page` + - Convert a list of waypoints into a smooth trajectory through a runtime-selectable plugin (dynamic_mav, mav, jerk_limited or gcopter). + - | Yaw: desired yaw mode. + | Path: list of waypoints with id. + | Max speed: desired speed along the path (m/s). + | Start on paused: if true, generate the trajectory and stay paused until the client resumes. .. _behaviors_param_estimation: diff --git a/docs/_04_robot_behaviors/trajectory_generation/index.rst b/docs/_04_robot_behaviors/trajectory_generation/index.rst new file mode 100644 index 00000000..f28ace79 --- /dev/null +++ b/docs/_04_robot_behaviors/trajectory_generation/index.rst @@ -0,0 +1,561 @@ +.. _behaviors_trajectory_generation_page: + +Generate Polynomial Trajectory Behavior +======================================= + +The ``generate_polynomial_trajectory_behavior`` is the core component of the +``as2_behaviors_trajectory_generation`` package in Aerostack2. + +This behavior turns a list of waypoints into a smooth time-parameterised +trajectory and feeds it to the controller through the +``motion_reference/trajectory_setpoints`` topic. It exposes a **plugin-based +architecture** so the actual trajectory-generation algorithm can be selected +at runtime via the ``plugin_name`` launch argument, while the action server, +TF handling, yaw policy, sampling and debug topics are shared by every +plugin. + +Working Principle +----------------- + +The behavior is split in two layers: + +* **Wrapper** (``GeneratePolynomialTrajectoryBehavior``): an + ``as2_behavior::BehaviorServer`` that owns the action lifecycle + (``on_activate`` / ``on_modify`` / ``on_deactivate`` / ``on_pause`` / + ``on_resume`` / ``on_run``), converts the incoming waypoints to the + configured ``desired_frame_id`` via TF, samples the trajectory at the + control-cycle rate and forwards the setpoints through the + ``as2::motionReferenceHandlers::TrajectoryMotion`` handler. +* **Plugin** (one of the available trajectory generators, see below): solves + the actual generation problem (polynomial fitting, S-curve simulation, + GCOPTER optimisation, …) behind a stable interface + (``GeneratePolynomialTrajectoryBase``). + +Pipeline per goal: + +1. The wrapper validates the incoming waypoints, converts them to + ``desired_frame_id`` and stores them in the pending mission queue. +2. The plugin is asked to generate (or update) the trajectory through its + contract (``generateTrajectory`` / ``updateWaypoints``). +3. On every control cycle (``on_run``), the wrapper evaluates the plugin at + ``trajectory_time_`` for ``sampling_n`` samples spaced by ``sampling_dt`` + and publishes a ``TrajectorySetpoints`` message. +4. When the plugin reports ``isFinished()``, the wrapper hands over to the + hover handler and closes the action with success. + +Yaw Policy +^^^^^^^^^^ + +The yaw goal mode is taken from the action ``yaw`` field +(``as2_msgs/YawMode``). The wrapper supports the following modes: + +* ``KEEP_YAW`` — hold the current yaw measured at goal acceptance. +* ``PATH_FACING`` — point the nose along the trajectory tangent. +* ``FIXED_YAW`` — track the yaw angle provided in the goal. +* ``YAW_FROM_TOPIC`` — track yaw from the ``motion_reference/yaw`` topic. +* ``FACE_REFERENCE`` — point the nose at a reference position; rate-limited + by ``yaw_speed_threshold``. + +The thresholds ``yaw_threshold`` (m) and ``yaw_speed_threshold`` (rad/s) +gate the yaw recomputation to avoid jitter when the drone is stationary or +very close to the reference. + +Pause-after-Generate (``start_on_paused``) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +When a goal is sent with ``start_on_paused: true`` the wrapper builds the +trajectory in ``on_activate`` but does **not** emit setpoints. The first +``on_run`` tick reports ``PAUSED`` and the behavior stays in that state +until the client sends ``resume``. After ``resume`` the trajectory is +sampled from ``trajectory_time_ = 0`` exactly, so the planned mission is +preserved without realignment. + +Partial-Trajectory Feed (``path_length``) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +For long missions the wrapper can deliver waypoints to the plugin in a +sliding window of ``path_length`` waypoints instead of feeding the full +list up front. This trades a CPU spike at start for a steady drip of +smaller re-plans while the drone flies. + +* ``path_length: 0`` (default) — feeds the full mission to the plugin + in one shot (legacy behavior). +* ``path_length: N`` — keeps at most ``N`` waypoints active in the + plugin; consumed waypoints are replaced by the next pending one through + ``plugin->updateWaypoints()``. Plugins that override ``updateWaypoints`` + for smooth stitching keep their internal time axis untouched; plugins + that don't override it regenerate using the live ``vehicle_pose`` / + ``vehicle_twist`` as boundary conditions. + +In both cases ``feedback.remaining_waypoints`` reflects the full mission +queue, not the active window. + +Frame-Drift Watchdog +^^^^^^^^^^^^^^^^^^^^ + +When ``frequency_update_frame > 0`` the wrapper checks the map↔odom +transform at that rate and triggers a regeneration when the translation +drift exceeds ``transform_threshold`` (m). This keeps the trajectory +consistent with the active state-estimation frame after re-localisations. + +Available Plugins +----------------- + +The package ships four plugins that implement the +``GeneratePolynomialTrajectoryBase`` contract. Choose one at launch time +through the ``plugin_name`` argument. + +1. **dynamic_mav_trajectory_generator** + + Wrapper around the + `dynamic_trajectory_generator `_ + library (modifiable polynomial generator on top of + `ETH-ASL mav_trajectory_generation `_). + Supports **smooth online stitching**: + ``updateWaypoints`` re-plans the pending segment without breaking the + internal time axis, so the boundary velocity is taken from the current + trajectory and there is no visible discontinuity. Regeneration is + asynchronous (the new trajectory is computed in a background thread and + swapped lazily). Recommended for long missions with frequent + ``on_modify`` requests. + +2. **mav_trajectory_generator** + + Wrapper around the + `mav_trajectory_generation_library `_ + (Aerostack2 fork of + `ETH-ASL mav_trajectory_generation `_, + static polynomial generator with linear or NLopt-based nonlinear + solver). Uses the **regenerate** semantics for + ``updateWaypoints``: every modify replans from scratch using + ``vehicle_pose_`` / ``vehicle_twist_`` as boundary state, with + ``Waypoint::velocity`` injecting the current twist for C\ :sup:`1` + continuity. + +3. **jerk_limited_trajectory_generator** + + Wrapper around the + `jerk_limited_trajectory_generator_lib `_ + S-curve simulator (based on the + `PX4-Autopilot `_ jerk-limited + trajectory profile). Enforces per-axis jerk, plus 3D-norm acceleration and + speed bounds. Optional L1 corner-cut smoothing through + ``acceptance_radius`` lets the drone fly through intermediate waypoints + without coming to a full stop. Uses **regenerate** semantics on + ``updateWaypoints``. + +4. **gcopter_trajectory_generator** + + Wrapper around the + `gcopter_trajectory_generator_lib `_ + optimiser (Aerostack2 fork of + `ZJU-FAST-Lab GCOPTER `_, + with safe-flight-corridor support). Optimises a polynomial + trajectory under hard dynamic limits (max body rate, tilt, thrust, + velocity) and an AABB corridor anchored on each user waypoint. Uses + **regenerate** semantics on ``updateWaypoints`` and honours the live + ``vehicle_twist_`` as a hard initial-velocity boundary condition. + +Action Interface +---------------- + +The behavior exposes the action ``as2_msgs/action/GeneratePolynomialTrajectory`` +on ``/TrajectoryGeneratorBehavior``. + +.. list-table:: Goal + :widths: 30 70 + :header-rows: 1 + + * - Field + - Description + * - **stamp** (``builtin_interfaces/Time``) + - Request timestamp. + * - **yaw** (``as2_msgs/YawMode``) + - Yaw goal mode (see *Yaw Policy* above). + * - **path** (``as2_msgs/PoseStampedWithID[]``) + - List of waypoints with id and stamped pose. Empty ids are auto-filled + as ``waypoint_XXX``. + * - **max_speed** (``float32``) + - Maximum desired translation speed along the path (m/s). + * - **start_on_paused** (``bool``) + - If ``true``, the trajectory is generated and the behavior stays in + ``PAUSED`` until the client sends ``resume`` (see *Pause-after-Generate* + above). Default: ``false`` — start emitting setpoints immediately. + +.. list-table:: Result + :widths: 30 70 + :header-rows: 1 + + * - Field + - Description + * - **trajectory_generator_success** (``bool``) + - ``false`` if the generated trajectory could not be followed to + completion. + +.. list-table:: Feedback + :widths: 30 70 + :header-rows: 1 + + * - Field + - Description + * - **next_waypoint_id** (``string``) + - Id of the next pending waypoint in the mission queue. + * - **remaining_waypoints** (``uint16``) + - Number of waypoints in the mission queue that have not yet been + consumed (counts the full mission, not the active window when + ``path_length`` is enabled). + +Configuration Parameters +------------------------ + +The configuration is split in two layers: the **wrapper YAML** with shared +parameters and one **plugin YAML** per backend with algorithm-specific keys. + +Wrapper Parameters +^^^^^^^^^^^^^^^^^^ + +Defaults in +``generate_polynomial_trajectory_behavior/config/config_default.yaml``. + +.. list-table:: Wrapper Parameters + :widths: 30 15 55 + :header-rows: 1 + + * - Parameter + - Default + - Description + * - **desired_frame_id** + - ``odom`` + - TF frame in which the trajectory is generated and evaluated. Incoming + waypoints are converted to this frame before being fed to the plugin. + * - **sampling_n** + - ``1`` + - Number of trajectory samples emitted per control cycle. Must be ≥ 1. + * - **sampling_dt** + - ``0.01`` + - Time step between consecutive samples (s). + * - **path_length** + - ``0`` + - Active waypoint window fed to the plugin. ``0`` disables the sliding + window and feeds the full mission up front. ``> 0`` enables + incremental feeding (see *Partial-Trajectory Feed* above). + * - **yaw_threshold** + - ``0.1`` + - Minimum planar distance (m) before recomputing yaw to avoid jitter + at low displacement. + * - **yaw_speed_threshold** + - ``0.0`` + - Maximum yaw rate (rad/s) for the ``FACE_REFERENCE`` mode. ``0.0`` + disables the rate limit. + * - **frequency_update_frame** + - ``0.0`` + - Map↔odom drift watchdog rate (Hz). ``0.0`` disables the watchdog. + * - **transform_threshold** + - ``1.0`` + - Translation drift (m) on the watched transform that triggers a + regeneration. ``0.0`` disables the drift check. + * - **debug.path_topic** + - ``debug/traj_generated`` + - Topic where the sampled generated trajectory is republished as + ``nav_msgs/Path``. Empty disables the publisher. + * - **debug.reference_setpoint** + - ``debug/ref_traj_point`` + - Topic for the current reference point as ``visualization_msgs/Marker``. + * - **debug.reference_end_waypoint** + - ``debug/ref_traj_end_point`` + - Topic for the end-of-horizon point as ``visualization_msgs/Marker``. + * - **debug.reference_waypoints** + - ``debug/waypoints`` + - Topic for the full waypoint queue as + ``visualization_msgs/MarkerArray``. + +mav_trajectory_generator Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Defaults in +``plugins/mav_trajectory_generator/config/mav_trajectory_generator.yaml``, +under the namespace ``mav_trajectory_generator.optimization.*``. + +.. list-table:: mav_trajectory_generator Parameters + :widths: 35 15 50 + :header-rows: 1 + + * - Parameter + - Default + - Description + * - **derivative_to_optimize** + - ``2`` + - Derivative order minimised by the cost: ``0`` POS, ``1`` VEL, + ``2`` ACC, ``3`` JERK, ``4`` SNAP. + * - **solver** + - ``nonlinear`` + - ``linear`` (closed-form QP) or ``nonlinear`` (NLopt with hard + constraints). + * - **a_max** + - ``9.81`` + - Maximum acceleration norm enforced by the nonlinear solver + (m/s\ :sup:`2`). + * - **nl_max_iterations** + - ``2000`` + - NLopt iteration cap. + * - **nl_f_rel** + - ``0.05`` + - NLopt relative cost tolerance. + * - **nl_x_rel** + - ``0.1`` + - NLopt relative parameter tolerance. + * - **nl_time_penalty** + - ``1000.0`` + - Penalty weight on total trajectory time. + * - **nl_initial_stepsize_rel** + - ``0.1`` + - Initial NLopt step size (relative). + * - **nl_inequality_constraint_tolerance** + - ``0.2`` + - Tolerance on the inequality (acceleration) constraint. + +dynamic_mav_trajectory_generator Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The dynamic plugin currently exposes no per-instance ROS parameters; the +underlying ``dynamic_trajectory_generator`` library is configured with its +own internal defaults. The plugin honours all the wrapper parameters +described above. + +jerk_limited_trajectory_generator Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Defaults in +``plugins/jerk_limited_trajectory_generator/config/jerk_limited_trajectory_generator.yaml``, +under the namespace ``jerk_limited_trajectory_generator.{limits,simulator}.*``. + +The action goal's ``max_speed`` transiently overrides ``limits.max_speed`` +for a single call. + +.. list-table:: jerk_limited_trajectory_generator Parameters + :widths: 35 15 50 + :header-rows: 1 + + * - Parameter + - Default + - Description + * - **limits.max_speed** + - ``1.0`` + - Maximum 3D speed norm (m/s). Set ≤ 0 to disable. + * - **limits.max_acceleration** + - ``10.0`` + - Maximum 3D acceleration norm (m/s\ :sup:`2`). Set ≤ 0 to disable. + * - **limits.max_jerk** + - ``20.0`` + - Maximum per-axis jerk (m/s\ :sup:`3`). Set ≤ 0 to disable. At least + one of ``max_speed`` / ``max_acceleration`` / ``max_jerk`` must + remain > 0. + * - **limits.acceptance_radius** + - ``0.01`` + - Radius (m) for the L1 corner-cut at intermediate waypoints. ``0`` + forces a full stop at every intermediate waypoint. + * - **limits.final_acceptance_radius** + - ``0.01`` + - Position tolerance at the last waypoint (m). ≤ 0 reuses + ``acceptance_radius``. + * - **limits.trajectory_gain** + - ``0.1`` + - Scales acceleration when computing the corner-speed cap. + * - **simulator.step_dt** + - ``0.01`` + - Internal integration step (s). + * - **simulator.max_simulation_time** + - ``600.0`` + - Hard horizon (s) for the offline simulation; trajectories that exceed + it fail to generate. + * - **simulator.settle_velocity** + - ``0.05`` + - Velocity norm (m/s) below which the drone is considered at rest. + * - **simulator.advance_radius_min** + - ``0.1`` + - Floor (m) for the per-waypoint advance threshold. The effective + threshold is ``max(acceptance_radius, advance_radius_min)``. + * - **simulator.polynomial_order** + - ``16`` + - Number of polynomial coefficients per axis and segment in the + resulting spline. + +gcopter_trajectory_generator Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Defaults in +``plugins/gcopter_trajectory_generator/config/gcopter_trajectory_generator.yaml``, +under the namespace +``gcopter_trajectory_generator.{drone,limits,optimization,waypoints}.*``. + +The action goal's ``max_speed`` transiently overrides ``limits.max_velocity`` +for a single call. + +.. list-table:: gcopter_trajectory_generator Parameters + :widths: 35 15 50 + :header-rows: 1 + + * - Parameter + - Default + - Description + * - **drone.mass** + - ``1.0`` + - Vehicle mass (kg) used by the flatness model. + * - **drone.gravity** + - ``9.81`` + - Gravity acceleration (m/s\ :sup:`2`). + * - **drone.horizontal_drag** + - ``0.1`` + - Horizontal drag coefficient. + * - **drone.vertical_drag** + - ``0.1`` + - Vertical drag coefficient. + * - **drone.parasitic_drag** + - ``0.01`` + - Parasitic drag coefficient. + * - **drone.speed_smooth_factor** + - ``0.01`` + - Smoothing factor on the speed term in the flatness model. + * - **limits.max_velocity** + - ``5.0`` + - Maximum velocity norm (m/s). + * - **limits.max_body_rate** + - ``6.0`` + - Maximum body rate (rad/s). + * - **limits.max_tilt_angle** + - ``0.785`` + - Maximum tilt angle (rad). + * - **limits.min_thrust** + - ``0.1`` + - Minimum collective thrust (N). + * - **limits.max_thrust** + - ``30.0`` + - Maximum collective thrust (N). + * - **optimization.time_weight** + - ``512.0`` + - Weight on the total trajectory time term. + * - **optimization.position_weight** + - ``10000.0`` + - Weight on position constraint violations. + * - **optimization.velocity_weight** + - ``10000.0`` + - Weight on velocity constraint violations. + * - **optimization.body_rate_weight** + - ``10000.0`` + - Weight on body-rate constraint violations. + * - **optimization.tilt_weight** + - ``10000.0`` + - Weight on tilt-angle constraint violations. + * - **optimization.thrust_weight** + - ``10000.0`` + - Weight on thrust constraint violations. + * - **optimization.smoothing_eps** + - ``0.01`` + - Smoothing epsilon for the soft-constraint penalty. + * - **optimization.integral_resolution** + - ``16`` + - Number of integration samples per segment for the constraint penalty. + * - **optimization.rel_cost_tol** + - ``0.001`` + - L-BFGS relative cost tolerance. + * - **optimization.corridor_margin** + - ``0.1`` + - AABB half-margin (m) used between anchors and on segments without + intermediate waypoints. + * - **waypoints.waypoint_margin** + - ``0.1`` + - AABB half-margin (m) on the two pinch segments around each + intermediate waypoint. + * - **waypoints.waypoint_anchor_radius** + - ``0.5`` + - Offset (m) of each anchor along the path. ≤ 0 disables the anchoring + and lets the solver cut corners. + +Launching the Behavior +---------------------- + +The plugin to load is selected through the ``plugin_name`` launch +argument. The set of valid choices is auto-populated from the registered +plugins via ``pluginlib``, so an invalid value is rejected at launch time. + +**Standalone node** + +.. code-block:: bash + + ros2 launch as2_behaviors_trajectory_generation generate_polynomial_trajectory_behavior_launch.py \ + namespace:= \ + plugin_name:=dynamic_mav_trajectory_generator + +Launch arguments: + +* ``namespace`` — drone namespace (defaults to the + ``AEROSTACK2_SIMULATION_DRONE_ID`` environment variable). +* ``plugin_name`` — one of + ``dynamic_mav_trajectory_generator``, ``mav_trajectory_generator``, + ``jerk_limited_trajectory_generator``, ``gcopter_trajectory_generator``. + If left empty, the launch tries to read it from ``config_file``; if it + is also missing there, the launch fails. +* ``config_file`` — path to the wrapper YAML (defaults to + ``config_default.yaml``). The plugin YAML is auto-located in + ``plugins//config/.yaml``. +* ``log_level`` — node log level (default ``info``). +* ``use_sim_time`` — use the simulation clock if ``true`` (default + ``false``). + +**Composable node** + +.. code-block:: bash + + ros2 launch as2_behaviors_trajectory_generation composable_generate_polynomial_trajectory_behavior.launch.py \ + namespace:= \ + plugin_name:=dynamic_mav_trajectory_generator \ + container:= + +If ``container`` is empty a new container is created; otherwise the +behavior is loaded into the existing one as a +``rclcpp_components`` composable node. + +**Sending a goal manually** + +.. code-block:: bash + + ros2 action send_goal //TrajectoryGeneratorBehavior \ + as2_msgs/action/GeneratePolynomialTrajectory \ + "{ + yaw: {mode: 0, angle: 0.0}, + path: [ + {id: 'wp_0', pose: {header: {frame_id: 'earth'}, + pose: {position: {x: 1.0, y: 0.0, z: 1.0}}}}, + {id: 'wp_1', pose: {header: {frame_id: 'earth'}, + pose: {position: {x: 2.0, y: 1.0, z: 1.5}}}} + ], + max_speed: 1.0, + start_on_paused: false + }" + +Set ``start_on_paused: true`` to generate the trajectory and keep the +behavior in ``PAUSED`` until the action client sends ``resume``. + +Debugging Topics +---------------- + +When the corresponding ``debug.*`` parameter is non-empty the wrapper +publishes: + +* ``/debug/traj_generated`` (``nav_msgs/Path``) — sampled + trajectory currently held by the plugin. Refreshed on every regeneration + and, for asynchronous plugins, on the cycle where the new backend + trajectory is swapped in. +* ``/debug/ref_traj_point`` (``visualization_msgs/Marker``) — + current reference point being commanded. +* ``/debug/ref_traj_end_point`` (``visualization_msgs/Marker``) — + end-of-horizon point of the active sampling window. +* ``/debug/waypoints`` (``visualization_msgs/MarkerArray``) — + pending mission waypoints. + +See also +-------- + +* :ref:`development_tutorials_trajectory_generation_plugin` — step-by-step + guide to writing a new trajectory-generation plugin against the + ``GeneratePolynomialTrajectoryBase`` contract. diff --git a/docs/_09_development/_tutorials/_tutorials/trajectory_generation_plugin.rst b/docs/_09_development/_tutorials/_tutorials/trajectory_generation_plugin.rst new file mode 100644 index 00000000..7512dc28 --- /dev/null +++ b/docs/_09_development/_tutorials/_tutorials/trajectory_generation_plugin.rst @@ -0,0 +1,487 @@ +.. _development_tutorials_trajectory_generation_plugin: + +================================================ +Writing a New Trajectory Generation Plugin +================================================ + +.. contents:: Table of Contents + :depth: 1 + :local: + + + +.. _development_tutorials_trajectory_generation_plugin_overview: + +-------- +Overview +-------- + +This tutorial shows how to add your own trajectory-generation algorithm to +Aerostack2 as a new plugin of the +:ref:`Generate Polynomial Trajectory Behavior `. + +The behavior follows a wrapper + plugin pattern: the +``GeneratePolynomialTrajectoryBehavior`` server owns the action lifecycle, +TF, yaw policy, sampling, debug topics and the partial-trajectory +machinery; the plugin only owns the trajectory math behind a stable +interface (``GeneratePolynomialTrajectoryBase``). Adding a new generator +therefore boils down to implementing that interface, registering the +plugin with ``pluginlib`` and providing a YAML with the backend +parameters — the wrapper, the launch files and the action server are +reused as-is. + +The reference implementations shipped with Aerostack2 live in +``as2_behaviors_trajectory_generation/generate_polynomial_trajectory_behavior/plugins/`` +on `GitHub +`_: + +* ``dynamic_mav_trajectory_generator`` — smooth-stitching async backend. +* ``mav_trajectory_generator`` — ETH-ASL static polynomial. +* ``jerk_limited_trajectory_generator`` — per-axis S-curve simulator. +* ``gcopter_trajectory_generator`` — GCOPTER with safe-flight-corridor. + +Use them as templates while following the steps below. + + + +.. _development_tutorials_trajectory_generation_plugin_requirements: + +------------ +Requirements +------------ + +- ROS 2 Humble +- Aerostack2 +- ``as2_behaviors_trajectory_generation`` available in your workspace +- ``pluginlib`` + + + +.. _development_tutorials_trajectory_generation_plugin_steps: + +-------------- +Tutorial Steps +-------------- + + + +.. _development_tutorials_trajectory_generation_plugin_steps_base: + +1. Plugin Base Class +==================== + +All plugins inherit from +``generate_polynomial_trajectory_behavior_plugin_base::GeneratePolynomialTrajectoryBase``. +The base class provides the following virtual methods: + +.. list-table:: List of Virtual Methods + :header-rows: 1 + + * - Virtual method + - Method description + - Requires override? + * - ownInitialize() + - Plugin-specific init hook. Declare and read parameters with the + protected ``getParameter()`` helper, build internal state. + Called from ``initialize()`` after the host node and the plugin + namespace have been cached. + - No (default is empty) + * - generateTrajectory(waypoints, max_speed, t_trajectory_now) + - Generate a fresh trajectory through the mission waypoints. Read + ``vehicle_pose_`` / ``vehicle_twist_`` from the protected base + members and inject them into your backend (``buildCurrentWaypoint()`` + is provided as a helper). Anchor your private offset so the host's + ``t_trajectory_now`` maps to the start of the new backend trajectory. + - Yes + * - updateWaypoints(waypoints, max_speed, t_trajectory_now) + - Update the active trajectory with a new pending waypoint list. + Default implementation calls ``reset()`` + ``generateTrajectory()`` + (full re-plan, plugin re-anchors against ``t_trajectory_now``). + Override only if your backend supports smooth online stitching + and you want to preserve its internal time origin. + - No (default re-plans) + * - evaluate(t_trajectory, out, is_horizon_sample) + - Sample the trajectory at host time ``t_trajectory``. Map it to your + backend axis with the private offset and clamp to the valid range. + - Yes + * - isFinished(t_trajectory) + - ``true`` when ``t_trajectory`` has reached or exceeded the end of + the currently held trajectory in the host time axis. + - Yes + * - reset() + - Clear internal state. The wrapper calls it on deactivate and on + the default ``updateWaypoints()`` re-plan path. + - Yes + * - getNextWaypointId() + - Id of the next pending mission waypoint (empty when none). Used by + the wrapper to build feedback and to drive the partial-trajectory + window. + - Yes + * - isTrajectoryGenerated() + - Whether the plugin currently holds a valid trajectory. + - Yes + * - consumeRegeneratedFlag() + - Consume and return whether the underlying backend trajectory has + been swapped since the previous call. Default ``false`` for + synchronous plugins. Async plugins override it to expose the + deferred swap event so the wrapper can refresh + ``debug/traj_generated`` once the backend has actually swapped its + internal trajectory. + - No (default is false) + + + +.. _development_tutorials_trajectory_generation_plugin_steps_initial_state: + +2. Initial-State Contract +========================= + +The trajectory start state (position **and** velocity) is **not** passed +as an argument to ``generateTrajectory()`` / ``updateWaypoints()``. It +flows through the protected base members: + +.. code-block:: cpp + + geometry_msgs::msg::PoseStamped vehicle_pose_; + geometry_msgs::msg::TwistStamped vehicle_twist_; + +The host refreshes them once per incoming state message via +``setVehicleState(pose, twist)`` directly from its odom/twist subscription +callback, so plugin entry points always see the latest state. The base +class exposes them on the read side too via ``getVehiclePose()`` / +``getVehicleTwist()`` — the host uses those for its yaw policies, so the +plugin remains the single source of truth for the live state. + +Each plugin decides how to inject the state into its backend: + +* Backends that take the start state as a "first waypoint with a fixed + pose" (gcopter_lib, mav_trajectory_generation_lib) can prepend a + synthetic entry built from ``vehicle_pose_`` and tag the optional + velocity boundary condition with ``vehicle_twist_.linear``. The base + helper ``buildCurrentWaypoint()`` returns a ready-to-use + ``PoseStampedWithID{id="current", pose=vehicle_pose_}`` for the + waypoint side; the velocity has to be propagated separately because + ``PoseStampedWithID`` does not carry twist. +* Backends that derive the boundary state from a previously generated + trajectory (dynamic_trajectory_generator) can ignore the live values on + ``updateWaypoints()`` and rely on the lib's stitching logic. + + + +.. _development_tutorials_trajectory_generation_plugin_steps_time_axis: + +3. Time-Axis Contract +===================== + +The wrapper owns a single logical trajectory time axis, +``trajectory_time_`` (seconds, ≥ 0). It starts at ``0`` on every fresh +``generateTrajectory()`` call (including the regeneration triggered by +external ``on_resume``), advances on every ``on_run`` tick, and is +preserved across ``updateWaypoints()``. + +Each plugin keeps a private offset that maps ``t_trajectory`` to its +backend axis: + +.. code-block:: cpp + + t_backend = t_trajectory + internal_offset_; + +The wrapper never observes that mapping. The contract is: + +* ``generateTrajectory(t_trajectory_now)`` re-anchors ``internal_offset_`` + so ``t_trajectory_now`` maps to the start of the new backend + trajectory. +* ``updateWaypoints(t_trajectory_now)`` lets the plugin choose: + + * **Smooth stitching** — keep ``internal_offset_`` untouched. The + backend axis continues uninterrupted, and the host's ``t_trajectory`` + keeps mapping to the same backend trajectory without realignment. + * **Regenerate** (base default) — re-anchor ``internal_offset_`` + against ``t_trajectory_now`` so the freshly generated backend + trajectory aligns with the host's current time. + +* ``evaluate()`` and ``isFinished()`` apply ``internal_offset_`` and + clamp internally to the valid backend range. + + + +.. _development_tutorials_trajectory_generation_plugin_steps_layout: + +4. Folder Layout +================ + +Create the following layout under +``as2_behaviors_trajectory_generation/generate_polynomial_trajectory_behavior/plugins/``: + +.. code-block:: text + + plugins// + ├── CMakeLists.txt + ├── include//.hpp + ├── src/.cpp + ├── config/.yaml + ├── tests/ + │ ├── CMakeLists.txt + │ └── _gtest.cpp + └── thirdparties/ # optional, gitignored, vendored deps + +The folder name is the canonical plugin name and is reused for the +library target, the namespace, the YAML filename and the launch +``plugin_name`` choice. Use ``snake_case`` consistently. + + + +.. _development_tutorials_trajectory_generation_plugin_steps_implement: + +5. Implementing the Plugin +========================== + +Header skeleton (``include//.hpp``): + +.. code-block:: cpp + + #pragma once + + #include "generate_polynomial_trajectory_behavior/generate_polynomial_trajectory_base.hpp" + + namespace my_plugin { + + class Plugin + : public generate_polynomial_trajectory_behavior_plugin_base:: + GeneratePolynomialTrajectoryBase { + public: + Plugin() = default; + ~Plugin() override = default; + + bool generateTrajectory( + const std::vector & waypoints, + double max_speed, + double t_trajectory_now) override; + + bool evaluate( + double t_trajectory, + dynamic_traj_generator::References & out, + bool is_horizon_sample) override; + + bool isFinished(double t_trajectory) override; + bool isTrajectoryGenerated() override; + void reset() override; + std::string getNextWaypointId() override; + + protected: + void ownInitialize() override; + + private: + double internal_offset_{0.0}; + // backend handles, cached parameters, etc. + }; + + } // namespace my_plugin + +Source skeleton (``src/.cpp``): + +.. code-block:: cpp + + #include "/.hpp" + #include + + namespace my_plugin { + + void Plugin::ownInitialize() { + // Read backend parameters using the auto-prefixed helper: + // getParameter("limits.max_speed", max_speed_, /*use_default=*/true); + // + // Build any backend objects you need. + } + + bool Plugin::generateTrajectory( + const std::vector & waypoints, + double max_speed, + double t_trajectory_now) { + // 1. Inject vehicle_pose_ / vehicle_twist_ into the backend. + // 2. Solve the trajectory. + // 3. Anchor internal_offset_ so that t_trajectory_now maps to the + // start of the new backend trajectory. + return true; + } + + bool Plugin::evaluate( + double t_trajectory, + dynamic_traj_generator::References & out, + bool is_horizon_sample) { + const double t_backend = t_trajectory + internal_offset_; + // Sample your backend at t_backend, fill `out`. Clamp internally. + return true; + } + + bool Plugin::isFinished(double t_trajectory) { + const double t_backend = t_trajectory + internal_offset_; + return t_backend >= backendDuration(); + } + + bool Plugin::isTrajectoryGenerated() { + // True iff a backend trajectory is currently held. + return false; + } + + void Plugin::reset() { + // Clear backend state and reset internal_offset_. + internal_offset_ = 0.0; + } + + std::string Plugin::getNextWaypointId() { + // Return the id of the next pending mission waypoint, or "" if none. + return {}; + } + + } // namespace my_plugin + + PLUGINLIB_EXPORT_CLASS( + my_plugin::Plugin, + generate_polynomial_trajectory_behavior_plugin_base:: + GeneratePolynomialTrajectoryBase) + + + +.. _development_tutorials_trajectory_generation_plugin_steps_parameters: + +6. Parameter Helper +=================== + +The base class exposes a templated helper: + +.. code-block:: cpp + + template + bool getParameter(const std::string & name, T & value, bool use_default); + +It **automatically prefixes** the parameter name with the plugin +namespace (resolved by ``qualifyParameterName()``). Plugins only deal +with short keys — there is no risk of collision with the wrapper +parameters or with another plugin loaded in the same node, because the +plugin name is enforced as a prefix. + +Supported scalar/array types are dispatched via ``if constexpr``: +``double``, ``int``, ``bool``, ``std::string``, ``std::vector``. +Other types fall back to the generic accessor with a warning. + + + +.. _development_tutorials_trajectory_generation_plugin_steps_register: + +7. Registering the Plugin +========================= + +Two files have to be touched in the ``generate_polynomial_trajectory_behavior`` +directory: + +1. **CMake** — append the plugin folder to the ``PLUGIN_LIST`` in + ``generate_polynomial_trajectory_behavior/CMakeLists.txt``: + + .. code-block:: cmake + + set(PLUGIN_LIST + dynamic_mav_trajectory_generator + mav_trajectory_generator + jerk_limited_trajectory_generator + gcopter_trajectory_generator + my_plugin + ) + + The loop in the parent CMake will pick the new folder up + automatically (``add_subdirectory``, install of the YAML, etc.). + +2. **pluginlib registry** — add an entry to + ``generate_polynomial_trajectory_behavior/plugins.xml``: + + .. code-block:: xml + + + + Short description of the new generator. + + + +The plugin's own ``CMakeLists.txt`` is responsible for exposing exactly +**one** SHARED library named after the plugin folder, exporting the +symbol ``::Plugin`` — which ``plugins.xml`` references +through ``pluginlib::ClassLoader``. + + + +.. _development_tutorials_trajectory_generation_plugin_steps_yaml: + +8. Configuration YAML +===================== + +Provide ``config/.yaml`` with the backend defaults under the +plugin namespace: + +.. code-block:: yaml + + /**: + ros__parameters: + my_plugin: + limits: + max_speed: 1.0 + max_acceleration: 5.0 + # ... your backend keys here ... + +The launch files locate the plugin YAML automatically as +``plugins//config/.yaml`` once the plugin folder is +discovered. + + + +.. _development_tutorials_trajectory_generation_plugin_steps_test: + +9. Testing +========== + +Add a minimal gtest in ``tests/_gtest.cpp`` that exercises +the contract end-to-end: + +1. ``setVehicleState(pose, twist)`` to seed the initial state. +2. ``generateTrajectory(waypoints, max_speed, /*t_trajectory_now=*/0.0)``. +3. ``evaluate(t_trajectory, out, false)`` at several points along the + trajectory. +4. ``isFinished(0.0)`` is ``false`` right after generation; ``isFinished(t)`` + becomes ``true`` after walking past the end. +5. ``reset()`` clears the state. + +For plugins that use the regenerate path, also verify that a second +``generateTrajectory(t_trajectory_now=t)`` (or the default +``updateWaypoints(t)``) re-anchors the offset so subsequent +``evaluate()`` calls stay valid for the host's progressing +``trajectory_time_``. + +Build and run with: + +.. code-block:: bash + + colcon build --packages-select as2_behaviors_trajectory_generation + colcon test --packages-select as2_behaviors_trajectory_generation + + + +.. _development_tutorials_trajectory_generation_plugin_steps_launch: + +10. Launching with the New Plugin +================================= + +Once the workspace is rebuilt and sourced, the new plugin name is +auto-registered as a valid ``plugin_name`` choice on the launch: + +.. code-block:: bash + + ros2 launch as2_behaviors_trajectory_generation generate_polynomial_trajectory_behavior_launch.py \ + namespace:= \ + plugin_name:=my_plugin + +The wrapper, the action server, the partial-trajectory window and the +debug topics work without any change. + +See the :ref:`Generate Polynomial Trajectory Behavior ` +page for details on the action interface and the wrapper-level +parameters that your plugin will inherit for free. diff --git a/docs/_09_development/_tutorials/index.rst b/docs/_09_development/_tutorials/index.rst index 90b08ef3..c4013e1e 100644 --- a/docs/_09_development/_tutorials/index.rst +++ b/docs/_09_development/_tutorials/index.rst @@ -11,4 +11,5 @@ Tutorials _tutorials/controller_plugin.rst _tutorials/estimator_plugin.rst _tutorials/behavior.rst + _tutorials/trajectory_generation_plugin.rst _tutorials/adding_gazebo_assets.rst \ No newline at end of file