diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 8a6946e7..ba38f6ee 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -1,7 +1,8 @@ { "permissions": { "allow": [ - "Read(//home/wcompton/repos/IsaacLab/source/isaaclab/isaaclab/sensors/ray_caster/patterns/**)" + "Read(//home/wcompton/repos/IsaacLab/source/isaaclab/isaaclab/sensors/ray_caster/patterns/**)", + "Bash(git *)" ] } } diff --git a/.gitignore b/.gitignore index e8c7c840..528815e0 100644 --- a/.gitignore +++ b/.gitignore @@ -246,3 +246,9 @@ MUJOCO_LOG.TXT obk_logs/ docker/user_setup.sh docker/install_sys_deps.sh + +# ###### # +# CLAUDE # +# ###### # +CLAUDE.md +.claude/ diff --git a/README.md b/README.md index e1416292..d162e4c6 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,9 @@ This repository hosts generic interfaces for controlling robots with ROS2. [Docs](https://caltech-amber.github.io/obelisk/) ## Setup -Obelisk should be used as a dependency for external robot control code that is written in other downstream projects. There are a few options: +Obelisk should be used as a dependency for external robot control code that is written in other downstream projects. There are two options: 1. Use Obelisk in Docker (this is mainly for developing in Obelisk). 2. Use Obelisk on your local filesystem. -3. Use Obelisk in a project that uses `pixi`. ### Initial Setup Initial setup proceeds by running the `setup.sh` script in the repository root. This script has the ability to make changes to your local dependencies - all such changes are opt-in. **It is very important that you run `setup.sh` using the `source` command, and not `bash`, because there are environment variables that will be sourced!** @@ -23,38 +22,22 @@ The options are as follows: source setup.sh [OPTIONS] Options: - --recommended Apply recommended system-level changes - (cyclone performance optimizations, pixi, obelisk aliases) + --dev-setup Setup for development of Obelisk (Docker-based). + --downstream-setup Setup for downstream use of Obelisk (local install). - --basic Enables basic dependencies necessary for Obelisk locally - --cyclone-perf Enables cyclone performance optimizations - --leap Enables LEAP hand dependencies - --zed Enables ZED SDK + Hardware: + --zed Enable ZED SDK + --unitree Enable Unitree interfaces - --docker-install Install Docker and nvidia-container-toolkit - --install-sys-deps-docker Installs system dependencies in Docker - - --config-groups Configures user groups associated with hardware - - --install-sys-deps Installs system dependencies - --source-ros Sources base ROS in ~/.bashrc (only used if --install-sys-deps) - - --pixi Install pixi - --obk-aliases Add obelisk aliases to the ~/.bash_aliases file + Simulation: + --mujoco Enable Mujoco simulation --help Display this help message and exit ``` -Some guidance/recommendations on choosing flags: -* If you don't have Docker on your machine, use the `--docker-install` flag -* If you are not using Docker, then you should use `--install-sys-deps` -* If you are using Docker, but not pixi, you should also use `--install-sys-deps` -* If you are using Docker, use `--install-sys-deps-docker` if and only if you are **not** using pixi within the container. -* If you are using pixi, regardless of whether you are using Docker, you should **not** use the `--basic` flag (note: you may have to manually install `mesa-common-dev`) -* If you are not using pixi or conda, you should probably use `--source-ros` along with `--install-sys-deps` -* We believe using `--pixi` will make your life easier, but you don't have to use it -* We strongly recommend using `--obk-aliases` and `--cyclone-perf` -* If you are using the LEAP Hand, use `--leap` +Guidance: +* For developing Obelisk inside the dev container: `source setup.sh --dev-setup` (plus any hardware flags you need). +* For consuming Obelisk in a downstream project on your local machine: `source setup.sh --downstream-setup`. * If you are using ZED cameras, use `--zed`. Additionally, you will need to adjust the `udev` permissions on your host machine if you want to use the ZED cameras in a Docker container with a non-root user (if you are acting as root in your container, you probably don't need to do this next step): * You should run `--config-groups` only if your local user isn't set up to interface with hardware of interest. If you are going to develop in a Docker container, the group settings will be set for you **without** setting this flag. If you are cloning this repo in a Docker container and running the setup script, setup will **only be complete if you do set this flag**. ``` @@ -79,28 +62,15 @@ newgrp docker ``` ### Building Obelisk ROS Packages -Next, since Obelisk acts as a dependency for a downstream ROS2 project, you have to build it. You can either build it on your local filesystem or in a virtual environment that we manage using `pixi`. +Once dependencies are in place, build the Obelisk ROS2 packages with `obk-build` (a colcon wrapper installed by `--dev-setup` / `--downstream-setup`). The recommended workflow is to develop inside the dev container: -* If you are building it on your local filesystem, you need some minimal set of local dependencies. These should have been installed in the previous step. - - If you have run the initial setup script with the `--obk-aliases` flag, then running - ``` - obk-build - ``` - will build the Obelisk ROS2 libraries on your local filesystem. -* If you are building it using `pixi`, we recommend using the Docker containers we provide: - ``` - # enter the docker container - cd docker - docker compose -f [docker-compose.yml | docker-compose-no-gpu.yml] run --build obelisk - - # build and enter the pixi shell - pixi shell -e [dev | dev-no-gpu] +``` +cd docker +docker compose -f docker-compose.yml run --build obelisk # GPU +docker compose -f docker-compose-no-gpu.yml run --build obelisk # no GPU +``` - # build in the pixi shell - pixi run ros-clean - obk-build - ``` +Inside the container, `obk` builds (first time) and sources the workspace; `obk-build` rebuilds after edits; `obk-clean` nukes `build/install/log` if you need a clean slate. ### Using Obelisk Once Obelisk has been built, you can use it. If you have set up Obelisk using the `--obk-aliases` flag, we provide a very useful command: diff --git a/docker/config_groups.sh b/docker/config_groups.sh index a29e1c4a..366a7946 100644 --- a/docker/config_groups.sh +++ b/docker/config_groups.sh @@ -1,7 +1,6 @@ #!/bin/bash # --- script flags --- # -leap=false zed=false for arg in "$@"; do diff --git a/docker/docker-compose-ci.yml b/docker/docker-compose-ci.yml index b7f53992..363081f8 100644 --- a/docker/docker-compose-ci.yml +++ b/docker/docker-compose-ci.yml @@ -8,7 +8,6 @@ services: UID: $UID GID: $UID OBELISK_ROOT: $OBELISK_ROOT - OBELISK_DOCKER_PIXI: true dockerfile: Dockerfile network_mode: host ipc: host @@ -18,7 +17,6 @@ services: UID: $UID GID: $UID OBELISK_ROOT: $OBELISK_ROOT - OBELISK_DOCKER_PIXI: true QT_X11_NO_MITSHM: 1 security_opt: - seccomp=unconfined diff --git a/docker/docker-compose-no-gpu.yml b/docker/docker-compose-no-gpu.yml index aca7cd77..cf2f6674 100644 --- a/docker/docker-compose-no-gpu.yml +++ b/docker/docker-compose-no-gpu.yml @@ -10,9 +10,6 @@ services: OBELISK_ROOT: $OBELISK_ROOT OBELISK_DOCKER_BASIC: $OBELISK_DOCKER_BASIC OBELISK_DOCKER_CYCLONE_PERF: $OBELISK_DOCKER_CYCLONE_PERF - OBELISK_DOCKER_LEAP: $OBELISK_DOCKER_LEAP - OBELISK_DOCKER_PIXI: $OBELISK_DOCKER_PIXI - OBELISK_DOCKER_GROUP_LEAP: $OBELISK_DOCKER_GROUP_LEAP dockerfile: Dockerfile network_mode: host ipc: host @@ -24,9 +21,6 @@ services: OBELISK_ROOT: $OBELISK_ROOT OBELISK_DOCKER_BASIC: $OBELISK_DOCKER_BASIC OBELISK_DOCKER_CYCLONE_PERF: $OBELISK_DOCKER_CYCLONE_PERF - OBELISK_DOCKER_LEAP: $OBELISK_DOCKER_LEAP - OBELISK_DOCKER_PIXI: $OBELISK_DOCKER_PIXI - OBELISK_DOCKER_GROUP_LEAP: $OBELISK_DOCKER_GROUP_LEAP QT_X11_NO_MITSHM: 1 security_opt: - seccomp=unconfined diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 664ad855..b6b0a77e 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -10,7 +10,6 @@ services: OBELISK_ROOT: $OBELISK_ROOT OBELISK_DOCKER_BASIC: $OBELISK_DOCKER_BASIC OBELISK_DOCKER_CYCLONE_PERF: $OBELISK_DOCKER_CYCLONE_PERF - OBELISK_DOCKER_LEAP: $OBELISK_DOCKER_LEAP OBELISK_DOCKER_ZED: $OBELISK_DOCKER_ZED OBELISK_DOCKER_GROUP_ZED: $OBELISK_DOCKER_GROUP_ZED OBELISK_DOCKER_MUJOCO: $OBELISK_DOCKER_MUJOCO @@ -26,7 +25,6 @@ services: OBELISK_ROOT: $OBELISK_ROOT OBELISK_DOCKER_BASIC: $OBELISK_DOCKER_BASIC OBELISK_DOCKER_CYCLONE_PERF: $OBELISK_DOCKER_CYCLONE_PERF - OBELISK_DOCKER_LEAP: $OBELISK_DOCKER_LEAP OBELISK_DOCKER_ZED: $OBELISK_DOCKER_ZED OBELISK_DOCKER_GROUP_ZED: $OBELISK_DOCKER_GROUP_ZED OBELISK_DOCKER_MUJOCO: $OBELISK_DOCKER_MUJOCO diff --git a/docs/source/development.md b/docs/source/development.md index 9d88ecc7..ac2cf08b 100644 --- a/docs/source/development.md +++ b/docs/source/development.md @@ -83,12 +83,12 @@ To launch a ROS stack we can use the following commands. In a seperate terminal, we can run a ROS stack with: ``` -obk-launch config= device= +obk-launch config= ``` Specifically, for the dummy examples this looks like: ``` -obk-launch config=dummy_cpp.yaml device=onboard +obk-launch config=dummy_cpp.yaml ``` All the documentation for the Obelisk terminal aliases can be found [here](obelisk_terminal_aliases.md). diff --git a/docs/source/getting_started.md b/docs/source/getting_started.md index f82029f2..cc0f522d 100644 --- a/docs/source/getting_started.md +++ b/docs/source/getting_started.md @@ -233,7 +233,7 @@ If you are using a stable version of Obelisk, you should only need to run this o Once Obelisk is activated and built we can run a basic example to make sure everything is working as expected. ``` -obk-launch config=dummy_cpp.yaml device=onboard +obk-launch config=dummy_cpp.yaml ``` At this point you should see a Mujoco simulation running. diff --git a/docs/source/obelisk_terminal_aliases.md b/docs/source/obelisk_terminal_aliases.md index 6d5b8eff..bcdeddcd 100644 --- a/docs/source/obelisk_terminal_aliases.md +++ b/docs/source/obelisk_terminal_aliases.md @@ -5,11 +5,11 @@ Launches the `obelisk_bringup.launch.py` with specified arguments. Usage: ``` -obk-launch config= device= auto_start= +obk-launch config= [auto_start=] [bag=] ``` Example: ``` -obk-launch config=example.yaml device=onboard auto_start=True +obk-launch config=example.yaml auto_start=True ``` ## `obk-build` diff --git a/docs/source/unitree_interfaces.md b/docs/source/unitree_interfaces.md index c919af61..3c0550fe 100644 --- a/docs/source/unitree_interfaces.md +++ b/docs/source/unitree_interfaces.md @@ -28,29 +28,49 @@ To avoid having the default FSM conflict with any user-specified FSM operating o where L1 refers to Layer 1, i.e. the layer button must be held down. Additionally, all buttons not passed to the fsm are passed through on a separate joystick node (`/obelisk/g1/joy_passthrough` by default). It is recommended that the user subscribe to this topic to use the controller, rather than the joystick topic (default `/obelisk/g1/joy`), to avoid interference with the fsm. -All of these buttons can be remapped as desired in the setup yaml. Here is a complete yaml, with all available options (omitted options will take default values): +All of these buttons can be remapped as desired in the setup yaml. The relevant pieces live in two +places: -``` +1. The **top-level `joystick:` block** configures the raw ROS `joy_node` that reads hardware joystick + events and publishes them on `/obelisk/g1/joy`. Only `on`, `pub_topic`, `sub_topic`, and standard + `joy_node` parameters (`device_id`, `device_name`, `deadzone`, `autorepeat_rate`, + `sticky_buttons`, `coalesce_interval_ms`) live here. +2. The **`obelisk_unitree_joystick` control entry** (under `control:`) translates raw button events + into FSM transitions and velocity commands. Its remap settings — button bindings and scale factors + — go in its own `params:` block. + +A complete config showing all available options (omitted options take defaults): + +```yaml +# 1. ROS joy_node — raw joystick → /obelisk/g1/joy joystick: on: True pub_topic: /obelisk/g1/joy sub_topic: /obelisk/g1/joy_feedback - ros_parameters: - v_x_scale: 0.5 - v_y_scale: 0.5 - w_z_scale: 0.5 - unitree_home_button: 116 // DL1 - user_pose_button: 127 // DR1 - low_level_ctrl_button: 128 // DD1 - high_level_ctrl_button: 117 // DU1 - damping_button: 12 // LT - estop: 15 // RT - vel_cmd_x: 11 // LY - vel_cmd_y: 10 // LX - vel_cmd_yaw: 14 // RY - layer: 4 // LB + +# 2. Obelisk unitree-joystick controller — buttons → FSM/vel commands +control: + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + params: + v_x_scale: 0.5 + v_y_scale: 0.5 + w_z_scale: 0.5 + axis_threshold: -0.1 + menu_button: 7 # MENU + unitree_home_button: 116 # DL1 + user_pose_button: 127 # DR1 + low_level_ctrl_button: 128 # DD1 + high_level_ctrl_button: 117 # DU1 + damping_button: 12 # LT + estop: 15 # RT + vel_cmd_x: 11 # LY + vel_cmd_y: 10 # LX + vel_cmd_yaw: 14 # RY + layer_button: 4 # LB + # publishers/subscribers/timers as in the example configs ``` -`scale` parameters refer to the multiplicative factor under which joystick inputs are scaled when passed to the unitree controller (or if the user controller is subscribed to the same message). +`scale` parameters refer to the multiplicative factor under which joystick inputs are scaled when passed to the unitree controller (or if the user controller is subscribed to the same message). Buttons are passed as integers, under the following map: ``` LT = 12 // 2 + AXIS_OFFSET, @@ -102,5 +122,5 @@ Note that to avoid conflicts between `button` indices and `axis` indices (and is ## Debugging You can also check what the default button mappings are by running: ``` -obk-launch config=unitree_fsm_debug.yaml device=onboard +obk-launch config=unitree_fsm_debug.yaml ``` \ No newline at end of file diff --git a/docs/source/using_obelisk.rst b/docs/source/using_obelisk.rst index 3138f187..07152d33 100644 --- a/docs/source/using_obelisk.rst +++ b/docs/source/using_obelisk.rst @@ -123,8 +123,8 @@ Estimator Code JointEncodersPassthroughEstimator(const std::string& name) : obelisk::ObeliskEstimator(name) { - this->RegisterSubscription( - "sub_sensor_setting", "sub_sensor", + this->RegisterObkSubscription( + "sub_sensor", std::bind(&JointEncodersPassthroughEstimator::JointEncoderCallback, this, std::placeholders::_1)); } @@ -167,9 +167,8 @@ Estimator Code """Initialize the joint encoders passthrough estimator.""" super().__init__(node_name) self.register_obk_subscription( - "sub_sensor_setting", - self.joint_encoder_callback, # type: ignore - key="subscriber_sensor", # key can be specified here or in the config file + key="sub_sensor", + callback=self.joint_encoder_callback, # type: ignore msg_type=JointEncoders, ) @@ -303,104 +302,124 @@ Obelisk nodes can be easily configured via a Obelisk configuration (yaml) file. .. code-block:: yaml config: dummy - onboard: - control: - - pkg: obelisk_control_cpp - executable: example_position_setpoint_controller - params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/dummy/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - estimation: - - pkg: obelisk_estimation_cpp - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/dummy/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - topic: /obelisk/dummy/sensor - msg_type: JointEncoders - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - - is_simulated: True - pkg: obelisk_sim_cpp - executable: obelisk_mujoco_robot - params: - ic_keyframe: ic - # callback_groups: - # publishers: - subscribers: - - ros_parameter: sub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - model_xml_path: dummy/dummy.xml - num_steps_per_viz: 5 - sensor_settings: - - topic: /obelisk/dummy/joint_encoders - dt: 0.001 - msg_type: ObkJointEncoders - sensor_names: - joint_pos: jointpos - joint_vel: jointvel - - topic: /obelisk/dummy/imu - dt: 0.002 - msg_type: ObkImu - sensor_names: - tip_acc_sensor: accelerometer - tip_gyro_sensor: gyro - tip_frame_sensor: framequat - - topic: /obelisk/dummy/framepose - dt: 0.002 - msg_type: ObkFramePose - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - viz_geoms: - dt: 1.0 - dummy_box: box - dummy_box_2: box - dummy_sphere: sphere + control: + - pkg: obelisk_control_cpp + executable: example_position_setpoint_controller + params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt + # callback_groups: + publishers: + - key: pub_ctrl + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + subscribers: + - key: sub_est + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + timers: + - key: timer_ctrl + timer_period_sec: 0.001 + callback_group: None + estimation: + - pkg: obelisk_estimation_cpp + executable: jointencoders_passthrough_estimator + # callback_groups: + publishers: + - key: pub_est + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + subscribers: + - key: sub_sensor + topic: /obelisk/dummy/sensor + history_depth: 10 + callback_group: None + timers: + - key: timer_est + timer_period_sec: 0.001 + callback_group: None + # sensing: + robot: + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + params: + ic_keyframe: ic + # callback_groups: + # publishers: + subscribers: + - key: sub_ctrl + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + model_xml_path: dummy/dummy.xml + num_steps_per_viz: 5 + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: ObkFramePose + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + viz_geoms: + dt: 1.0 + dummy_box: box + dummy_box_2: box + dummy_sphere: sphere + + + +Composing configs with ``include:`` +----------------------------------- +A launch YAML may pull in other YAMLs via an optional top-level ``include:`` list. The loader merges them before the launch system consumes the result, so you can factor out shared chunks (controller, estimator, joystick) into reusable files and assemble them differently for, say, sim and hardware variants. + +.. code-block:: yaml + + # dummy_composed.yaml — what you point obk-launch at + config: dummy_composed + include: + - dummy_composed_base.yaml # shared control + estimation + joystick + - dummy_composed_robot_sim.yaml # the sim-mode robot backend + # No other keys needed; everything comes from the included files. +The included files are themselves regular Obelisk YAMLs. They can also have ``include:`` lists of their own (recursion is allowed; cycles are caught and raise a ``RuntimeError``). + +**Path rules.** The primary file (the one named on ``obk-launch config=…``) resolves the same way it always did: absolute path used as-is, otherwise relative to ``share/obelisk_ros/config/``. Paths inside an ``include:`` directive resolve **relative to the directory of the YAML containing the include**. Absolute paths in include lists pass through unchanged. + +**Merge rules.** When merging an outer YAML over its includes: + +* The list-shaped sections (``control``, ``estimation``, ``robot``, ``sensing``) **concatenate** in include order, with the outer file's entries appended last. +* The dict-shaped sections (``joystick``, ``viz``) **deep-merge** with the outer file winning on per-key conflicts. +* Scalar keys (``config``, ``params_path``): the outer file wins. +* The ``include:`` key is stripped from the final result. + +A worked end-to-end example lives at ``obelisk_ws/src/obelisk_ros/config/dummy_composed*.yaml``. Breaking down the configuration file ------------------------------------ .. code-block:: yaml config: dummy - onboard: -First we give the name of this configuration (``dummy``), and which device this is running on. +First we give the name of this configuration (``dummy``). The remaining top-level keys (``control``, ``estimation``, ``robot``, ``sensing``, ``viz``, ``joystick``) hold the per-component settings. .. code-block:: yaml @@ -409,38 +428,38 @@ First we give the name of this configuration (``dummy``), and which device this executable: example_position_setpoint_controller params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt publishers: - - ros_parameter: pub_ctrl_setting + - key: pub_ctrl topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - key: "asdf" history_depth: 10 callback_group: None subscribers: - - ros_parameter: sub_est_setting + - key: sub_est topic: /obelisk/dummy/est - msg_type: EstimatedState history_depth: 10 callback_group: None timers: - - ros_parameter: timer_ctrl_setting + - key: timer_ctrl timer_period_sec: 0.001 callback_group: None Now we configure our Controller node. ``pkg`` gives the name of the package containing the Obelisk node, and ``executable`` tells us what the name is of the executable with ``main`` in it. ``params_path`` (optional) is a string parameter that allows you to specify a file path that can be accessed within your code. This is useful for things like accessing controller gains that are specfied through a seperate yaml file. Note that there is no convention on how the file path is processed as that is up to you as the user. -Now we need to configure all of the Components in this node. Publishers and subscribers have the following options. -- ``ros_parameter`` gives the string name of the ros parameter declared in the code. This is how the launch file gets these options to the correct node. +The launch-side producer bundles the ``publishers``, ``subscribers``, ``timers``, and ``callback_groups`` sections of each node into a single ROS string parameter named ``obelisk_settings`` whose value is a YAML document. Each Obelisk node parses that YAML at ``on_configure`` time and looks up its registered components by ``key``. (For backward compatibility with older configs, the producer still accepts ``ros_parameter: _setting`` and derives ``key`` by stripping the ``_setting`` suffix.) + +Publishers and subscribers have the following options: + +- ``key`` gives the string key associated with the component. It must match the key the node registered via ``register_obk_publisher`` / ``register_obk_subscription`` (Python) or ``RegisterObkPublisher`` / ``RegisterObkSubscription`` (C++). - ``topic`` gives the string topic name that will either be published or subscribed to. -- ``msg_type`` gives the type of message we want to publish or subscribe to. **Note this is only ever used in the Python implementation. In C++ the message type must be specified in the code as a templated parameter.** -- ``key`` gives the string key associated with the component if not already specified in the code implementation. **Note this is only ever used in the Python implementation. In C++, the key must be specified during component declaration time.** -- ``history_depth`` (optional) gives the number of messages to hold in the queue before deleting additional messages. If this not set we the use the default value of 10. +- ``history_depth`` (optional) gives the number of messages to hold in the queue before deleting additional messages. If this is not set we use the default value of 10. - ``callback_group`` (optional) gives the string name of the callback group to use. The callback groups can be configured within this configuration file. If no value is specified, then the node's default callback group is used. -Timers have the following options. +Note: the message type is no longer specified in YAML for normal pub/sub entries — it is bound at code-registration time (the ``msg_type`` argument to ``register_obk_publisher`` in Python, or the template parameter in C++). -- ``ros_parameter`` gives the string name of the ros parameter declared in the code. +Timers have the following options: + +- ``key`` gives the string key associated with the timer (matches the key used in ``register_obk_timer`` / ``RegisterObkTimer``). - ``timer_period_sec`` gives the period of the timer in seconds. - ``callback_group`` (optional) gives the string name of the callback group to use. The callback groups can be configured within this configuration file. If no value is specified, then the node's default callback group is used. @@ -452,19 +471,17 @@ This is repeated for every non-system node in the block diagram, which in this c pkg: obelisk_estimation_py executable: jointencoders_passthrough_estimator publishers: - - ros_parameter: pub_est_setting + - key: pub_est topic: /obelisk/dummy/est - msg_type: EstimatedState history_depth: 10 callback_group: None subscribers: - - ros_parameter: sub_sensor_setting + - key: sub_sensor topic: /obelisk/dummy/sensor - msg_type: JointEncoders history_depth: 10 callback_group: None timers: - - ros_parameter: timer_est_setting + - key: timer_est timer_period_sec: 0.001 callback_group: None @@ -482,9 +499,8 @@ Lastly, we need to configure the ``robot`` (aka, the system). # callback_groups: # publishers: subscribers: - - ros_parameter: sub_ctrl_setting + - key: sub_ctrl topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint history_depth: 10 callback_group: None sim: @@ -500,7 +516,7 @@ Lastly, we need to configure the ``robot`` (aka, the system). joint_vel: jointvel - topic: /obelisk/dummy/imu dt: 0.002 - msg_type: ObkImu + msg_type: Imu sensor_names: tip_acc_sensor: accelerometer tip_gyro_sensor: gyro @@ -511,18 +527,18 @@ Lastly, we need to configure the ``robot`` (aka, the system). sensor_names: tip_pos_sensor: framepos tip_orientation_sensor: framequat - viz_geoms: - dt: 1.0 - dummy_box: box - dummy_box_2: box - dummy_sphere: sphere + viz_geoms: + dt: 1.0 + dummy_box: box + dummy_box_2: box + dummy_sphere: sphere ``is_simulated`` marks if we are running on hardware or in simulation. ``pkg`` and ``executable`` are as before. ``ic_keyframe`` (optional) in the params section tells the simulation which keyframe to use for an initial condition. Now, we must configure the Components of the node, which in this example is just a subscriber. These Components have all the same options as the non-system Components given above. -Lastly, since this is a simulation, we must provide the simulator with all relevant information. Here, we are using the Mujoco simulation interface. The new settings here are: +Lastly, since this is a simulation, we must provide the simulator with all relevant information. Here, we are using the Mujoco simulation interface. The simulator's settings are passed as a YAML-formatted string in their own ROS parameter (``mujoco_setting``), which is why the ``sim:`` entry still uses ``ros_parameter:``. The new settings here are: - ``num_steps_per_viz`` (optional) gives the number of steps to use between simulation rendering. If no value is provided, the default value of 8 steps will be used. @@ -530,15 +546,15 @@ Lastly, since this is a simulation, we must provide the simulator with all relev - ``msg_type`` gives the ROS message type associated with the given group of sensors. - ``dt`` gives the sensor publishing period in seconds. -- Each element under ``sensor_names`` follows ``sensor_name: sensor_type`` **Note that the Mujoco XML must have all the sensors listed in the Obelisk configuration file, if you request a sensor here that is not available in Mujoco, there will be an error.** All supported Mujoco sensors and corresponding Obelisk messages are listed below. +- Each element under ``sensor_names`` follows ``sensor_name: sensor_type`` **Note that the Mujoco XML must have all the sensors listed in the Obelisk configuration file, if you request a sensor here that is not available in Mujoco, there will be an error.** All supported Mujoco sensors and corresponding message types are listed below. -=============================================== ==================== -Mujoco sensor type Obelisk Message Type -=============================================== ==================== +=============================================== ============================ +Mujoco sensor type Message type (``msg_type``) +=============================================== ============================ ``jointpos`` and ``jointvel`` ``ObkJointEncoders`` -``accelerometer``, ``gyro``, and ``framequat`` ``ObkImu`` +``accelerometer``, ``gyro``, and ``framequat`` ``Imu`` (``sensor_msgs/Imu``) ``framepos`` and ``framequat`` ``ObkFramePose`` -=============================================== ==================== +=============================================== ============================ You may have multiple of the same type of sensor in the yaml. diff --git a/docs/source/visualization.md b/docs/source/visualization.md index 66897460..0fe21d40 100644 --- a/docs/source/visualization.md +++ b/docs/source/visualization.md @@ -1,6 +1,6 @@ # Visualization Obelisk comes with a suite of tools for visualizing the robot. Some of these include: -- Robot visualization nodes to display robots in Rviz or [Foxglove](https://docs.foxglove.dev/docs/introduction) +- Robot visualization nodes to display robots in Rviz ## Robot Visualization Oftentimes you may want to display a visual of a robot in a specified configuration, or see the motion of a robot throughout a trajectory, or maybe you want to check the estimated state vs the true state. In all of these cases we need to be able to see the robot. Obelisk provides two nodes to help with this: @@ -21,10 +21,9 @@ The `base_link_name` must match a link in the URDF and `joint_names` must all be ### Visualization Configuration Settings #### Example Configuration -``` +```yaml viz: on: True - viz_tool: rviz rviz_pkg: obelisk_ros rviz_config: basic_obk_config.rviz viz_nodes: @@ -35,18 +34,17 @@ The `base_link_name` must match a link in the URDF and `joint_names` must all be robot_topic: robot_description tf_prefix: g1 subscribers: - - ros_parameter: sub_viz_est_setting + - key: sub_est topic: estimated_state history_depth: 10 callback_group: None - non_obelisk: False publishers: - - ros_parameter: pub_viz_joint_setting + - key: pub_viz_joint topic: joint_states_g1 history_depth: 10 callback_group: None timers: - - ros_parameter: timer_viz_joint_setting + - key: time_joints timer_period_sec: 0.05 callback_group: None - pkg: obelisk_viz_cpp @@ -56,23 +54,22 @@ The `base_link_name` must match a link in the URDF and `joint_names` must all be robot_topic: go2_description tf_prefix: go2 subscribers: - - ros_parameter: sub_viz_est_setting + - key: sub_est topic: estimated_state2 history_depth: 10 callback_group: None - non_obelisk: False publishers: - - ros_parameter: pub_viz_joint_setting + - key: pub_viz_joint topic: joint_states_go2 history_depth: 10 callback_group: None timers: - - ros_parameter: timer_viz_joint_setting + - key: time_joints timer_period_sec: 0.05 callback_group: None ``` #### Breaking Down the Configuration -``` +```yaml viz: on: True rviz_pkg: obelisk_ros @@ -80,12 +77,11 @@ The `base_link_name` must match a link in the URDF and `joint_names` must all be ``` The visualization section starts with the `viz` tag. Then we have all the "global" visualization settings, i.e., all the settings that apply to everything, such as Rviz settings. - `on` is a boolean flag to turn on or off the visualizer and spin the nodes. If this is false, all the following settings are skipped. -- `viz_tool` (optional) selects which visualization tool to bring up. For now the only two supported options are `rviz` and `foxglove`. If not present, the default is `rviz`. -- `rviz_pkg` (optional) is the package where the Rviz configuration file is found. Not need if `viz_tool` is `foxglove`, but required for `rviz`. -- `rviz_config` (optional) is the name of the Rviz configuration file. ***Note that we assume this is stored in a folder named `rviz` in the package listed above.*** Be sure that this folder is "installed" when building ROS. Not need if `viz_tool` is `foxglove`, but required for `rviz`. +- `rviz_pkg` (optional) is the package where the Rviz configuration file is found. +- `rviz_config` (optional) is the name of the Rviz configuration file. ***Note that we assume this is stored in a folder named `rviz` in the package listed above.*** Be sure that this folder is "installed" when building ROS. Then, under `viz_nodes` we have a list of nodes and their settings. We will examine only one as they always have the same fields. -``` +```yaml - pkg: obelisk_viz_cpp executable: default_robot_viz robot_pkg: g1_description @@ -93,28 +89,27 @@ Then, under `viz_nodes` we have a list of nodes and their settings. We will exam robot_topic: robot_description tf_prefix: g1 subscribers: - - ros_parameter: sub_viz_est_setting - topic: estimated_state - history_depth: 10 - callback_group: None - non_obelisk: False + - key: sub_est + topic: estimated_state + history_depth: 10 + callback_group: None publishers: - - ros_parameter: pub_viz_joint_setting - topic: joint_states_g1 - history_depth: 10 - callback_group: None + - key: pub_viz_joint + topic: joint_states_g1 + history_depth: 10 + callback_group: None timers: - - ros_parameter: timer_viz_joint_setting - timer_period_sec: 0.05 - callback_group: None + - key: time_joints + timer_period_sec: 0.05 + callback_group: None ``` - `pkg` gives the package where the executable is. - `executable` is the name of the executable. - `robot_pkg` is the name of the package where all the robot files are stored. - `urdf` is the name of the URDF file. ***Note that we assume the urdf is stored in a folder named `rviz` in the `robot_pkg`.*** -- `robot_topic` (optional) is the name of the topic where the robot description (urdf) will be published for Rviz. This remaps the `robot_description` topic give in the [`robot_state_publisher`](https://index.ros.org/p/robot_state_publisher/github-ros-robot_state_publisher/#humble). +- `robot_topic` (optional) is the name of the topic where the robot description (urdf) will be published for Rviz. This remaps the `robot_description` topic given in the [`robot_state_publisher`](https://index.ros.org/p/robot_state_publisher/github-ros-robot_state_publisher/#humble). - `tf_prefix` (optional) is the prefix on all of the transform messages put out by the `robot_state_publisher`. In the `ObeliskVizRobotDefault` implementation this is accounted for. If you extend `ObeliskVizRobot` manually, then you must be sure to use this prefix (given as a node parameter) to prefix the base transform. -- Finally we have all the normal component settings, which we will not go over here. +- Finally we have the standard `publishers` / `subscribers` / `timers` component settings. The keys `sub_est`, `pub_viz_joint`, and `time_joints` are the defaults the `ObeliskVizRobot` constructor registers — see `obelisk/cpp/viz/include/obelisk_viz_robot.h` if you need to override them. The `robot_topic` and `tf_prefix` are mostly useful when you have multiple robots to display. By changing these options we can prevent name clashes. diff --git a/obelisk/cpp/hardware/include/jointencoders_passthrough_estimator.h b/obelisk/cpp/hardware/include/jointencoders_passthrough_estimator.h index e556172d..b0619472 100644 --- a/obelisk/cpp/hardware/include/jointencoders_passthrough_estimator.h +++ b/obelisk/cpp/hardware/include/jointencoders_passthrough_estimator.h @@ -10,7 +10,7 @@ class JointEncodersPassthroughEstimator : obelisk::ObeliskEstimator(name) { this->RegisterObkSubscription( - "sub_sensor_setting", "sub_sensor", + "sub_sensor", std::bind(&JointEncodersPassthroughEstimator::JointEncoderCallback, this, std::placeholders::_1)); } diff --git a/obelisk/cpp/hardware/include/position_setpoint_controller.h b/obelisk/cpp/hardware/include/position_setpoint_controller.h index 20395a28..5209b2eb 100644 --- a/obelisk/cpp/hardware/include/position_setpoint_controller.h +++ b/obelisk/cpp/hardware/include/position_setpoint_controller.h @@ -42,11 +42,12 @@ class PositionSetpointController : public obelisk::ObeliskControllerRegisterObkSubscription( - "joystick_sub_setting", "joystick_sub", + "joystick_sub", std::bind(&PositionSetpointController::JoystickCallback, this, std::placeholders::_1)); // ----- Joystick Publisher ----- // - this->RegisterObkPublisher("joystick_feedback_setting", "joystick_pub"); + // Key must match what the launch-side producer derives from `joystick_feedback_setting` in the YAML. + this->RegisterObkPublisher("joystick_feedback"); } protected: @@ -93,7 +94,7 @@ class PositionSetpointController : public obelisk::ObeliskControllerGetPublisher("joystick_pub")->publish(joy_feedback); + this->GetPublisher("joystick_feedback")->publish(joy_feedback); } } diff --git a/obelisk/cpp/hardware/include/unitree_example_controller.h b/obelisk/cpp/hardware/include/unitree_example_controller.h index 861bf47d..e0b6081b 100644 --- a/obelisk/cpp/hardware/include/unitree_example_controller.h +++ b/obelisk/cpp/hardware/include/unitree_example_controller.h @@ -31,7 +31,7 @@ namespace obelisk { } // ----- Joystick Subscriber ----- // this->RegisterObkSubscription( - "joystick_sub_setting", "joystick_sub", + "joystick_sub", std::bind(&UnitreeExampleController::JoystickCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM(this->get_logger(), "New Joint Index: " << joint_idx_ << ", Expected Joint: " << joint_names_[joint_idx_]); diff --git a/obelisk/cpp/hardware/include/unitree_example_estimator.h b/obelisk/cpp/hardware/include/unitree_example_estimator.h index 195289b0..d6a27936 100644 --- a/obelisk/cpp/hardware/include/unitree_example_estimator.h +++ b/obelisk/cpp/hardware/include/unitree_example_estimator.h @@ -11,7 +11,7 @@ namespace obelisk { : obelisk::ObeliskEstimator(name) { this->RegisterObkSubscription( - "sub_sensor_setting", "sub_sensor", + "sub_sensor", std::bind(&UnitreeExampleEstimator::JointEncoderCallback, this, std::placeholders::_1)); } diff --git a/obelisk/cpp/hardware/include/unitree_go2_estimator.h b/obelisk/cpp/hardware/include/unitree_go2_estimator.h index 47c49d7d..e79d88bb 100644 --- a/obelisk/cpp/hardware/include/unitree_go2_estimator.h +++ b/obelisk/cpp/hardware/include/unitree_go2_estimator.h @@ -1,4 +1,5 @@ #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" #include "obelisk_estimator.h" #include "obelisk_ros_utils.h" @@ -11,11 +12,11 @@ namespace obelisk { : obelisk::ObeliskEstimator(name) { this->RegisterObkSubscription( - "sub_sensor_setting", "sub_sensor", + "sub_sensor", std::bind(&UnitreeGo2Estimator::JointEncoderCallback, this, std::placeholders::_1)); - this->RegisterObkSubscription( - "sub_imu_setting", "imu_sensor", + this->RegisterObkSubscription( + "sub_imu", std::bind(&UnitreeGo2Estimator::TorsoIMUCallback, this, std::placeholders::_1)); } @@ -26,7 +27,7 @@ namespace obelisk { joint_names_ = msg.joint_names; } - void TorsoIMUCallback(const obelisk_sensor_msgs::msg::ObkImu& msg) { + void TorsoIMUCallback(const sensor_msgs::msg::Imu& msg) { pose_ = {0., 0., 0., msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w}; omega_ = {0., 0., 0., msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z}; } diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index db6a2886..f0092b4f 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -181,8 +181,8 @@ namespace obelisk { // Register publishers - this->RegisterObkPublisher("pub_exec_fsm_setting", pub_exec_fsm_key_); - this->RegisterObkPublisher("pub_joy_passthrough_setting", pub_joy_passthrough_key_); + this->RegisterObkPublisher(pub_exec_fsm_key_); + this->RegisterObkPublisher(pub_joy_passthrough_key_); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick node has been initialized."); } @@ -424,8 +424,8 @@ namespace obelisk { }; // Publisher key - const std::string pub_exec_fsm_key_ = "pub_exec_fsm_key"; - const std::string pub_joy_passthrough_key_ = "pub_joy_passthrough_key"; + const std::string pub_exec_fsm_key_ = "pub_exec_fsm"; + const std::string pub_joy_passthrough_key_ = "pub_joy_passthrough"; // Hold velocity bounds float v_x_scale_; diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index a1dd7cf2..7dca3327 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -3,7 +3,6 @@ #include "obelisk_robot.h" #include "obelisk_ros_utils.h" #include "obelisk_sensor_msgs/msg/obk_joint_encoders.h" -#include "obelisk_sensor_msgs/msg/obk_imu.h" #include "obelisk_control_msgs/msg/pd_feed_forward.h" #include "obelisk_control_msgs/msg/execution_fsm.hpp" #include "obelisk_control_msgs/msg/velocity_command.hpp" @@ -54,15 +53,15 @@ namespace obelisk { high_level_ctrl_engaged_ = this->get_parameter("init_high_level").as_bool(); // Additional Publishers - this->RegisterObkPublisher("pub_sensor_setting", pub_joint_state_key_); - this->RegisterObkPublisher("pub_imu_setting", pub_imu_state_key_); + this->RegisterObkPublisher(pub_joint_state_key_); + this->RegisterObkPublisher(pub_imu_state_key_); // Register Execution FSM Subscriber this->RegisterObkSubscription( - "sub_fsm_setting", sub_fsm_key_, std::bind(&ObeliskUnitreeInterface::TransitionFSM, this, std::placeholders::_1)); + sub_fsm_key_, std::bind(&ObeliskUnitreeInterface::TransitionFSM, this, std::placeholders::_1)); // Register High Level Control Subscriber this->RegisterObkSubscription( - "sub_vel_cmd_setting", sub_UNITREE_VEL_CTRL_key_, std::bind(&ObeliskUnitreeInterface::ApplyVelCmd, this, std::placeholders::_1)); + sub_UNITREE_VEL_CTRL_key_, std::bind(&ObeliskUnitreeInterface::ApplyVelCmd, this, std::placeholders::_1)); // ---------- Default PD gains ---------- // this->declare_parameter>("default_kp"); @@ -102,7 +101,7 @@ namespace obelisk { startup_time_ = this->now(); // NOTE: InitializeLogging and timer registration are deferred to on_activate // because they call pure virtual methods that aren't available during construction. - this->RegisterObkTimer("timer_logging_setting", timer_logging_key_, + this->RegisterObkTimer(timer_logging_key_, std::bind(&ObeliskUnitreeInterface::WriteMotorData, this)); } @@ -319,11 +318,14 @@ namespace obelisk { std::vector kd_damping_; // Keys - const std::string sub_fsm_key_ = "sub_exec_fsm_key"; - const std::string sub_UNITREE_VEL_CTRL_key_ = "sub_vel_cmd_key"; - const std::string pub_joint_state_key_ = "joint_state_pub"; - const std::string pub_imu_state_key_ = "imu_state_pub"; - const std::string timer_logging_key_ = "timer_logging_key"; + // These key strings match the names the launch-side producer derives from + // `ros_parameter: _setting` entries in the YAML configs (suffix `_setting` stripped), + // so existing YAMLs migrate to the obelisk_settings mechanism without explicit `key:` edits. + const std::string sub_fsm_key_ = "sub_fsm"; + const std::string sub_UNITREE_VEL_CTRL_key_ = "sub_vel_cmd"; + const std::string pub_joint_state_key_ = "pub_sensor"; + const std::string pub_imu_state_key_ = "pub_imu"; + const std::string timer_logging_key_ = "timer_logging"; // Logging bool logging_; diff --git a/obelisk/cpp/hardware/sensing/zed/zed2_sensors_interface.h b/obelisk/cpp/hardware/sensing/zed/zed2_sensors_interface.h index e5c433ef..a4416f6f 100644 --- a/obelisk/cpp/hardware/sensing/zed/zed2_sensors_interface.h +++ b/obelisk/cpp/hardware/sensing/zed/zed2_sensors_interface.h @@ -61,7 +61,7 @@ class ObeliskZed2Sensors : public obelisk::ObeliskSensor { this->set_camera_params(params_path); // Register publisher for ZED2 cameras - this->RegisterObkPublisher("pub_img_setting", pub_img_key_); + this->RegisterObkPublisher(pub_img_key_); } /** diff --git a/obelisk/cpp/obelisk_cpp/CMakeLists.txt b/obelisk/cpp/obelisk_cpp/CMakeLists.txt index cfc81360..641624df 100644 --- a/obelisk/cpp/obelisk_cpp/CMakeLists.txt +++ b/obelisk/cpp/obelisk_cpp/CMakeLists.txt @@ -22,6 +22,9 @@ find_package(nav_msgs REQUIRED) # --------- Viz Messages --------- # find_package(visualization_msgs REQUIRED) +# ------- yaml-cpp (required for obelisk_settings parsing) ------- # +find_package(yaml-cpp REQUIRED) + # ------- Mujoco (optional) ------- # # Read from environment variable, default to ON if not set if(DEFINED ENV{OBELISK_USE_MUJOCO}) @@ -35,7 +38,6 @@ message(STATUS "USE_MUJOCO option set to: ${USE_MUJOCO}") if(USE_MUJOCO) find_package(Eigen3 REQUIRED) - find_package(yaml-cpp REQUIRED) include(FetchContent) set(MUJOCO_VERSION "3.1.6" CACHE STRING "mujoco version") @@ -80,14 +82,17 @@ add_library(ObkCore INTERFACE) add_library(Obelisk::Core ALIAS ObkCore) # Namespaced alias target_include_directories(ObkCore INTERFACE ${CORE_INC}) +# yaml-cpp is required by ObeliskNode for parsing the obelisk_settings YAML parameter. +target_link_libraries(ObkCore INTERFACE yaml-cpp) + # Conditionally add Mujoco dependencies if(USE_MUJOCO) target_include_directories(ObkCore INTERFACE ${mujoco_SOURCE_DIR}/include ${LIDAR_INC}) - target_link_libraries(ObkCore INTERFACE mujoco-lib glfw Eigen3::Eigen yaml-cpp) + target_link_libraries(ObkCore INTERFACE mujoco-lib glfw Eigen3::Eigen) # Define compile definition for conditional compilation target_compile_definitions(ObkCore INTERFACE OBELISK_USE_MUJOCO) - + message(STATUS "ObkCore built WITH Mujoco support") else() message(STATUS "ObkCore built WITHOUT Mujoco support") diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_controller.h b/obelisk/cpp/obelisk_cpp/include/obelisk_controller.h index 1e5b477d..27f247a2 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_controller.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_controller.h @@ -26,11 +26,10 @@ namespace obelisk { : ObeliskNode(name), ctrl_key_(ctrl_key), est_key_(est_key), timer_key_(timer_key) { // Register all components - this->RegisterObkTimer("timer_ctrl_setting", timer_key_, - std::bind(&ObeliskController::ComputeControl, this)); - this->RegisterObkPublisher("pub_ctrl_setting", ctrl_key_); + this->RegisterObkTimer(timer_key_, std::bind(&ObeliskController::ComputeControl, this)); + this->RegisterObkPublisher(ctrl_key_); this->RegisterObkSubscription( - "sub_est_setting", est_key_, std::bind(&ObeliskController::UpdateXHat, this, std::placeholders::_1)); + est_key_, std::bind(&ObeliskController::UpdateXHat, this, std::placeholders::_1)); } /** diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_estimator.h b/obelisk/cpp/obelisk_cpp/include/obelisk_estimator.h index f2e7f975..9650375e 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_estimator.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_estimator.h @@ -24,9 +24,8 @@ namespace obelisk { : ObeliskNode(name), est_pub_key_(est_pub_key), est_timer_key_(est_timer_key) { // Register all components - this->RegisterObkPublisher("pub_est_setting", est_pub_key_); - this->RegisterObkTimer("timer_est_setting", est_timer_key_, - std::bind(&ObeliskEstimator::ComputeStateEstimate, this)); + this->RegisterObkPublisher(est_pub_key_); + this->RegisterObkTimer(est_timer_key_, std::bind(&ObeliskEstimator::ComputeStateEstimate, this)); } /** diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 1303d08d..ae448149 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -61,14 +61,26 @@ namespace obelisk { on_configure(const rclcpp_lifecycle::State& prev_state) { this->ObeliskSimRobot::on_configure(prev_state); - // Read in the config string - std::string mujoco_setting = this->get_parameter("mujoco_setting").as_string(); - auto mujoco_config_map = this->ParseConfigStr(mujoco_setting); - - // Get config params - xml_path_ = GetXMLPath(mujoco_config_map); // Required - std::string robot_pkg = GetRobotPackage(mujoco_config_map); // Optional - // Search for the model + // Read in the YAML-encoded mujoco settings + const std::string mujoco_setting = this->get_parameter("mujoco_setting").as_string(); + YAML::Node mujoco_cfg; + try { + mujoco_cfg = YAML::Load(mujoco_setting); + } catch (const YAML::Exception& e) { + throw std::runtime_error(std::string("Failed to parse mujoco_setting YAML: ") + e.what()); + } + + // Required: model XML path + if (!mujoco_cfg["model_xml_path"]) { + throw std::runtime_error("No model XML path was provided in mujoco_setting."); + } + xml_path_ = std::filesystem::path(mujoco_cfg["model_xml_path"].as()); + + // Optional: robot package — used to resolve XML path relative to a ROS2 package's share dir. + std::string robot_pkg = "None"; + if (mujoco_cfg["robot_pkg"]) { + robot_pkg = mujoco_cfg["robot_pkg"].as(); + } if (!std::filesystem::exists(xml_path_)) { if (robot_pkg != "None" && robot_pkg != "none") { std::string share_directory = ament_index_cpp::get_package_share_directory(robot_pkg); @@ -83,15 +95,18 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "XML Path:" << xml_path_); - num_steps_per_viz_ = GetNumStepsPerViz(mujoco_config_map); + num_steps_per_viz_ = mujoco_cfg["num_steps_per_viz"] + ? mujoco_cfg["num_steps_per_viz"].as() + : STEPS_PER_VIZ_DEFAULT; configuration_complete_ = true; - ParseSensorString(mujoco_config_map.at("sensor_settings")); - - try { - ParseVizString(mujoco_config_map.at("viz_geoms")); - } catch (const std::exception& e) { + if (mujoco_cfg["sensor_settings"]) { + ParseSensors(mujoco_cfg["sensor_settings"]); + } + if (mujoco_cfg["viz_geoms"]) { + ParseVizGeoms(mujoco_cfg["viz_geoms"]); + } else { RCLCPP_INFO_STREAM(this->get_logger(), "No geoms to visualize in the simulator."); } @@ -386,56 +401,34 @@ namespace obelisk { return retval; } - // --------------------------------------------- // - // ----------- Config String Helpers ----------- // - // --------------------------------------------- // - std::filesystem::path GetXMLPath(const std::map& config_map) { - try { - std::string path = config_map.at("model_xml_path"); - return std::filesystem::path(path); - } catch (const std::exception& e) { - throw std::runtime_error("No model XML path was provided!"); - } - } + // ------------------------------------------ // + // ----------- Sensor / viz parse ----------- // + // ------------------------------------------ // - std::string GetRobotPackage(const std::map& config_map) { - try { - return config_map.at("robot_pkg"); - } catch (const std::exception& e) { - return "None"; + /** + * @brief Resolve the ray-caster scan config for a sensor entry. + * + * Prefers an inline ``scan_config:`` map on the entry. If that is absent, falls back to + * loading the YAML file at ``config_path:`` (legacy form). Throws if neither is provided. + */ + static YAML::Node ResolveScanConfig(const YAML::Node& entry, const std::string& config_path) { + if (entry["scan_config"] && entry["scan_config"].IsMap()) { + return entry["scan_config"]; } - } - - int GetNumStepsPerViz(const std::map& config_map) { - try { - int num_steps = std::stoi(config_map.at("num_steps_per_viz")); - return num_steps; - } catch (const std::exception& e) { - return STEPS_PER_VIZ_DEFAULT; + if (!config_path.empty()) { + return YAML::LoadFile(config_path); } + throw std::runtime_error( + "RayCaster sensor (ObkScan/PointCloud2/DepthImage) requires either an inline " + "`scan_config:` block on the sensor entry or a legacy `config_path:` pointing to a YAML file."); } - void ParseSensorString(std::string sensor_settings) { - // Remove {} and [] - sensor_settings.erase(std::remove(sensor_settings.begin(), sensor_settings.end(), '{'), - sensor_settings.end()); - sensor_settings.erase(std::remove(sensor_settings.begin(), sensor_settings.end(), '}'), - sensor_settings.end()); - sensor_settings.erase(std::remove(sensor_settings.begin(), sensor_settings.end(), '['), - sensor_settings.end()); - sensor_settings.erase(std::remove(sensor_settings.begin(), sensor_settings.end(), ']'), - sensor_settings.end()); - - // Break the string by the "+"s. - const std::string group_delim = "+"; - std::vector sensor_groups; - while (!sensor_settings.empty()) { - size_t plus_idx = sensor_settings.find(group_delim); - if (plus_idx == std::string::npos) { - plus_idx = sensor_settings.length(); - } - sensor_groups.emplace_back(sensor_settings.substr(0, plus_idx)); - sensor_settings.erase(0, plus_idx + group_delim.length()); + /** + * @brief Walk the YAML ``sensor_settings`` sequence and create a publisher + timer per entry. + */ + void ParseSensors(const YAML::Node& sensor_settings) { + if (!sensor_settings || !sensor_settings.IsSequence()) { + return; } // Wait for mujoco setup to complete @@ -446,88 +439,39 @@ namespace obelisk { callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - for (auto group : sensor_groups) { - // Create a new map between the names and their string values - const std::string setting_delim = "|"; - const std::string val_delim = "="; - std::map setting_map; - - size_t val_idx; - while (!group.empty()) { - val_idx = group.find(setting_delim); - if (val_idx == std::string::npos) { - val_idx = group.length(); - } - - size_t setting_idx = group.find(val_delim); - if (setting_idx == std::string::npos) { - throw std::runtime_error("Invalid sensor setting string!"); - } - - std::string setting = group.substr(0, setting_idx); - std::string val = group.substr(setting_idx + 1, val_idx - (setting_idx + 1)); - setting_map.emplace(setting, val); - group.erase(0, val_idx + val_delim.length()); - } - - // The dt for the timer - double dt = -1; - try { - dt = std::stod(setting_map.at("dt")); - if (dt <= 0) { - throw std::runtime_error("Invalid dt. Must be > 0."); - } - } catch (const std::exception& e) { + for (const auto& entry : sensor_settings) { + if (!entry["dt"]) { throw std::runtime_error("No dt provided for a sensor!"); } + const double dt = entry["dt"].as(); + if (dt <= 0) { + throw std::runtime_error("Invalid dt. Must be > 0."); + } - // The topic for the publisher - std::string topic; - try { - topic = setting_map.at("topic"); - } catch (const std::exception& e) { + if (!entry["topic"]) { throw std::runtime_error("No topic provided for a sensor!"); } + const std::string topic = entry["topic"].as(); + const int depth = entry["history_depth"] ? entry["history_depth"].as() : 10; - const int depth = this->ObeliskNode::GetHistoryDepth(setting_map); - - std::string sensor_type; - try { - sensor_type = setting_map.at("msg_type"); - } catch (const std::exception& e) { + if (!entry["msg_type"]) { throw std::runtime_error("No sensor type provided for a sensor!"); } + const std::string sensor_type = entry["msg_type"].as(); - // Check for config path std::string sensor_config_path = ""; - if (auto it = setting_map.find("config_path"); it != setting_map.end()) { - sensor_config_path = it->second; + if (entry["config_path"]) { + sensor_config_path = entry["config_path"].as(); } - std::vector sensor_names; std::vector mj_sensor_types; - const std::string name_delim = "&"; - const std::string type_delim = "$"; - try { - std::string names = setting_map.at("sensor_names"); - while (!names.empty()) { - size_t plus_idx = names.find(name_delim); - if (plus_idx == std::string::npos) { - plus_idx = names.length(); - } - // now determine sensor type - size_t dollar_idx = names.find(type_delim); - if (dollar_idx == std::string::npos) { - dollar_idx = names.length(); - } - sensor_names.emplace_back(names.substr(0, dollar_idx)); - mj_sensor_types.emplace_back(names.substr(dollar_idx + type_delim.length(), - plus_idx - dollar_idx - type_delim.length())); - names.erase(0, plus_idx + name_delim.length()); - } - } catch (const std::exception& e) { - throw std::runtime_error("No sensor name was provided for a sensor group!"); + if (!entry["sensor_names"] || !entry["sensor_names"].IsMap()) { + throw std::runtime_error("No sensor_names mapping was provided for a sensor group!"); + } + for (const auto& kv : entry["sensor_names"]) { + sensor_names.push_back(kv.first.as()); + mj_sensor_types.push_back(kv.second.as()); } // Create the timer and publishers with the settings @@ -552,19 +496,6 @@ namespace obelisk { this->publishers_[sensor_key] = std::make_shared>(pub); - // Add the timer to the list - this->timers_[sensor_key] = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1e3 * dt)), - CreateTimerCallback( - sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), - callback_group_); - } else if (sensor_type == "Imu") { - // Make a publisher and add it to the list - auto pub = ObeliskNode::create_publisher(topic, depth); - this->publishers_[sensor_key] = - std::make_shared>(pub); - // Add the timer to the list this->timers_[sensor_key] = this->create_wall_timer( std::chrono::milliseconds(static_cast(1e3 * dt)), @@ -599,9 +530,9 @@ namespace obelisk { this->template GetPublisher(sensor_key)), callback_group_); } else if ((sensor_type == "ObkScan") || (sensor_type == "PointCloud2")) { - // Create the scanner interface - // Parse the yaml - YAML::Node scan_config = YAML::LoadFile(sensor_config_path); + // Resolve scan config: prefer inline `scan_config:` block, fall back to + // legacy `config_path:` pointing at a separate YAML file. + YAML::Node scan_config = ResolveScanConfig(entry, sensor_config_path); const auto type_node = scan_config["pattern"]["type"]; if (!type_node) { throw std::runtime_error("scan config missing pattern.type"); @@ -609,9 +540,9 @@ namespace obelisk { const std::string type_str = type_node.as(); if (type_str == "height_scan") { - scan_interface_ = std::make_unique(sensor_config_path); + scan_interface_ = std::make_unique(scan_config); } else if (type_str == "lidar_scan") { - scan_interface_ = std::make_unique(sensor_config_path); + scan_interface_ = std::make_unique(scan_config); } else { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -660,8 +591,9 @@ namespace obelisk { this->publishers_[sensor_key] = std::make_shared>(pub); - // Create the scanner interface - YAML::Node scan_config = YAML::LoadFile(sensor_config_path); + // Resolve scan config: prefer inline `scan_config:` block, fall back to + // legacy `config_path:` pointing at a separate YAML file. + YAML::Node scan_config = ResolveScanConfig(entry, sensor_config_path); const auto type_node = scan_config["pattern"]["type"]; if (!type_node) { throw std::runtime_error("scan config missing pattern.type"); @@ -669,9 +601,9 @@ namespace obelisk { const std::string type_str = type_node.as(); if (type_str == "lidar_scan") { - depth_scan_interface_ = std::make_unique(sensor_config_path); + depth_scan_interface_ = std::make_unique(scan_config); } else if (type_str == "depth_camera") { - depth_scan_interface_ = std::make_unique(sensor_config_path); + depth_scan_interface_ = std::make_unique(scan_config); } else { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -712,75 +644,49 @@ namespace obelisk { /** * @brief Parse the settings for visualizing geometries from the mujoco scene. + * + * Expected YAML shape (a map with a ``dt`` and arbitrary ``geom_name: geom_type`` entries — + * the type is not actually used; mujoco supplies it): + * ``` + * dt: 1.0 + * dummy_box: box + * dummy_sphere: sphere + * ``` */ - void ParseVizString(std::string viz_settings) { - // TODO: We should probably make the yaml entry a normal list, but with the way config strings are done that - // will be a huge pain So in the future we should adjust this. This is because I don't actually need the - // user to provide the geom type. - - // Remove {} and [] - viz_settings.erase(std::remove(viz_settings.begin(), viz_settings.end(), '{'), viz_settings.end()); - viz_settings.erase(std::remove(viz_settings.begin(), viz_settings.end(), '}'), viz_settings.end()); - viz_settings.erase(std::remove(viz_settings.begin(), viz_settings.end(), '['), viz_settings.end()); - viz_settings.erase(std::remove(viz_settings.begin(), viz_settings.end(), ']'), viz_settings.end()); + void ParseVizGeoms(const YAML::Node& viz_settings) { + if (!viz_settings || !viz_settings.IsMap()) { + return; + } // Wait for mujoco setup to complete while (!mujoco_setup_) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } - // Extract the geom id's for publishing - // Create a new map between the names and their string values - const std::string setting_delim = "|"; - const std::string val_delim = ":"; - std::map setting_map; - - size_t val_idx; - while (!viz_settings.empty()) { - val_idx = viz_settings.find(setting_delim); - if (val_idx == std::string::npos) { - val_idx = viz_settings.length(); - } - - size_t setting_idx = viz_settings.find(val_delim); - if (setting_idx == std::string::npos) { - throw std::runtime_error("Invalid viz setting string!"); - } - - std::string setting = viz_settings.substr(0, setting_idx); - std::string val = viz_settings.substr(setting_idx + 1, val_idx - (setting_idx + 1)); - setting_map.emplace(setting, val); - viz_settings.erase(0, val_idx + val_delim.length()); - } - - // The dt for the timer - double dt = -1; - try { - dt = std::stod(setting_map.at("dt")); - if (dt <= 0) { - throw std::runtime_error("Invalid dt. Must be > 0."); - } - } catch (const std::exception& e) { + if (!viz_settings["dt"]) { throw std::runtime_error("No dt provided for the viz publisher!"); } + const double dt = viz_settings["dt"].as(); + if (dt <= 0) { + throw std::runtime_error("Invalid dt. Must be > 0."); + } - for (const auto& [key, value] : setting_map) { - if (key != "dt") { - // Find the mujoco geom with that name - const int geom_id = mj_name2id(model_, mjtObj::mjOBJ_GEOM, key.c_str()); - if (geom_id == -1) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Geom " << key << " not found in the model! Skipping!"); - } else { - // TODO: Can easily add elipsoids in - if (model_->geom_type[geom_id] != mjtGeom::mjGEOM_BOX && - model_->geom_type[geom_id] != mjtGeom::mjGEOM_SPHERE && - model_->geom_type[geom_id] != mjtGeom::mjGEOM_CYLINDER) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "Geom type not supported! Only support Box, Sphere, and Cylinder."); - } else { - viz_geoms_.emplace_back(geom_id); - } - } + for (const auto& kv : viz_settings) { + const std::string key = kv.first.as(); + if (key == "dt") { + continue; + } + // Find the mujoco geom with that name + const int geom_id = mj_name2id(model_, mjtObj::mjOBJ_GEOM, key.c_str()); + if (geom_id == -1) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Geom " << key << " not found in the model! Skipping!"); + } else if (model_->geom_type[geom_id] != mjtGeom::mjGEOM_BOX && + model_->geom_type[geom_id] != mjtGeom::mjGEOM_SPHERE && + model_->geom_type[geom_id] != mjtGeom::mjGEOM_CYLINDER) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Geom type not supported! Only support Box, Sphere, and Cylinder."); + } else { + viz_geoms_.emplace_back(geom_id); } } diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h index a2fa1378..6d86910c 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h @@ -1,14 +1,15 @@ #pragma once -#include #include #include #include -#include +#include +#include -#include "rcl_interfaces/msg/parameter_event.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include + #include "obelisk_control_msgs/msg/pd_feed_forward.hpp" #include "obelisk_control_msgs/msg/position_setpoint.hpp" @@ -16,7 +17,6 @@ #include "obelisk_sensor_msgs/msg/obk_frame_pose.hpp" #include "obelisk_sensor_msgs/msg/obk_image.hpp" -#include "obelisk_sensor_msgs/msg/obk_imu.hpp" #include "obelisk_sensor_msgs/msg/obk_joint_encoders.hpp" #include "obelisk_sensor_msgs/msg/true_sim_state.hpp" @@ -30,68 +30,42 @@ namespace obelisk { // ------------------------------------ // /** - * @brief Internal class used to create a heirarchy to allow to polymorphism to avoid type casting when - * Activating, Deactivating and Releasing all publishers. End users should never need this class. + * @brief Internal class used to create a heirarchy to allow polymorphism over publisher types. */ class ObeliskPublisherBase { public: virtual void Activate() = 0; virtual void Deactivate() = 0; virtual void Release() = 0; - - protected: - private: }; - /** - * @brief Class that inherits from ObeliskPublisherBase and holds a specific ROS2 publisher. - * End users should never need this class. - */ template class ObeliskPublisher : public ObeliskPublisherBase { public: ObeliskPublisher(typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher) : publisher_(publisher) {} void Activate() override { publisher_->on_activate(); } - void Deactivate() override { publisher_->on_deactivate(); } - void Release() override { publisher_.reset(); } typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr GetPublisher() { return publisher_; } - protected: private: typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; }; - /** - * @brief Internal struct used to hold registered publisher info and a factory function to make the publisher. - */ struct ObeliskPublisherInfo { - std::string ros_param; - std::function(const std::string& config_str)> creator; + std::function(const YAML::Node& entry)> creator; }; // ------------------------------------- // // ------------ Subscribers ------------ // // ------------------------------------- // - /** - * @brief Internal class used to create a heirarchy to allow to polymorphism to avoid type casting when - * Activating, Deactivating and Releasing all subscribers. End users should never need this class. - */ class ObeliskSubscriptionBase { public: virtual void Release() = 0; - - protected: - private: }; - /** - * @brief Class that inherits from ObeliskPublisherBase and holds a specific ROS2 subscriber. - * End users should never need this class. - */ template class ObeliskSubscription : public ObeliskSubscriptionBase { public: ObeliskSubscription(typename rclcpp::Subscription::SharedPtr subscription) @@ -101,71 +75,48 @@ namespace obelisk { typename rclcpp::Subscription::SharedPtr GetSubscription() { return subscription_; } - protected: private: typename rclcpp::Subscription::SharedPtr subscription_; }; - /** - * @brief Internal struct used to hold registered subscription info and a factory function to make the - * subscription. - */ struct ObeliskSubscriptionInfo { - std::string ros_param; - std::function(const std::string& config_str)> creator; + std::function(const YAML::Node& entry)> creator; }; // ------------------------------------- // // --------------- Timers -------------- // // ------------------------------------- // - /** - * @brief Internal struct used to hold registered timer info and a factory function to make the timer. - */ struct ObeliskTimerInfo { - std::string ros_param; - std::function creator; + std::function creator; }; } // namespace internal /** - * @brief Abstract generic Obelisk node + * @brief Abstract generic Obelisk node. * - * A lifecycle node whose publishers and subscribers can only publish and subscribe to Obelisk messages. + * A lifecycle node whose publishers, subscribers, and timers are configured from a single + * ROS string parameter named ``obelisk_settings`` whose value is a YAML document. The launch-side + * producer (``obelisk_py.core.utils.launch_utils``) builds that YAML directly from the launch + * config, so no flat ``field:value,...`` config-string format is needed. * - * By convention, the initialization function should only declare ROS parameters and define stateful quantities. - * Some guidelines for the on_configure, on_activate, and on_deactivate callbacks are provided below. + * Components are registered by ``key`` via ``RegisterObkPublisher`` / ``RegisterObkSubscription`` + * / ``RegisterObkTimer``. At ``on_configure`` time the node walks the parsed YAML and instantiates + * each registered component from the entry whose ``key`` field matches. * * Lifecycle Callbacks * ------------------- * The on_configure callback should do the following: * * Instantiate required ROS parameters. * * Instantiate optional ROS parameters. - * * Declare publishers, timers, and subscribers (any timers should be deactivated here). + * * Declare publishers, timers, and subscribers (timers start deactivated). * * Initialize stateful quantities. * * The on_activate callback should do the following: - * * Activate any timers that were declared in on_configure (by calling reset()). - * * Resetting any variables or stateful quantities that need particular initial values upon activation. + * * Activate any timers that were declared in on_configure. * * The on_deactivate callback should do the following: - * * Deactivate any timers that were activated in on_activate (by calling cancel()). - * - * The on_cleanup callback should do the following: - * * Clean up any resources that were allocated in on_configure. - * - * The on_shutdown callback should do the following: - * * Also perform clean up. The main difference is that if shutting down, the node cannot reactivate. - * - * Configuration Strings - * --------------------- - * The configuration of publishers, subscribers, and timers are done via a concept called configuration strings. A - * configuration string is formatted as follows: - * "field1:value1,field2:value2,...,fieldN:valueN". - * That is, each field-value pair is separated by a comma, and the field and value are separated by a colon. This - * lets the user specify the configuration of the publisher in a single string, which is a compressed way to - * configure publishers or subscribers using the data types available to ROS2 parameters. This also means the user - * can omit fields in favor of default values if desired or pass fields in any order. + * * Deactivate any timers that were activated in on_activate. */ class ObeliskNode : public rclcpp_lifecycle::LifecycleNode { public: @@ -173,11 +124,8 @@ namespace obelisk { bool enable_communication_interface = true) : LifecycleNode(node_name, options, enable_communication_interface), CB_GROUP_NONE("None"), CB_GROUP_MUTUALLY_EXEC("MutuallyExclusiveCallbackGroup"), CB_GROUP_REENTRANT("ReentrantCallbackGroup") { - this->declare_parameter("callback_group_settings", ""); - - // ROS parameter designed to let the user feed a file path for their own code + this->declare_parameter(OBK_SETTINGS_PARAM, ""); this->declare_parameter("params_path", ""); - RCLCPP_INFO_STREAM(this->get_logger(), node_name << " created."); }; @@ -186,44 +134,30 @@ namespace obelisk { bool enable_communication_interface = true) : LifecycleNode(node_name, namespace_, options, enable_communication_interface), CB_GROUP_NONE("None"), CB_GROUP_MUTUALLY_EXEC("MutuallyExclusiveCallbackGroup"), CB_GROUP_REENTRANT("ReentrantCallbackGroup") { - this->declare_parameter("callback_group_settings", ""); - - // ROS parameter designed to let the user feed a file path for their own code + this->declare_parameter(OBK_SETTINGS_PARAM, ""); this->declare_parameter("params_path", ""); - RCLCPP_INFO_STREAM(this->get_logger(), node_name << " created."); }; + // ---------------------------------------------------------- // + // -------------------- Component Register ------------------ // + // ---------------------------------------------------------- // + /** - * @brief Registers a publisher with the node so that the node can handle all configuration, activation, and - * cleanup. - * - * Adds the publisher to the registered_publishers_ map, so the publisher can be created at configure time. + * @brief Register a publisher under ``key``. * - * @param ros_param the string name of the ros parameter - * @param key the string key used to refer to (and get) this publisher - * @param default_config_str (optional, default is "") string giving the default configuration + * The publisher is created at on_configure time from the entry in obelisk_settings.publishers + * whose ``key`` field equals ``key``. */ - template - void RegisterObkPublisher(const std::string& ros_param, const std::string& key, - const std::string& default_config_str = "") { - this->declare_parameter(ros_param, default_config_str); - + template void RegisterObkPublisher(const std::string& key) { internal::ObeliskPublisherInfo info; - info.ros_param = ros_param; - info.creator = [this](const std::string& config_str) { - auto publisher = this->CreatePublisherFromConfigStr(config_str); + info.creator = [this](const YAML::Node& entry) { + auto publisher = this->CreatePublisherFromEntry(entry); return std::make_shared>(publisher); }; - registered_publishers_[key] = info; } - /** - * @brief Get the publisher associated with the key. - * - * @param key the string key - */ template typename rclcpp_lifecycle::LifecyclePublisher::SharedPtr GetPublisher(const std::string& key) { auto it = publishers_.find(key); @@ -231,45 +165,25 @@ namespace obelisk { auto pub = std::dynamic_pointer_cast>(it->second); if (pub) { return pub->GetPublisher(); - } else { - throw std::runtime_error("Publisher is empty!"); } + throw std::runtime_error("Publisher is empty!"); } - throw std::runtime_error("Publisher not found for the key!"); } /** - * @brief Registers a subscription with the node so that the node can handle all configuration and - * cleanup. - * - * Adds the subscription to the registered_subscription_ map, so the subscription can be created at configure - * time. - * - * @param ros_param the string name of the ros parameter - * @param key the string key used to refer to (and get) this subscription - * @param default_config_str (optional, default is "") string giving the default configuration + * @brief Register a subscription under ``key``. */ template - void RegisterObkSubscription(const std::string& ros_param, const std::string& key, CallbackT&& callback, - const std::string& default_config_str = "") { - this->declare_parameter(ros_param, default_config_str); - + void RegisterObkSubscription(const std::string& key, CallbackT&& callback) { internal::ObeliskSubscriptionInfo info; - info.ros_param = ros_param; - info.creator = [this, callback](const std::string& config_str) { - auto subscription = this->CreateSubscriptionFromConfigStr(config_str, std::move(callback)); + info.creator = [this, callback](const YAML::Node& entry) { + auto subscription = this->CreateSubscriptionFromEntry(entry, std::move(callback)); return std::make_shared>(subscription); }; - registered_subscriptions_[key] = info; } - /** - * @brief Get the subscription associated with the key. - * - * @param key the string key - */ template typename rclcpp::Subscription::SharedPtr GetSubscription(const std::string& key) { auto it = subscriptions_.find(key); @@ -277,66 +191,45 @@ namespace obelisk { auto sub = std::dynamic_pointer_cast>(it->second); if (sub) { return sub->GetSubscription(); - } else { - throw std::runtime_error("Subscription is empty!"); } + throw std::runtime_error("Subscription is empty!"); } - throw std::runtime_error("Subscription not found for the key!"); } /** - * @brief Registers a timer with the node so that the node can handle all configuration and - * cleanup. - * - * Adds the timer to the registered_timer_ map, so the timer can be created at configure time. - * - * @param ros_param the string name of the ros parameter - * @param key the string key used to refer to (and get) this timer - * @param default_config_str (optional, default is "") string giving the default configuration + * @brief Register a timer under ``key``. */ - template - void RegisterObkTimer(const std::string& ros_param, const std::string& key, CallbackT&& callback, - const std::string& default_config_str = "") { - this->declare_parameter(ros_param, default_config_str); - + template void RegisterObkTimer(const std::string& key, CallbackT&& callback) { internal::ObeliskTimerInfo info; - info.ros_param = ros_param; - info.creator = [this, callback](const std::string& config_str) { - return this->CreateWallTimerFromConfigStr(config_str, std::move(callback)); + info.creator = [this, callback](const YAML::Node& entry) { + return this->CreateWallTimerFromEntry(entry, std::move(callback)); }; - registered_timers_[key] = info; } - /** - * @brief Get the timer associated with the key. - * - * @param key the string key - */ typename rclcpp::TimerBase::SharedPtr GetTimer(const std::string& key) { auto it = timers_.find(key); if (it != timers_.end()) { return it->second; } - throw std::runtime_error("Timer not found for the key!"); } - /** - * @brief Configures the node. - * Specifically, here we parse the the configuration string that determines - * the callback groups and names. - * - * @return success if everything is executed. - */ + // ---------------------------------------------------------- // + // -------------------- Lifecycle Hooks --------------------- // + // ---------------------------------------------------------- // + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_configure( const rclcpp_lifecycle::State& prev_state) { rclcpp_lifecycle::LifecycleNode::on_configure(prev_state); - // Parse the configuration groups for this node - ParseCallbackGroupConfig(this->get_parameter("callback_group_settings").as_string()); + obk_settings_ = LoadObkSettings(); + publisher_specs_ = ToVector(obk_settings_["publishers"]); + subscriber_specs_ = ToVector(obk_settings_["subscribers"]); + timer_specs_ = ToVector(obk_settings_["timers"]); + BuildCallbackGroups(obk_settings_["callback_groups"]); CreatePublishers(); CreateSubscriptions(); CreateTimers(); @@ -345,11 +238,6 @@ namespace obelisk { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - /** - * @brief activates up the node. - * - * @param prev_state the state of the ros node. - */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_activate( const rclcpp_lifecycle::State& prev_state) { rclcpp_lifecycle::LifecycleNode::on_activate(prev_state); @@ -359,22 +247,15 @@ namespace obelisk { pub->Activate(); } } - for (auto& [key, timer] : timers_) { if (timer) { - timer->reset(); // Start the timer + timer->reset(); } } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " activated."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - /** - * @brief deactivates up the node. - * - * @param prev_state the state of the ros node. - */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_deactivate( const rclcpp_lifecycle::State& prev_state) { rclcpp_lifecycle::LifecycleNode::on_deactivate(prev_state); @@ -384,429 +265,272 @@ namespace obelisk { pub->Deactivate(); } } - for (auto& [key, timer] : timers_) { if (timer) { - timer->cancel(); // Stop the timer + timer->cancel(); } } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " deactivated."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - /** - * @brief cleans up the node. - * - * @param prev_state the state of the ros node. - */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_cleanup( const rclcpp_lifecycle::State& prev_state) { rclcpp_lifecycle::LifecycleNode::on_cleanup(prev_state); + ReleaseAll(); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " cleaned up."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } - // Clear current data - callback_groups_.clear(); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_shutdown( + const rclcpp_lifecycle::State& prev_state) { + rclcpp_lifecycle::LifecycleNode::on_shutdown(prev_state); + ReleaseAll(); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " shutdown."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } - // Clean up publishers - for (auto& [key, pub] : publishers_) { - if (pub) { - pub->Release(); - } + protected: + // The single ROS parameter that carries all Obelisk-internal settings as a YAML string. + static constexpr const char* OBK_SETTINGS_PARAM = "obelisk_settings"; + + /** + * @brief Loads the obelisk_settings ROS parameter and returns it as a YAML::Node. + * + * Returns an empty (null) YAML::Node if the parameter is empty. + */ + YAML::Node LoadObkSettings() { + const std::string raw = this->get_parameter(OBK_SETTINGS_PARAM).as_string(); + if (raw.empty()) { + return YAML::Node{}; } - publishers_.clear(); - registered_publishers_.clear(); + try { + return YAML::Load(raw); + } catch (const YAML::Exception& e) { + throw std::runtime_error(std::string("Failed to parse obelisk_settings YAML: ") + e.what()); + } + } - // Clean up subscriptions - for (auto& [key, sub] : subscriptions_) { - if (sub) { - sub->Release(); + /** + * @brief Convert a YAML sequence to a std::vector (skipping null/missing). + */ + static std::vector ToVector(const YAML::Node& node) { + std::vector out; + if (node && node.IsSequence()) { + out.reserve(node.size()); + for (const auto& item : node) { + out.push_back(item); } } - subscriptions_.clear(); - registered_subscriptions_.clear(); + return out; + } - // Clean up timers - for (auto& [key, timer] : timers_) { - if (timer) { - timer->cancel(); // Stop the timer - timer.reset(); // Release the timer + /** + * @brief Find an entry in ``entries`` whose ``key`` field equals ``key``. Returns null Node if absent. + */ + static YAML::Node FindEntryByKey(const std::vector& entries, const std::string& key) { + for (const auto& entry : entries) { + if (entry["key"] && entry["key"].as() == key) { + return entry; } } - timers_.clear(); - registered_timers_.clear(); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " cleaned up."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return YAML::Node{}; } /** - * @brief shutsdown the node the node. - * - * @param prev_state the state of the ros node. + * @brief Build callback groups from a YAML::Node map of {name: type_name}. */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_shutdown( - const rclcpp_lifecycle::State& prev_state) { - rclcpp_lifecycle::LifecycleNode::on_shutdown(prev_state); - - // Clear current data + void BuildCallbackGroups(const YAML::Node& spec) { callback_groups_.clear(); - - // Clean up publishers - for (auto& [key, pub] : publishers_) { - if (pub) { - pub->Release(); + if (spec && spec.IsMap()) { + for (const auto& kv : spec) { + const std::string name = kv.first.as(); + const std::string type_name = kv.second.as(); + if (type_name == CB_GROUP_MUTUALLY_EXEC) { + callback_groups_.emplace( + name, this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)); + } else if (type_name == CB_GROUP_REENTRANT) { + callback_groups_.emplace(name, + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant)); + } else { + callback_groups_.emplace(name, nullptr); + RCLCPP_WARN_STREAM(this->get_logger(), + "Unknown callback group type '" << type_name << "' for group '" << name + << "'; using default."); + } } } - publishers_.clear(); - registered_publishers_ - .clear(); // TODO: Consider if this should be called, or if I should leave this in place + callback_groups_.emplace(CB_GROUP_NONE, nullptr); + } - // Clean up subscriptions - for (auto& [key, sub] : subscriptions_) { - if (sub) { - sub->Release(); - } + /** + * @brief Look up a callback group by the (string) value in entry["callback_group"]. Defaults to nullptr. + */ + rclcpp::CallbackGroup::SharedPtr ResolveCallbackGroup(const YAML::Node& entry) { + if (!entry || !entry["callback_group"]) { + return nullptr; } - subscriptions_.clear(); - registered_subscriptions_ - .clear(); // TODO: Consider if this should be called, or if I should leave this in place - - // Clean up timers - for (auto& [key, timer] : timers_) { - if (timer) { - timer->cancel(); // Stop the timer - timer.reset(); // Release the timer - } + const std::string name = entry["callback_group"].as(); + auto it = callback_groups_.find(name); + if (it != callback_groups_.end()) { + return it->second; } - timers_.clear(); - registered_timers_.clear(); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " shutdown."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return nullptr; } - protected: /** - * @brief Creates all the registered publishers. + * @brief Common helper: read entry["topic"] (required) and entry["history_depth"] (optional, default 10). */ + static std::string GetTopic(const YAML::Node& entry) { + if (!entry["topic"]) { + throw std::runtime_error("Entry is missing required field 'topic'."); + } + return entry["topic"].as(); + } + + static int GetHistoryDepth(const YAML::Node& entry) { + if (entry["history_depth"]) { + return entry["history_depth"].as(); + } + return DEFAULT_DEPTH; + } + + static double GetTimerPeriodSec(const YAML::Node& entry) { + if (!entry["timer_period_sec"]) { + throw std::runtime_error("Timer entry is missing required field 'timer_period_sec'."); + } + const double period = entry["timer_period_sec"].as(); + if (period <= 0) { + throw std::runtime_error("timer_period_sec must be > 0."); + } + return period; + } + + // -------------------- Component creation -------------------- + void CreatePublishers() { for (const auto& [key, info] : registered_publishers_) { - const std::string param = this->get_parameter(info.ros_param).as_string(); - if (param == "") { + const auto entry = FindEntryByKey(publisher_specs_, key); + if (!entry) { RCLCPP_WARN_STREAM(this->get_logger(), - "The registered publisher <" - << key << "> was not provided a config string! Skipping creation."); - } else { - publishers_[key] = info.creator(param); + "Publisher <" << key << "> has no entry in obelisk_settings.publishers."); + continue; } + publishers_[key] = info.creator(entry); } } - /** - * @brief Creates all the registered subscriptions. - */ void CreateSubscriptions() { for (const auto& [key, info] : registered_subscriptions_) { - const std::string param = this->get_parameter(info.ros_param).as_string(); - if (param == "") { + const auto entry = FindEntryByKey(subscriber_specs_, key); + if (!entry) { RCLCPP_WARN_STREAM(this->get_logger(), - "The registered subscription <" - << key << "> was not provided a config string! Skipping creation."); - } else { - subscriptions_[key] = info.creator(param); + "Subscription <" << key << "> has no entry in obelisk_settings.subscribers."); + continue; } + subscriptions_[key] = info.creator(entry); } } - /** - * @brief Creates all the registered timers. - */ void CreateTimers() { for (const auto& [key, info] : registered_timers_) { - const std::string param = this->get_parameter(info.ros_param).as_string(); - if (param == "") { + const auto entry = FindEntryByKey(timer_specs_, key); + if (!entry) { RCLCPP_WARN_STREAM(this->get_logger(), - "Registered timer was not provided a config string! Skipping creation" << key); - } else { - timers_[key] = info.creator(param); - timers_[key]->cancel(); // Pause the timer + "Timer <" << key << "> has no entry in obelisk_settings.timers."); + continue; } + timers_[key] = info.creator(entry); + timers_[key]->cancel(); // Pause until on_activate } } - /** - * @brief Create a subscriber from a configuration string. - * - * @param config the configuration string - * @param callback the callback function (generall of the form - * std::bind(&MyFunc, this, _1)) - * @return the subscription - */ template , typename SubscriptionT = rclcpp::Subscription> - std::shared_ptr CreateSubscriptionFromConfigStr(const std::string& config, - CallbackT&& callback) { - // Parse the configuration string - const auto config_map = ParseConfigStr(config); - const std::string topic = GetTopic(config_map); - const int depth = GetHistoryDepth(config_map); - - rclcpp::CallbackGroup::SharedPtr cbg = nullptr; // default group - try { - // Get the callback group based on the string name - cbg = callback_groups_.at(GetCallbackGroupName(config_map)); - } catch (const std::exception& e) { - } - + std::shared_ptr CreateSubscriptionFromEntry(const YAML::Node& entry, CallbackT&& callback) { + const std::string topic = GetTopic(entry); + const int depth = GetHistoryDepth(entry); rclcpp::SubscriptionOptions options; - options.callback_group = cbg; - - // Create the subscriber + options.callback_group = ResolveCallbackGroup(entry); return create_subscription(topic, depth, std::move(callback), options); } - /** - * @brief Create a publisher from a configuration string. - * - * @param config the configuration string. - * @return the publisher. - */ template > std::shared_ptr> - CreatePublisherFromConfigStr(const std::string& config) { - // Parse the configuration string - const auto config_map = ParseConfigStr(config); - const std::string topic = GetTopic(config_map); - const int depth = GetHistoryDepth(config_map); - + CreatePublisherFromEntry(const YAML::Node& entry) { + const std::string topic = GetTopic(entry); + const int depth = GetHistoryDepth(entry); return create_publisher(topic, depth); } - /** - * @brief Create a wall timer from a configuration string. - * - * @param config the configuration string. - * @param callback the callback function. - * - * @return the timer. - */ template - typename rclcpp::TimerBase::SharedPtr CreateWallTimerFromConfigStr(const std::string& config, - CallbackT&& callback) { - // Finest frequency is determined by DurationT - using namespace std::chrono_literals; - - const auto config_map = ParseConfigStr(config); - const double period_sec = GetPeriod(config_map); // Period in seconds - - rclcpp::CallbackGroup::SharedPtr cbg = nullptr; // default group - try { - // Get the callback group based on the string name - cbg = callback_groups_.at(GetCallbackGroupName(config_map)); - } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "Callback group " - << GetCallbackGroupName(config_map) - << " not found as a callback group. Initializing with the default group."); - } - - // Create the timer + typename rclcpp::TimerBase::SharedPtr CreateWallTimerFromEntry(const YAML::Node& entry, CallbackT&& callback) { + const double period_sec = GetTimerPeriodSec(entry); + rclcpp::CallbackGroup::SharedPtr cbg = ResolveCallbackGroup(entry); return this->create_wall_timer(std::chrono::microseconds(static_cast(1e6 * period_sec)), std::move(callback), cbg); } - /** - * @brief Parses the configuration string into a map from strings to - * strings. the value strings are meant to be parsed in other functions. - * - * @return a map of configuration options to their settings as strings. - */ - std::map ParseConfigStr(std::string config) { - const std::string val_delim = ":"; - const std::string type_delim = ","; - - std::map config_map; - - // Parse the string - size_t type_idx; - while (!config.empty()) { - type_idx = config.find(type_delim); - if (type_idx == std::string::npos) { - type_idx = config.length(); - } + // -------------------- Cleanup helper -------------------- - size_t val_idx = config.find(val_delim); - if (val_idx == std::string::npos) { - throw std::runtime_error("Invalid configuration string! Missing `:` before last " - "`,`."); - } - - std::string config_id = config.substr(0, val_idx); - - std::string val = config.substr(val_idx + 1, type_idx - (val_idx + 1)); - config_map.emplace(config_id, val); - config.erase(0, type_idx + type_delim.length()); - } - - return config_map; - } - - /** - * @brief Parses the configuration string map to see if there is a topic. - * Throws an error if there is no topic. - * - * @param config_map the map created by ParseConfigStr. - * @return the topic. - */ - std::string GetTopic(const std::map& config_map) { - try { - std::string topic = config_map.at("topic"); - return topic; - } catch (const std::exception& e) { - throw std::runtime_error("No topic name was provided in the configuration string."); - } - } - - /** - * @brief Parses the configuration string map to see if there is a history - * depth. - * - * @param config_map the map created by ParseConfigStr. - * @return the message history depth. - */ - int GetHistoryDepth(const std::map& config_map) { - try { - return std::stoi(config_map.at("history_depth")); - } catch (const std::exception& e) { - return DEFAULT_DEPTH; - } - } + void ReleaseAll() { + callback_groups_.clear(); - /** - * @brief Parse the configuration string map to get the period of the timer. - * Throws an error if there is no period. - * - * @param config_map the map created by ParseConfigStr. - * @return the period (in seconds). - */ - double GetPeriod(const std::map& config_map) { - try { - double period = std::stod(config_map.at("timer_period_sec")); - if (period <= 0) { - throw std::runtime_error("Period must be >= 0!"); + for (auto& [key, pub] : publishers_) { + if (pub) { + pub->Release(); } - - return period; - } catch (const std::exception& e) { - throw std::runtime_error("Invalid timer period time provided"); } - } - - /** - * @brief Parses the configuration string map to get the message name from a - * config string. Throws an error if there is no message name. - * - * @param config_map the map created by ParseConfigStr. - * @return the message name. - */ - std::string GetMessageName(const std::map& config_map) { - try { - return config_map.at("message_type"); - } catch (const std::exception& e) { - throw std::runtime_error("No message type provided in this config string!"); - } - } + publishers_.clear(); + registered_publishers_.clear(); - /** - * @brief Parses the configuration string map to get the callback group - * name. - * - * @param config_map the map created by ParseConfigStr. - * @return the callback group name. - */ - std::string GetCallbackGroupName(const std::map& config_map) { - try { - return config_map.at("callback_group"); - } catch (const std::exception& e) { - return CB_GROUP_NONE; + for (auto& [key, sub] : subscriptions_) { + if (sub) { + sub->Release(); + } } - } - - /** - * @brief Parses a configuration string to determine the names and types of - * callback groups. Sets callback_groups_. - * - * @param config the configuration string. - */ - void ParseCallbackGroupConfig(const std::string& config) { - // Parse the config string into group name, group type - const auto callback_group_names = ParseConfigStr(config); - - callback_groups_.clear(); + subscriptions_.clear(); + registered_subscriptions_.clear(); - // Iterate through all the group names - for (const auto& cbg : callback_group_names) { - // If the name is associated with a specific type, then make that - // type If the name is not then assign to the default callback group - if (cbg.second == CB_GROUP_MUTUALLY_EXEC) { - callback_groups_.emplace(cbg.first, - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)); - RCLCPP_INFO_STREAM(this->get_logger(), - "Creating a mutually exclusive callback group with name " << cbg.first); - } else if (cbg.second == CB_GROUP_REENTRANT) { - callback_groups_.emplace(cbg.first, - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant)); - RCLCPP_INFO_STREAM(this->get_logger(), - "Creating a reentrant callback group with name " << cbg.first); - } else { - // Node's default callback group - callback_groups_.emplace(cbg.first, nullptr); - RCLCPP_WARN_STREAM(this->get_logger(), "Creating a default callback group with name " << cbg.first); + for (auto& [key, timer] : timers_) { + if (timer) { + timer->cancel(); + timer.reset(); } } + timers_.clear(); + registered_timers_.clear(); - callback_groups_.emplace(CB_GROUP_NONE, nullptr); + publisher_specs_.clear(); + subscriber_specs_.clear(); + timer_specs_.clear(); } - // --------- Member Variables --------- // + // ------------ Member variables ------------ // const std::string CB_GROUP_NONE; const std::string CB_GROUP_MUTUALLY_EXEC; const std::string CB_GROUP_REENTRANT; - // Map callback group names to their callbacks + // Parsed from `obelisk_settings` at on_configure time. + YAML::Node obk_settings_; + std::vector publisher_specs_; + std::vector subscriber_specs_; + std::vector timer_specs_; + + // Callback groups by name. std::map callback_groups_; - // Hold all the publishers + // Registered components (factory closures keyed by string). std::unordered_map registered_publishers_; std::unordered_map> publishers_; - // Hold all the subscriptions std::unordered_map registered_subscriptions_; std::unordered_map> subscriptions_; - // Hold all the timers std::unordered_map registered_timers_; std::unordered_map timers_; private: - // --------- Member Variables --------- // - static constexpr int DEFAULT_DEPTH = 10; - static constexpr bool DEFAULT_IS_OBK_MSG = true; - - /** - * Helper functions for determining if we have a valid message. - * These functions recursively disassemble a tuple to check if the type - * matches. See this stackoverflow post: - * https://stackoverflow.com/questions/25958259/how-do-i-find-out-if-a-tuple-contains-a-type - */ - template struct has_type; - - template struct has_type> : std::false_type {}; - - template - struct has_type> : has_type> {}; - - template struct has_type> : std::true_type {}; - - template using ValidMessage = typename has_type::type; + static constexpr int DEFAULT_DEPTH = 10; }; } // namespace obelisk diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h index 9bac807f..f8117c30 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_robot.h @@ -19,7 +19,7 @@ namespace obelisk { // Register all components this->RegisterObkSubscription( - "sub_ctrl_setting", sub_ctrl_key_, std::bind(&ObeliskRobot::ApplyControl, this, std::placeholders::_1)); + sub_ctrl_key_, std::bind(&ObeliskRobot::ApplyControl, this, std::placeholders::_1)); } /** diff --git a/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h index e6456cba..a7a0737d 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h @@ -33,6 +33,10 @@ class DepthInterface : public RayCasterInterface { setup_rays(get_config()); } + explicit DepthInterface(const YAML::Node& config) : RayCasterInterface(config) { + setup_rays(get_config()); + } + void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, MatrixX3d& ray_starts_world, MatrixX3d& ray_directions_world) const override { @@ -43,6 +47,16 @@ class DepthInterface : public RayCasterInterface { ray_directions_world = (site_xmat * ray_directions_local_.transpose()).transpose(); } + // Public accessors (overrides of virtual methods declared public in RayCasterInterface). + float get_return(const std::array hit_point, const float dist) override { + (void)hit_point; + return dist; + } + + int get_image_width() const override { return width_; } + int get_image_height() const override { return height_; } + Vector3d get_image_forward_local() const override { return image_forward_local_; } + protected: void setup_rays(const YAML::Node& config) override { auto pattern = config["pattern"]; @@ -132,15 +146,6 @@ class DepthInterface : public RayCasterInterface { image_forward_local_.normalize(); } - float get_return(const std::array hit_point, const float dist) override { - (void)hit_point; - return dist; - } - - int get_image_width() const override { return width_; } - int get_image_height() const override { return height_; } - Vector3d get_image_forward_local() const override { return image_forward_local_; } - private: int width_ = 0; int height_ = 0; diff --git a/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h index 54856230..e746a012 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h @@ -30,6 +30,10 @@ class HeightScanInterface : public RayCasterInterface { setup_rays(get_config()); } + explicit HeightScanInterface(const YAML::Node& config) : RayCasterInterface(config) { + setup_rays(get_config()); + } + void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, MatrixX3d& ray_starts_world, MatrixX3d& ray_directions_world) const override { @@ -54,6 +58,12 @@ class HeightScanInterface : public RayCasterInterface { ray_directions_world = ray_directions_local_; } + // Public accessor (override of virtual method declared public in RayCasterInterface). + float get_return(const std::array hit_point, const float dist) override { + (void)dist; + return float(hit_point[2]); + } + protected: void setup_rays(const YAML::Node& config) override { auto pattern = config["pattern"]; @@ -157,10 +167,6 @@ class HeightScanInterface : public RayCasterInterface { apply_offset(offset); } - float get_return(const std::array hit_point, const float dist) override { - (void)dist; - return float(hit_point[2]); - } }; } // namespace obelisk diff --git a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h index 428288ee..642c92d4 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -34,6 +34,10 @@ class LidarInterface : public RayCasterInterface { setup_rays(get_config()); } + explicit LidarInterface(const YAML::Node& config) : RayCasterInterface(config) { + setup_rays(get_config()); + } + void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, MatrixX3d& ray_starts_world, MatrixX3d& ray_directions_world) const override { @@ -47,6 +51,16 @@ class LidarInterface : public RayCasterInterface { ray_directions_world = (site_xmat * ray_directions_local_.transpose()).transpose(); } + // Public accessors (overrides of virtual methods declared public in RayCasterInterface). + float get_return(const std::array hit_point, const float dist) override { + (void)hit_point; + return dist; + } + + int get_image_width() const override { return nh_; } + int get_image_height() const override { return nv_; } + Vector3d get_image_forward_local() const override { return image_forward_local_; } + protected: void setup_rays(const YAML::Node& config) override { auto pattern = config["pattern"]; @@ -184,15 +198,6 @@ class LidarInterface : public RayCasterInterface { image_forward_local_.normalize(); } - float get_return(const std::array hit_point, const float dist) override { - (void)hit_point; - return dist; - } - - int get_image_width() const override { return nh_; } - int get_image_height() const override { return nv_; } - Vector3d get_image_forward_local() const override { return image_forward_local_; } - private: int nv_ = 0; int nh_ = 0; diff --git a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h index 61e081b0..2e3559ea 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -49,44 +49,15 @@ class RayCasterInterface { if (!std::filesystem::exists(yaml_path)) { throw std::runtime_error("Ray caster config file not found: " + yaml_path); } - - config_ = YAML::LoadFile(yaml_path); - - // Read site name (required) - if (config_["site_name"]) { - site_name_ = config_["site_name"].as(); - } else { - throw std::runtime_error("site_name is required in ray caster config"); - } - - // Read geom groups for mj_ray filtering (optional, default: group 0 only) - std::fill(std::begin(geom_group_mask_), std::end(geom_group_mask_), 0); - if (config_["geom_groups"]) { - auto groups = config_["geom_groups"].as>(); - for (int g : groups) { - if (g >= 0 && g < mjNGROUP) { - geom_group_mask_[g] = 1; - } - } - } else { - geom_group_mask_[0] = 1; - } - - // Read frame (optional, default: "world") - if (config_["frame"]) { - frame_ = config_["frame"].as(); - } - - // Read max ray distance (optional, default: 0.0 meaning "disabled") - // Hits beyond this distance are treated as no-hit (apply_max_distance returns -1). - if (config_["max_distance"]) { - max_distance_ = config_["max_distance"].as(); - if (max_distance_ < 0.0) { - throw std::runtime_error("max_distance must be >= 0"); - } - } + load_common(YAML::LoadFile(yaml_path)); } + /** + * @brief Construct from a parsed YAML node (inline config; no file I/O). + * @throws std::runtime_error if the node is missing required fields + */ + explicit RayCasterInterface(const YAML::Node& config) { load_common(config); } + virtual ~RayCasterInterface() = default; // Disable copy (YAML::Node has complex copy semantics) @@ -244,6 +215,47 @@ class RayCasterInterface { MatrixX3d ray_directions_local_; // Nx3 matrix of ray directions (unit vectors) private: + /** + * @brief Common initialization: validate and pull top-level fields out of the YAML node. + */ + void load_common(const YAML::Node& config) { + config_ = config; + + // Read site name (required) + if (config_["site_name"]) { + site_name_ = config_["site_name"].as(); + } else { + throw std::runtime_error("site_name is required in ray caster config"); + } + + // Read geom groups for mj_ray filtering (optional, default: group 0 only) + std::fill(std::begin(geom_group_mask_), std::end(geom_group_mask_), 0); + if (config_["geom_groups"]) { + auto groups = config_["geom_groups"].as>(); + for (int g : groups) { + if (g >= 0 && g < mjNGROUP) { + geom_group_mask_[g] = 1; + } + } + } else { + geom_group_mask_[0] = 1; + } + + // Read frame (optional, default: "world") + if (config_["frame"]) { + frame_ = config_["frame"].as(); + } + + // Read max ray distance (optional, default: 0.0 meaning "disabled") + // Hits beyond this distance are treated as no-hit (apply_max_distance returns -1). + if (config_["max_distance"]) { + max_distance_ = config_["max_distance"].as(); + if (max_distance_ < 0.0) { + throw std::runtime_error("max_distance must be >= 0"); + } + } + } + YAML::Node config_; std::string site_name_; mjtByte geom_group_mask_[mjNGROUP]; diff --git a/obelisk/cpp/viz/include/obelisk_viz_robot.h b/obelisk/cpp/viz/include/obelisk_viz_robot.h index f84eb8f7..968d4e78 100644 --- a/obelisk/cpp/viz/include/obelisk_viz_robot.h +++ b/obelisk/cpp/viz/include/obelisk_viz_robot.h @@ -25,28 +25,24 @@ namespace obelisk::viz { template class ObeliskVizRobot : public ObeliskNode { public: ObeliskVizRobot(const std::string& name, const std::string& est_key = "sub_est", - const std::string& timer_key = "time_joints") - : ObeliskNode(name), est_key_(est_key), timer_key_(timer_key), first_message_received_(false) { + const std::string& timer_key = "time_joints", + const std::string& pub_viz_joint_key = "pub_viz_joint") + : ObeliskNode(name), est_key_(est_key), timer_key_(timer_key), + pub_viz_joint_key_(pub_viz_joint_key), first_message_received_(false) { this->declare_parameter("urdf_path_param", ""); this->declare_parameter("tf_prefix", ""); - this->declare_parameter("pub_viz_joint_setting", ""); - this->RegisterObkTimer("timer_viz_joint_setting", timer_key_, - std::bind(&ObeliskVizRobot::PublishJointState, this)); - + this->RegisterObkTimer(timer_key_, std::bind(&ObeliskVizRobot::PublishJointState, this)); this->RegisterObkSubscription( - "sub_viz_est_setting", est_key_, - std::bind(&ObeliskVizRobot::ParseEstimatedStateSafe, this, std::placeholders::_1)); + est_key_, std::bind(&ObeliskVizRobot::ParseEstimatedStateSafe, this, std::placeholders::_1)); + this->template RegisterObkPublisher(pub_viz_joint_key_); // Create the tf broadcaster base_broadcaster_ = std::make_unique(*this); } /** - * @brief Configures the node. - * - * @param prev_state the state of the ros node. - * @return success if everything completes. + * @brief Configures the node — load URDF and let ObeliskNode wire up pubs/subs/timers. */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& prev_state) override { @@ -69,75 +65,6 @@ namespace obelisk::viz { RCLCPP_INFO_STREAM(this->get_logger(), "Successfully parsed URDF file."); RCLCPP_INFO_STREAM(this->get_logger(), "Robot name: " << model_.getName()); - // Use the original lifecycle create function to avoid the warning, as we know we need non-obelisk here. - // The user can only configure the topic name and history depth. - // *** Note: The launch file must re-map the corresponding `robot_state_publisher` - // topic to be this topic, otherwise the topics won't match. *** - // TODO: Let the user configure the callback - auto config_map = ParseConfigStr(this->get_parameter("pub_viz_joint_setting").as_string()); - js_publisher_ = rclcpp_lifecycle::LifecycleNode::create_publisher( - GetTopic(config_map), GetHistoryDepth(config_map)); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - /** - * @brief Activates the node. - * - * @param prev_state the state of the ros node. - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State& prev_state) override { - this->ObeliskNode::on_activate(prev_state); - if (js_publisher_) { - js_publisher_->on_activate(); - } - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - /** - * @brief Deactivates the node. - * - * @param prev_state the state of the ros node. - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State& prev_state) override { - this->ObeliskNode::on_deactivate(prev_state); - if (js_publisher_) { - js_publisher_->on_deactivate(); - } - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - /** - * @brief Cleans up the node. - * - * @param prev_state the state of the ros node. - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State& prev_state) override { - ObeliskNode::on_cleanup(prev_state); - if (js_publisher_) { - js_publisher_.reset(); - } - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - /** - * @brief Shuts down the ros node. - * - * @param prev_state the state of the ros node. - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State& prev_state) override { - ObeliskNode::on_shutdown(prev_state); - if (js_publisher_) { - js_publisher_.reset(); - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -170,14 +97,11 @@ namespace obelisk::viz { // Publish only once we have received a message if (first_message_received_) { - js_publisher_->publish(joint_state_); + this->template GetPublisher(pub_viz_joint_key_)->publish(joint_state_); base_broadcaster_->sendTransform(base_tf_); } } - // JointState publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr js_publisher_; - // Need a mutex for the common joint state message std::mutex state_mut_; @@ -192,6 +116,7 @@ namespace obelisk::viz { std::string est_key_; std::string timer_key_; + std::string pub_viz_joint_key_; bool first_message_received_; diff --git a/obelisk/python/obelisk_py/core/control.py b/obelisk/python/obelisk_py/core/control.py index 7d04db5a..eac8f950 100644 --- a/obelisk/python/obelisk_py/core/control.py +++ b/obelisk/python/obelisk_py/core/control.py @@ -20,21 +20,12 @@ class ObeliskController(ABC, ObeliskNode): def __init__(self, node_name: str, ctrl_msg_type: Type, est_msg_type: Type) -> None: """Initialize the Obelisk controller.""" super().__init__(node_name) - self.register_obk_timer( - "timer_ctrl_setting", - self.compute_control, - key="timer_ctrl", - ) - self.register_obk_publisher( - "pub_ctrl_setting", - msg_type=ctrl_msg_type, - key="pub_ctrl", - ) + self.register_obk_timer(key="timer_ctrl", callback=self.compute_control) + self.register_obk_publisher(key="pub_ctrl", msg_type=ctrl_msg_type) self.register_obk_subscription( - "sub_est_setting", - self.update_x_hat, - msg_type=est_msg_type, key="sub_est", + callback=self.update_x_hat, + msg_type=est_msg_type, ) @abstractmethod @@ -49,9 +40,9 @@ def update_x_hat(self, x_hat_msg: Type) -> None: def compute_control(self) -> Type: """Compute the control signal. - This is the control timer callback and is expected to call 'publisher_ctrl' internally. Note that the control - message is still returned afterwards, mostly for logging/debugging purposes. The publish call is the important - part, NOT the returned value, since the topic is what the ObeliskRobot subscribes to. + This is the control timer callback and is expected to call ``self.obk_publishers["pub_ctrl"]`` internally. Note + that the control message is still returned afterwards, mostly for logging/debugging purposes. The publish call + is the important part, NOT the returned value, since the topic is what the ObeliskRobot subscribes to. Returns: obelisk_control_msg: An Obelisk message type containing the control signal and relevant metadata. diff --git a/obelisk/python/obelisk_py/core/estimation.py b/obelisk/python/obelisk_py/core/estimation.py index 9d34f4ec..8768c3c6 100644 --- a/obelisk/python/obelisk_py/core/estimation.py +++ b/obelisk/python/obelisk_py/core/estimation.py @@ -33,24 +33,17 @@ def __init__(self, node_name: str, est_msg_type: Type) -> None: [NOTE] In derived classes, you should declare settings for sensor subscribers. """ super().__init__(node_name) - self.register_obk_timer( - "timer_est_setting", - self.compute_state_estimate, - key="timer_est", - ) - self.register_obk_publisher( - "pub_est_setting", - msg_type=est_msg_type, - key="pub_est", - ) + self.register_obk_timer(key="timer_est", callback=self.compute_state_estimate) + self.register_obk_publisher(key="pub_est", msg_type=est_msg_type) @abstractmethod def compute_state_estimate(self) -> Type: """Compute the state estimate. - This is the state estimate timer callback and is expected to call 'publisher_est' internally. Note that the - state estimate message is still returned afterwards, mostly for logging/debugging purposes. The publish call is - the important part, NOT the returned value, since the topic is what the ObeliskController subscribes to. + This is the state estimate timer callback and is expected to call ``self.obk_publishers["pub_est"]`` internally. + Note that the state estimate message is still returned afterwards, mostly for logging/debugging purposes. The + publish call is the important part, NOT the returned value, since the topic is what the ObeliskController + subscribes to. Returns: state_estimate: the state estimate message. Can be either an estimator message or, in the case of output diff --git a/obelisk/python/obelisk_py/core/node.py b/obelisk/python/obelisk_py/core/node.py index dfa76f33..11815c3e 100644 --- a/obelisk/python/obelisk_py/core/node.py +++ b/obelisk/python/obelisk_py/core/node.py @@ -1,6 +1,6 @@ from typing import Any, Callable, Dict, List, Optional, Tuple, Type, TypeVar, Union -import rclpy +import yaml from rclpy.callback_groups import CallbackGroup, MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.lifecycle import LifecycleNode from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn @@ -35,33 +35,39 @@ class ObeliskNode(LifecycleNode): The on_shutdown callback should do the following: * Also perform clean up. The main difference is that if shutting down, the node cannot reactivate. - Configuration Strings - --------------------- - The configuration of publishers, subscribers, and timers are done via a concept called configuration strings. A - configuration string is formatted as follows: - "field1:value1,field2:value2,...,fieldN:valueN". - That is, each field-value pair is separated by a comma, and the field and value are separated by a colon. This - lets the user specify the configuration of the publisher in a single string, which is a compressed way to - configure publishers or subscribers using the data types available to ROS2 parameters. This also means the user - can omit fields in favor of default values if desired or pass fields in any order. + Settings + -------- + Each ObeliskNode receives a single ROS parameter named ``obelisk_settings`` whose value is a YAML + string holding the node's full configuration: ``publishers``, ``subscribers``, ``timers``, ``sim``, + and ``callback_groups``. The launch-side producer (``obelisk_py.core.utils.launch_utils``) builds + that YAML directly from the launch config file, so the structure inside ``obelisk_settings`` is the + same as the YAML the user wrote — no escape characters, no flat ``field:value,...`` strings, full + support for nested structures. + + Components are registered by ``key`` via ``register_obk_publisher`` / ``register_obk_subscription`` + / ``register_obk_timer``. At ``on_configure`` time the node walks the parsed settings dict and + instantiates each registered component from the entry whose ``key`` matches. """ + OBK_SETTINGS_PARAM = "obelisk_settings" + def __init__(self, node_name: str) -> None: """Initialize the Obelisk node.""" super().__init__(node_name) - self.declare_parameter("callback_group_settings", "") - # ROS parameter designed to let the user feed a file path for their own code + # The single YAML-string parameter that carries all Obelisk-internal settings. + self.declare_parameter(self.OBK_SETTINGS_PARAM, "") + # ROS parameter designed to let the user feed a file path for their own code. self.declare_parameter("params_path", "") - # for auto-configuration - self._obk_pub_settings = [] - self._obk_sub_settings = [] - self._obk_timer_settings = [] + # Deferred component registrations — populated by register_obk_*, consumed in on_configure. + self._obk_pub_settings: List[Dict[str, Any]] = [] + self._obk_sub_settings: List[Dict[str, Any]] = [] + self._obk_timer_settings: List[Dict[str, Any]] = [] - self.obk_callback_groups = {} - self.obk_publishers = {} - self.obk_subscriptions = {} - self.obk_timers = {} + self.obk_callback_groups: Dict[str, CallbackGroup] = {} + self.obk_publishers: Dict[str, Any] = {} + self.obk_subscriptions: Dict[str, Any] = {} + self.obk_timers: Dict[str, Any] = {} @property def node_name(self) -> str: @@ -77,489 +83,227 @@ def t(self) -> float: # COMPONENT REGISTRATION # # ###################### # - def register_obk_publisher( - self, - ros_parameter: str, - msg_type: Type, - key: Optional[str] = None, - default_config_str: Optional[str] = None, - ) -> None: - """Register a publisher using a configuration string. + def register_obk_publisher(self, key: str, msg_type: Type) -> None: + """Register a publisher under ``key``. + + The publisher is created at ``on_configure`` time from the matching entry in + ``obelisk_settings.publishers`` (the entry whose ``key`` field equals ``key``). Parameters: - ros_parameter: The ROS parameter that contains the configuration string. - msg_type: The message type. If passed, we just use this directly. Otherwise, you can configure this at - runtime by passing a msg_type field in the config string that corresponds to an Obelisk message type. - key: The key for the publisher. If None, we look for the key as a field in the config string later. - default_config_str: The default configuration string. If None, the parameter must be initialized. - - Raises: - ParameterUninitializedException: If the parameter is uninitialized and no default config string is provided. + key: Identifier used to look up the publisher in ``obelisk_settings.publishers`` and + in ``self.obk_publishers``. + msg_type: The message type class. """ - if default_config_str is None: - self.declare_parameter(ros_parameter, rclpy.Parameter.Type.STRING) - else: - self.declare_parameter(ros_parameter, value=default_config_str) - self._obk_pub_settings.append({"key": key, "ros_parameter": ros_parameter, "msg_type": msg_type}) + self._obk_pub_settings.append({"key": key, "msg_type": msg_type}) def register_obk_subscription( self, - ros_parameter: str, + key: str, callback: Callable[[MsgType], None], msg_type: Type, - key: Optional[str] = None, - default_config_str: Optional[str] = None, ) -> None: - """Register a subscription using a configuration string. + """Register a subscription under ``key``. Parameters: - ros_parameter: The ROS parameter that contains the configuration string. - callback: The callback function. - msg_type: The message type. If passed, we just use this directly. Otherwise, you can configure this at - runtime by passing a msg_type field in the config string that corresponds to an Obelisk message type. - key: The key for the subscription. If None, we look for the key as a field in the config string later. - default_config_str: The default configuration string. If None, the parameter must be initialized. - - Raises: - ParameterUninitializedException: If the parameter is uninitialized and no default config string is provided. + key: Identifier used to look up the subscription in ``obelisk_settings.subscribers`` and + in ``self.obk_subscriptions``. + callback: Callback invoked when a message is received. + msg_type: The message type class. """ - if default_config_str is None: - self.declare_parameter(ros_parameter, rclpy.Parameter.Type.STRING) - else: - self.declare_parameter(ros_parameter, value=default_config_str) - self._obk_sub_settings.append( - {"key": key, "ros_parameter": ros_parameter, "callback": callback, "msg_type": msg_type} - ) + self._obk_sub_settings.append({"key": key, "callback": callback, "msg_type": msg_type}) def register_obk_timer( self, - ros_parameter: str, + key: str, callback: Callable[[], Union[Any, Tuple[Any, ...]]], - key: Optional[str] = None, - default_config_str: Optional[str] = None, ) -> None: - """Register a timer using a configuration string. - - Parameters: - ros_parameter: The ROS parameter that contains the configuration string. - callback: The callback function. - key: The key for the timer. If None, we look for the key as a field in the config string later. - default_config_str: The default configuration string. If None, the parameter must be initialized. - - Raises: - ParameterUninitializedException: If the parameter is uninitialized and no default config string is provided. - """ - if default_config_str is None: - self.declare_parameter(ros_parameter, rclpy.Parameter.Type.STRING) - else: - self.declare_parameter(ros_parameter, value=default_config_str) - self._obk_timer_settings.append({"key": key, "ros_parameter": ros_parameter, "callback": callback}) - - # ############## # - # STATIC METHODS # - # ############## # - - @staticmethod - def _parse_config_str(config_str: str) -> Tuple[List[str], List[Union[str, float, int]]]: - """Parse a configuration string into a list of field names and a list of values. + """Register a timer under ``key``. Parameters: - config_str: The configuration string. - - Returns: - field_names: A list of field names. - value_names: A list of values. - - Raises: - ValueError: If the configuration string is invalid. + key: Identifier used to look up the timer in ``obelisk_settings.timers`` and in + ``self.obk_timers``. + callback: Callback invoked on each timer tick. """ - if not config_str: - return [], [] - - config_str = config_str.replace(" ", "") # remove all spaces - config_str = config_str.replace("\n", "") # remove all newlines - field_value_pairs = config_str.split(",") # split by comma - - def _convert_values(value: str) -> Union[str, float, int]: - """Convert a value to an int or float if possible (helper function).""" - if value.isdigit(): - return int(value) - try: - return float(value) - except ValueError: - return value - - try: - field_names, value_names = list(zip(*[pair.split(":") for pair in field_value_pairs])) # split by colon - field_names = list(field_names) # Convert to list - value_names = [_convert_values(value) for value in value_names] # convert ints/floats - except ValueError as e: + self._obk_timer_settings.append({"key": key, "callback": callback}) + + # ############# # + # SETTINGS LOAD # + # ############# # + + def _load_obk_settings(self) -> Dict[str, Any]: + """Parse the ``obelisk_settings`` ROS parameter into a Python dict.""" + raw = self.get_parameter(self.OBK_SETTINGS_PARAM).get_parameter_value().string_value + if not raw: + return {} + loaded = yaml.safe_load(raw) + if loaded is None: + return {} + if not isinstance(loaded, dict): raise ValueError( - "Each field-value pair in the configuration string must be separated by a colon!\n" - f"Original stacktrace: {e}" - ) from e - - return field_names, value_names - - @staticmethod - def _check_fields(field_names: List[str], required_field_names: List[str], optional_field_names: List[str]) -> None: - """Check if a configuration string is valid. - - Parameters: - config_str: The configuration string. - required_field_names: The required field names. - optional_field_names: The optional field names. - - Raises: - AssertionError: If the configuration string is invalid. - """ - assert all( - [field in field_names for field in required_field_names] - ), f"config_str must contain the following fields: {required_field_names}" - assert all([field in required_field_names + optional_field_names for field in field_names]), ( - f"""The following fields in the config_str are invalid: { - set(field_names) - set(required_field_names + optional_field_names) - }""", - f"Currently-supported fields in Obelisk are: {required_field_names + optional_field_names}", - ) - - @staticmethod - def _check_values(value_names: List[str], allowable_value_names: List[str]) -> None: - """Check if the values in a configuration string are valid. - - Parameters: - value_names: The list of values. - allowable_value_names: The allowable values. - - Raises: - AssertionError: If the values are invalid. - """ - assert all([value in allowable_value_names for value in value_names]), ( - f"""The following values in the config_str are invalid: { - set(value_names) - set(allowable_value_names) - }. """, - f"Currently-supported values in Obelisk are: {allowable_value_names}", - ) + f"obelisk_settings must be a YAML mapping at the top level; got {type(loaded).__name__}." + ) + return loaded @staticmethod - def _get_key_from_config_dict(config_dict: Dict) -> str: - """Get the key from a configuration dictionary.""" - assert "key" in config_dict, "No key supplied!" - assert isinstance(config_dict["key"], str), "The 'key' field must be a string!" - return config_dict["key"] + def _build_callback_groups(spec: Dict[str, str]) -> Dict[str, CallbackGroup]: + """Build callback groups from a ``{name: type_name}`` mapping.""" + groups: Dict[str, CallbackGroup] = {} + for name, type_name in spec.items(): + if type_name == "MutuallyExclusiveCallbackGroup": + groups[name] = MutuallyExclusiveCallbackGroup() + elif type_name == "ReentrantCallbackGroup": + groups[name] = ReentrantCallbackGroup() + else: + raise ValueError(f"Invalid callback group type: {type_name}") + return groups + + def _resolve_callback_group(self, entry: Dict[str, Any]) -> Optional[CallbackGroup]: + """Look up the callback group named in ``entry['callback_group']`` (if any).""" + cbg_name = entry.get("callback_group") + if cbg_name is None: + return None + if not isinstance(cbg_name, str): + raise ValueError(f"`callback_group` must be a string, got {type(cbg_name).__name__}.") + if cbg_name.lower() == "none": + return None + cbg = self.obk_callback_groups.get(cbg_name) + if cbg is None: + self.get_logger().warn(f"Callback group {cbg_name} not found in node. Using None instead.") + return cbg @staticmethod - def _create_callback_groups_from_config_str(config_str: str) -> Dict[str, CallbackGroup]: - """Create callback groups from a configuration string. - - Parameters: - config_str: The list of configuration strings. - - Returns: - callback_group_dict: A dict whose keys are the callback group names and values are the callback groups. + def _find_entry_by_key(entries: List[Dict[str, Any]], key: str) -> Optional[Dict[str, Any]]: + """Return the first entry in ``entries`` with ``entry['key'] == key``, or None.""" + for entry in entries: + if entry.get("key") == key: + return entry + return None - Raises: - ValueError: If the configuration string is invalid. - """ - # parse and check the configuration string - field_names, value_names = [], [] - for config in config_str.split(","): - field_name, value_name = ObeliskNode._parse_config_str(config) - field_names.extend(field_name) - value_names.extend(value_name) - - if not field_names and not value_names: - return {} # case: no callback groups to create - - allowable_value_names = ["MutuallyExclusiveCallbackGroup", "ReentrantCallbackGroup"] - ObeliskNode._check_values(value_names, allowable_value_names) - config_dict = dict(zip(field_names, value_names)) - - # create the callback groups - callback_group_dict = {} - for callback_group_name, callback_group_type in config_dict.items(): - if callback_group_type == "MutuallyExclusiveCallbackGroup": - callback_group = MutuallyExclusiveCallbackGroup() - elif callback_group_type == "ReentrantCallbackGroup": - callback_group = ReentrantCallbackGroup() - else: - raise ValueError(f"Invalid callback group type: {callback_group_type}") - callback_group_dict[callback_group_name] = callback_group + # ################### # + # LIFECYCLE CALLBACKS # + # ################### # - return callback_group_dict + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + """Configure the node: parse ``obelisk_settings`` and instantiate registered components.""" + super().on_configure(state) - # ####################### # - # INSTANCE-SPECIFIC UTILS # - # ####################### # + settings = self._load_obk_settings() - def _get_callback_group_from_config_dict(self, config_dict: Dict) -> Optional[CallbackGroup]: - """Get the callback group from a configuration dictionary.""" - if "callback_group" in config_dict: - assert isinstance(config_dict["callback_group"], str), "The 'callback_group' field must be a string!" - if config_dict["callback_group"].lower() == "none": - return None - else: - cbg = self.obk_callback_groups.get(config_dict["callback_group"], None) - if cbg is None: - self.get_logger().warn( - f"Callback group {config_dict['callback_group']} not found in node. Using None instead." - ) - return cbg - return None + # Callback groups first — components reference them by name. + cbg_spec = settings.get("callback_groups", {}) or {} + self.obk_callback_groups = ObeliskNode._build_callback_groups(cbg_spec) + for name, cbg in self.obk_callback_groups.items(): + setattr(self, name, cbg) - def _create_publisher_from_config_str( - self, - config_str: str, - msg_type: Type, - key: Optional[str] = None, - ) -> str: - """Create a publisher from a configuration string and adds it to the publisher dictionary. + pub_entries: List[Dict[str, Any]] = settings.get("publishers", []) or [] + sub_entries: List[Dict[str, Any]] = settings.get("subscribers", []) or [] + timer_entries: List[Dict[str, Any]] = settings.get("timers", []) or [] - Also creates a key attribute for the publisher. For example, if the key is "pub_ctrl", then the publisher can be - accessed as self.pub_ctrl. + # Publishers + for pub_dict in self._obk_pub_settings: + key = pub_dict["key"] + entry = self._find_entry_by_key(pub_entries, key) + if entry is None: + self.get_logger().warn(f"Publisher {key} has no entry in obelisk_settings.publishers!") + continue + self._create_publisher_from_entry(entry, key=key, msg_type=pub_dict["msg_type"]) - [NOTE] There are many unsupported features in this function, such as setting QoS profiles, callback groups, etc. - For now, we assume the only property of the QoS profile the user will set is history depth, and we don't even - expose the event_callbacks, qos_overriding_options, or publisher_class parameters. + # Subscriptions + for sub_dict in self._obk_sub_settings: + key = sub_dict["key"] + entry = self._find_entry_by_key(sub_entries, key) + if entry is None: + self.get_logger().warn(f"Subscription {key} has no entry in obelisk_settings.subscribers!") + continue + self._create_subscription_from_entry( + entry, key=key, msg_type=sub_dict["msg_type"], callback=sub_dict["callback"] + ) - Parameters: - config_str: The configuration string. - msg_type: The message type. If passed, we just use this directly. Otherwise, you can configure this at - runtime by passing a msg_type field in the config string that corresponds to an Obelisk message type. - key: The key for the publisher. If None, we look for the key as a field in the config string. + # Timers + for timer_dict in self._obk_timer_settings: + key = timer_dict["key"] + entry = self._find_entry_by_key(timer_entries, key) + if entry is None: + self.get_logger().warn(f"Timer {key} has no entry in obelisk_settings.timers!") + continue + self._create_timer_from_entry(entry, key=key, callback=timer_dict["callback"]) - Returns: - key: The key for the publisher. + self.get_logger().info(f"{self.get_name()} configured.") + return TransitionCallbackReturn.SUCCESS - Raises: - AssertionError: If the configuration string is invalid. - ObeliskMsgError: If the message type is not an Obelisk message. - """ - # parse and check the configuration string - field_names, value_names = ObeliskNode._parse_config_str(config_str) - required_field_names = ["topic"] - optional_field_names = ["key", "msg_type", "history_depth", "callback_group"] - ObeliskNode._check_fields(field_names, required_field_names, optional_field_names) - config_dict = dict(zip(field_names, value_names)) - - # parse the key - if key is None: - key = ObeliskNode._get_key_from_config_dict(config_dict) - elif "key" in field_names: - self.get_logger().warn( - f"'key'={key} registered for this publisher, and 'key'={config_dict['key']} specified in the config " - f"string. Using the value 'key'={key}, as hardcoded specifications take precedence!" - ) + # ############### # + # COMPONENT CREATE # + # ############### # - # set the callback group - callback_group = self._get_callback_group_from_config_dict(config_dict) + def _create_publisher_from_entry(self, entry: Dict[str, Any], key: str, msg_type: Type) -> None: + if "topic" not in entry: + raise ValueError(f"Publisher entry for key '{key}' is missing required field 'topic'.") + topic = entry["topic"] + if not isinstance(topic, str): + raise ValueError(f"Publisher '{key}': 'topic' must be a string, got {type(topic).__name__}.") + history_depth = entry.get("history_depth", 10) + if not isinstance(history_depth, int): + raise ValueError(f"Publisher '{key}': 'history_depth' must be an int, got {type(history_depth).__name__}.") - # run type assertions and create the publisher - history_depth = config_dict.get("history_depth", 10) - assert isinstance(config_dict["topic"], str), "The 'topic' field must be a string!" - assert isinstance(history_depth, int), "The 'history_depth' field must be an int!" self.obk_publishers[key] = self.create_publisher( msg_type=msg_type, - topic=config_dict["topic"], + topic=topic, qos_profile=history_depth, - callback_group=callback_group, + callback_group=self._resolve_callback_group(entry), ) - assert not hasattr(self, key), f"Attribute {key} already exists in the node!" - setattr(self, key + "_key", self.obk_publishers[key]) # create key attribute for publisher - return key - def _create_subscription_from_config_str( + def _create_subscription_from_entry( self, - config_str: str, + entry: Dict[str, Any], + key: str, msg_type: Type, callback: Callable[[MsgType], None], - key: Optional[str] = None, - ) -> str: - """Create a subscription from a configuration string and adds it to the subscription dictionary. - - Also creates a key attribute for the subscription. For example, if the key is "sub_ctrl", then the subscription - can be accessed as self.sub_ctrl. - - [NOTE] There are many unsupported features in this function, such as setting QoS profiles, callback groups, etc. - For now, we assume the only property of the QoS profile the user will set is history depth, and we don't even - expose the event_callbacks, qos_overriding_options, or raw parameters. - - Parameters: - config_str: The configuration string. - msg_type: The message type. If passed, we just use this directly. Otherwise, you can configure this at - runtime by passing a msg_type field in the config string that corresponds to an Obelisk message type. - callback: The callback function. - key: The key for the subscription. If None, we look for the key as a field in the config string. - - Returns: - key: The key for the subscription. - - Raises: - AssertionError: If the configuration string is invalid. - ObeliskMsgError: If the message type is not an Obelisk message. - """ - # parse and check the configuration string - field_names, value_names = ObeliskNode._parse_config_str(config_str) - required_field_names = ["topic"] - optional_field_names = ["key", "msg_type", "history_depth", "callback_group"] - ObeliskNode._check_fields(field_names, required_field_names, optional_field_names) - config_dict = dict(zip(field_names, value_names)) - - # parse the key - if key is None: - key = ObeliskNode._get_key_from_config_dict(config_dict) - elif "key" in field_names: - self.get_logger().warn( - f"'key'={key} registered for this subscription, and 'key'={config_dict['key']} specified in the config " - f"string. Using the value 'key'={key}, as hardcoded specifications take precedence!" + ) -> None: + if "topic" not in entry: + raise ValueError(f"Subscription entry for key '{key}' is missing required field 'topic'.") + topic = entry["topic"] + if not isinstance(topic, str): + raise ValueError(f"Subscription '{key}': 'topic' must be a string, got {type(topic).__name__}.") + history_depth = entry.get("history_depth", 10) + if not isinstance(history_depth, int): + raise ValueError( + f"Subscription '{key}': 'history_depth' must be an int, got {type(history_depth).__name__}." ) - # set the callback group - callback_group = self._get_callback_group_from_config_dict(config_dict) - - # run type assertions and return the subscription - history_depth = config_dict.get("history_depth", 10) - assert isinstance(config_dict["topic"], str), "The 'topic' field must be a string!" - assert isinstance(history_depth, int), "The 'history_depth' field must be an int!" - self.obk_subscriptions[key] = self.create_subscription( msg_type=msg_type, - topic=config_dict["topic"], - callback=callback, # type: ignore + topic=topic, + callback=callback, # type: ignore[arg-type] qos_profile=history_depth, - callback_group=callback_group, + callback_group=self._resolve_callback_group(entry), ) - assert not hasattr(self, key), f"Attribute {key} already exists in the node!" - setattr(self, key + "_key", self.obk_subscriptions[key]) # create key attribute for subscription - return key - def _create_timer_from_config_str( + def _create_timer_from_entry( self, - config_str: str, + entry: Dict[str, Any], + key: str, callback: Callable[[], Union[Any, Tuple[Any, ...]]], - key: Optional[str] = None, - ) -> str: - """Create a timer from a configuration string and adds it to the timer dictionary. - - Also creates a key attribute for the timer. For example, if the key is "timer_ctrl", then the timer can be - accessed as self.timer_ctrl. - - Parameters: - config_str: The configuration string. - callback: The callback function. - key: The key for the timer. If None, we look for the key as a field in the config string. - - Returns: - key: The key for the timer. - - Raises: - AssertionError: If the configuration string is invalid. - """ - # parse and check the configuration string - field_names, value_names = ObeliskNode._parse_config_str(config_str) - required_field_names = ["timer_period_sec"] - optional_field_names = ["key", "callback_group"] - ObeliskNode._check_fields(field_names, required_field_names, optional_field_names) - config_dict = dict(zip(field_names, value_names)) - - # parse the key - if key is None: - key = ObeliskNode._get_key_from_config_dict(config_dict) - elif "key" in field_names: - self.get_logger().warn( - f"'key'={key} registered for this timer, and 'key'={config_dict['key']} specified in the config " - f"string. Using the value 'key'={key}, as hardcoded specifications take precedence!" - ) - - # set the callback group - callback_group = self._get_callback_group_from_config_dict(config_dict) - - # run type assertions and return the timer - assert isinstance( - config_dict["timer_period_sec"], (int, float) - ), "The 'timer_period_sec' field must be a number!" + ) -> None: + if "timer_period_sec" not in entry: + raise ValueError(f"Timer entry for key '{key}' is missing required field 'timer_period_sec'.") + period = entry["timer_period_sec"] + if not isinstance(period, (int, float)): + raise ValueError(f"Timer '{key}': 'timer_period_sec' must be a number, got {type(period).__name__}.") timer = self.create_timer( - config_dict["timer_period_sec"], + float(period), callback=callback, - callback_group=callback_group, # type: ignore - # autostart=False, # TODO(ahl): feature only available after humble + callback_group=self._resolve_callback_group(entry), # type: ignore[arg-type] ) - timer.cancel() # initially, the timer should be deactivated, TODO(ahl): remove if distro upgraded + # Timers should start deactivated; they're enabled in on_activate. + timer.cancel() self.obk_timers[key] = timer - assert not hasattr(self, key), f"Attribute {key} already exists in the node!" - setattr(self, key + "_key", self.obk_timers[key]) # create key attribute for timer - return key - - # ################### # - # LIFECYCLE CALLBACKS # - # ################### # - - def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: - """Configure the node. - - Raises: - AssertionError: If the configuration string is invalid. - """ - super().on_configure(state) - - # parsing config strings - callback_group_settings = self.get_parameter("callback_group_settings").get_parameter_value().string_value - - # create callback groups - self.obk_callback_groups = ObeliskNode._create_callback_groups_from_config_str(callback_group_settings) - for callback_group_name, callback_group in self.obk_callback_groups.items(): - setattr(self, callback_group_name, callback_group) - - # create components - for pub_dict in self._obk_pub_settings: - key = pub_dict["key"] - ros_parameter = pub_dict["ros_parameter"] - msg_type = pub_dict["msg_type"] - - pub_config_str = self.get_parameter(ros_parameter).get_parameter_value().string_value - if pub_config_str == "": - self.get_logger().warn(f"Publisher {key} has no configuration string!") - continue - final_key = self._create_publisher_from_config_str(pub_config_str, key=key, msg_type=msg_type) - pub_dict["key"] = final_key # if no key passed, use value from config file - - for sub_dict in self._obk_sub_settings: - key = sub_dict["key"] - ros_parameter = sub_dict["ros_parameter"] - msg_type = sub_dict["msg_type"] - callback = sub_dict["callback"] - - sub_config_str = self.get_parameter(ros_parameter).get_parameter_value().string_value - if sub_config_str == "": - self.get_logger().warn(f"Subscription {key} has no configuration string!") - continue - final_key = self._create_subscription_from_config_str( - sub_config_str, callback=callback, key=key, msg_type=msg_type - ) - sub_dict["key"] = final_key # if no key passed, use value from config file - - for timer_dict in self._obk_timer_settings: - key = timer_dict["key"] - ros_parameter = timer_dict["ros_parameter"] - callback = timer_dict["callback"] - - timer_config_str = self.get_parameter(ros_parameter).get_parameter_value().string_value - if timer_config_str == "": - self.get_logger().warn(f"Timer {key} has no configuration string!") - continue - final_key = self._create_timer_from_config_str(timer_config_str, callback=callback, key=key) - timer_dict["key"] = final_key # if no key passed, use value from config file - - self.get_logger().info(f"{self.get_name()} configured.") - return TransitionCallbackReturn.SUCCESS def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: """Activate the node.""" super().on_activate(state) for timer in self.obk_timers.values(): - timer.reset() # activate timers - + timer.reset() self.get_logger().info(f"{self.get_name()} activated.") return TransitionCallbackReturn.SUCCESS @@ -567,8 +311,7 @@ def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: """Deactivate the node.""" super().on_deactivate(state) for timer in self.obk_timers.values(): - timer.cancel() # deactivate timers - + timer.cancel() self.get_logger().info(f"{self.get_name()} deactivated.") return TransitionCallbackReturn.SUCCESS @@ -576,21 +319,17 @@ def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn: """Clean up the node.""" super().on_cleanup(state) - # reset internal dicts self.obk_callback_groups = {} self.obk_publishers = {} self.obk_subscriptions = {} self.obk_timers = {} - # destroy publishers, timers, and subscriptions if self.timers is not None: for timer in self.timers: self.destroy_timer(timer) - if self.publishers is not None: for publisher in self.publishers: self.destroy_publisher(publisher) - if self.subscriptions is not None: for subscription in self.subscriptions: self.destroy_subscription(subscription) @@ -599,7 +338,7 @@ def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn: return TransitionCallbackReturn.SUCCESS def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: - """Shut down the controller.""" + """Shut down the node.""" super().on_shutdown(state) self.on_cleanup(state) self.get_logger().info(f"{self.get_name()} shut down.") diff --git a/obelisk/python/obelisk_py/core/robot.py b/obelisk/python/obelisk_py/core/robot.py index 80b55b42..1596e45e 100644 --- a/obelisk/python/obelisk_py/core/robot.py +++ b/obelisk/python/obelisk_py/core/robot.py @@ -20,10 +20,9 @@ def __init__(self, node_name: str, ctrl_msg_type: Type) -> None: """Initialize the Obelisk robot.""" super().__init__(node_name) self.register_obk_subscription( - "sub_ctrl_setting", - self.apply_control, - ctrl_msg_type, key="sub_ctrl", + callback=self.apply_control, + msg_type=ctrl_msg_type, ) @abstractmethod @@ -51,18 +50,8 @@ class ObeliskSimRobot(ObeliskRobot): def __init__(self, node_name: str, ctrl_msg_type: Type) -> None: """Initialize the Obelisk sim robot.""" super().__init__(node_name, ctrl_msg_type) - self.register_obk_timer( - "timer_true_sim_state_setting", - self.publish_true_sim_state, - key="timer_true_sim_state", - default_config_str="", - ) - self.register_obk_publisher( - "pub_true_sim_state_setting", - osm.TrueSimState, - key="pub_true_sim_state", - default_config_str="", - ) + self.register_obk_timer(key="timer_true_sim_state", callback=self.publish_true_sim_state) + self.register_obk_publisher(key="pub_true_sim_state", msg_type=osm.TrueSimState) self.shared_ctrl = None def _set_shared_ctrl(self, ctrl: List[float]) -> None: @@ -89,8 +78,8 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: """Configure the simulation.""" super().on_configure(state) - # checking the settings of the true sim state pub/timer - if "publisher_true_sim_state" in self.obk_publishers and "timer_true_sim_state" in self.obk_timers: + # checking the settings of the true sim state pub/timer (optional in the YAML) + if "pub_true_sim_state" in self.obk_publishers and "timer_true_sim_state" in self.obk_timers: assert ( self.obk_timers["timer_true_sim_state"].callback == self.publish_true_sim_state ), f"Timer callback must be publish_true_sim_state! Is {self.obk_timers['timer_true_sim_state'].callback}." diff --git a/obelisk/python/obelisk_py/core/utils/launch_utils.py b/obelisk/python/obelisk_py/core/utils/launch_utils.py index a6739984..fc681fab 100644 --- a/obelisk/python/obelisk_py/core/utils/launch_utils.py +++ b/obelisk/python/obelisk_py/core/utils/launch_utils.py @@ -1,163 +1,209 @@ import os -import re from datetime import datetime from pathlib import Path -from typing import Dict, List, Optional, Union +from typing import Any, Dict, List, Optional, Union import launch import launch_ros import lifecycle_msgs.msg +import yaml from ament_index_python.packages import get_package_share_directory -from launch.actions import EmitEvent, IncludeLaunchDescription, RegisterEventHandler -from launch.launch_description_sources import FrontendLaunchDescriptionSource +from launch.actions import EmitEvent, RegisterEventHandler from launch_ros.actions import LifecycleNode, Node from launch_ros.events.lifecycle import ChangeState from ruamel.yaml import YAML +# Sections of a node's settings dict that Obelisk owns and bundles into the single +# `obelisk_settings` ROS parameter at launch time. +_OBELISK_SETTINGS_KEYS = ("publishers", "subscribers", "timers", "sim", "callback_groups") + def load_config_file(file_path: Union[str, Path], package_name: Optional[str] = None) -> Dict: - """Loads an Obelisk configuration file. + """Loads an Obelisk configuration file, recursively expanding ``include:`` directives. + + The file at ``file_path`` is the *primary* config: an absolute path is used as-is, otherwise + the path resolves against the ``config/`` subdirectory of an installed ROS2 package (default + ``obelisk_ros``). + + If the loaded YAML has a top-level ``include:`` key (a list of paths), each entry is loaded + recursively and merged into a single dict before the parent's own keys are applied. Paths inside + an ``include:`` resolve **relative to the directory of the file containing the include**; + absolute paths still pass through unchanged. The ``include:`` key is stripped from the result. + + Merge rules (parent = file containing the include, children = files it includes): + * List-shaped sections (``control``, ``estimation``, ``robot``, ``sensing``) **concatenate** + in include order, with the parent's entries appended last. + * Dict-shaped sections (``joystick``, ``viz``) **deep-merge**, with parent values winning on + scalar conflicts. + * Scalar / other keys (e.g. ``config``, ``params_path``): parent wins. Parameters: - file_path: the path to the configuration file, either absolute or relative to the 'config' directory of an - installed ROS2 package. - package_name: the name of the ROS2 package that contains the configuration file. If not provided, the - 'obelisk_ros' package is assumed. + file_path: path to the configuration file. + package_name: the ROS2 package whose ``config/`` dir to resolve relative ``file_path`` + against. If None, ``obelisk_ros`` is used. Returns: - config: A dictionary containing the configuration settings. + config: A dictionary containing the merged configuration settings. + + Raises: + FileNotFoundError: if the file (or any include) cannot be located/parsed. + RuntimeError: if the include graph contains a cycle. """ + abs_file_path = _resolve_primary_config_path(file_path, package_name) + return _load_config_recursive(abs_file_path, stack=[]) + + +def _resolve_primary_config_path(file_path: Union[str, Path], package_name: Optional[str]) -> Path: + """Resolve the primary (top-level) config path: absolute as-is, else under /share/config/.""" file_path = str(file_path) if not file_path.startswith("/"): try: obk_ros_dir = get_package_share_directory("obelisk_ros" if package_name is None else package_name) - abs_file_path = Path(obk_ros_dir) / "config" / file_path except Exception as e: raise FileNotFoundError( f"Could not find package {package_name}. If you supply a relative path to the config file, it must lie " "in a subdirectory named 'config' of either the obelisk_ros package or some other ROS2 package of your " "choice. Otherwise, you can supply an absolute path to the config file." ) from e - else: - abs_file_path = Path(file_path) + return Path(obk_ros_dir) / "config" / file_path + return Path(file_path) + + +def _load_config_recursive(abs_file_path: Path, stack: List[Path]) -> Dict: + """Load a single YAML and recursively expand any ``include:`` it has, merging the results. - yaml = YAML(typ="safe") + ``stack`` tracks the chain of resolved-absolute paths so we can detect include cycles. + """ + abs_file_path = abs_file_path.resolve() + if abs_file_path in stack: + chain = " -> ".join(str(p) for p in stack + [abs_file_path]) + raise RuntimeError(f"Cycle detected in obelisk config include chain: {chain}") + + loader = YAML(typ="safe") try: - return yaml.load(abs_file_path) + raw = loader.load(abs_file_path) except Exception as e: raise FileNotFoundError(f"Could not load a configuration file at {abs_file_path}!") from e + # An empty or null YAML file should be treated as an empty mapping rather than failing. + if raw is None: + raw = {} -def get_component_settings_subdict(node_settings: Dict, subdict_name: str) -> Dict: - """Returns a subdictionary of the component settings associated with an Obelisk node. - - Parameters: - node_settings: the settings dictionary of an Obelisk node. - subdict_name: the name of the subdictionary to extract from the node settings. + includes = raw.pop("include", []) or [] + if not isinstance(includes, list): + raise ValueError( + f"`include:` in {abs_file_path} must be a list of YAML paths; got {type(includes).__name__}." + ) - Returns: - A dictionary with the ROS parameter names as keys and the corresponding settings as values. - """ - component_settings_subdict = {} + new_stack = stack + [abs_file_path] + base_dir = abs_file_path.parent - # iterate over the settings for each component - if doesn't exist, make a new dict entry, else append it - for component_settings in node_settings[subdict_name]: - if component_settings["ros_parameter"] not in component_settings_subdict: - component_settings_subdict[component_settings["ros_parameter"]] = [ - ",".join([f"{k}:{v}" for k, v in component_settings.items() if k != "ros_parameter"]) - ] - else: - component_settings_subdict[component_settings["ros_parameter"]].append( - ",".join([f"{k}:{v}" for k, v in component_settings.items() if k != "ros_parameter"]) - ) + merged: Dict[str, Any] = {} + for inc in includes: + inc_path = Path(str(inc)) + # Includes are resolved relative to the directory of the file containing the include. + # Absolute paths pass through unchanged. + if not inc_path.is_absolute(): + inc_path = base_dir / inc_path + included = _load_config_recursive(inc_path, new_stack) + merged = _merge_obelisk_configs(merged, included) - # if there is only one setting, don't return a list, just a single element - for ros_parameter, pub_settings_list in component_settings_subdict.items(): - if len(pub_settings_list) == 1: - component_settings_subdict[ros_parameter] = pub_settings_list[0] + # Parent's own keys take precedence over included ones. + return _merge_obelisk_configs(merged, raw) - return component_settings_subdict +# Top-level sections whose values are lists of complete node entries; concatenated when merging. +_LIST_SECTIONS = ("control", "estimation", "robot", "sensing") -def get_component_sim_settings_subdict(node_settings: Dict) -> Dict: - """Returns a subdictionary of the simulation settings associated with an Obelisk node. - Parameters: - node_settings: the settings dictionary of an Obelisk node. +def _merge_obelisk_configs(base: Dict, override: Dict) -> Dict: + """Merge ``override`` onto ``base`` according to the obelisk schema rules. - Returns: - A dictionary with the ROS parameter names as keys and the corresponding settings as values. + List-shaped sections concat (with override's entries appended); dicts deep-merge with override + winning on scalar conflicts; everything else: override wins. """ - sim_settings_dict = {} - for sim_settings in node_settings["sim"]: - # convert the dictionary of simulation settings to a string, excluding 'ros_parameter' - sim_settings_strs = [] - for k, v in sim_settings.items(): - if k != "ros_parameter": - sim_settings_strs.append(f"{k}:{v}") - sim_settings_str = ",".join(sim_settings_strs).replace(" ", "") - - # replace colons in 'sensor_names':{...} with '$', delete curly braces only within the outermost braces - def _replace_colons_delete_inner_braces(match: re.Match) -> str: - dollar_str = match.group(0).replace(":", "$") - dollar_str = dollar_str.replace("$", ":", 1) - - # find the position of the outermost braces - open_brace = dollar_str.find("{") - close_brace = dollar_str.rfind("}") - - if open_brace != -1 and close_brace != -1 and open_brace < close_brace: - # keep content before the first brace and after the last brace - prefix = dollar_str[: open_brace + 1] - suffix = dollar_str[close_brace:] - - # remove braces from the inner content - inner_content = dollar_str[open_brace + 1 : close_brace] - inner_content_no_braces = inner_content.replace("{", "").replace("}", "") - - return f"{prefix}{inner_content_no_braces}{suffix}" - else: - # if we can't find matching outermost braces, return the original string (SHOULD NEVER HAPPEN) - return dollar_str - - sim_settings_str = re.sub(r"'sensor_names':\{[^\}]*\}", _replace_colons_delete_inner_braces, sim_settings_str) - - # replace commas in 'sensor_names':{...} with '&', remove outermost braces - def _replace_commas_delete_outer_braces(match: re.Match) -> str: - return match.group(0).replace(",", "&").replace("{", "").replace("}", "") - - sim_settings_str = re.sub(r"'sensor_names':\{[^\}]*\}", _replace_commas_delete_outer_braces, sim_settings_str) + out: Dict[str, Any] = dict(base) + for k, v in override.items(): + if k in _LIST_SECTIONS and isinstance(out.get(k), list) and isinstance(v, list): + out[k] = list(out[k]) + list(v) + elif isinstance(out.get(k), dict) and isinstance(v, dict): + out[k] = _deep_merge(out[k], v) + else: + out[k] = v + return out - # remove single quotes - sim_settings_str = sim_settings_str.replace("'", "") - # replace commas between matching curly braces with "|" - def _replace_commas_between_curly_braces(match: re.Match) -> str: - return match.group(0).replace(",", "|") +def _deep_merge(a: Dict, b: Dict) -> Dict: + """Recursive dict merge; ``b`` wins on scalar conflicts. Returns a new dict.""" + out = dict(a) + for k, v in b.items(): + if isinstance(out.get(k), dict) and isinstance(v, dict): + out[k] = _deep_merge(out[k], v) + else: + out[k] = v + return out - sim_settings_str = re.sub(r"\{[^{}]*\}", _replace_commas_between_curly_braces, sim_settings_str) - # replace commas in 'sensor_settings':[...,...] with "+" - def _replace_commas_in_sensor_settings(match: re.Match) -> str: - return match.group(0).replace(",", "+") +def _strip_ros_parameter(entry: Dict[str, Any]) -> Dict[str, Any]: + """Return a shallow copy of an entry with ``ros_parameter`` dropped, deriving ``key`` if needed. - sim_settings_str = re.sub(r"sensor_settings:\[.*?\]", _replace_commas_in_sensor_settings, sim_settings_str) + The pre-refactor YAML schema named each pub/sub/timer entry by ``ros_parameter`` (e.g. + ``pub_ctrl_setting``). The new schema uses ``key`` (e.g. ``pub_ctrl``). For YAMLs that haven't + been migrated yet, derive ``key`` by stripping a trailing ``_setting``. + """ + cleaned = {k: v for k, v in entry.items() if k != "ros_parameter"} + if "key" not in cleaned and "ros_parameter" in entry: + rp = entry["ros_parameter"] + if isinstance(rp, str): + cleaned["key"] = rp[: -len("_setting")] if rp.endswith("_setting") else rp + return cleaned - # replace colons inside 'sensor_settings':[...:...] with "=" - def _replace_colons_in_sensor_settings(match: re.Match) -> str: - content = match.group(0) - return re.sub(r"(?<=\[).*?(?=\])", lambda m: m.group(0).replace(":", "="), content) - sim_settings_str = re.sub(r"sensor_settings:\[.*?\]", _replace_colons_in_sensor_settings, sim_settings_str) +def _build_obelisk_settings(node_settings: Dict) -> str: + """Bundle the publishers / subscribers / timers / callback_groups sections into a single YAML string. - # add the formatted string to the dictionary with the ROS parameter as the key - sim_settings_dict[sim_settings["ros_parameter"]] = sim_settings_str + Returns the empty string if the node has no Obelisk-internal sections (caller can then skip + setting the ``obelisk_settings`` ROS parameter entirely if desired, though setting it to an + empty string is also valid — the node treats empty as "no components"). - return sim_settings_dict + Note: ``sim`` entries are *not* bundled here. They are emitted as separate ROS parameters + (one per ``ros_parameter`` in the entry, e.g. ``mujoco_setting``) so that simulator nodes can + read their dedicated setting independently. Each sim entry's value is YAML-encoded. + """ + bundle: Dict[str, Any] = {} + for section in ("publishers", "subscribers", "timers"): + if section in node_settings and node_settings[section] is not None: + bundle[section] = [_strip_ros_parameter(entry) for entry in node_settings[section]] + if "callback_groups" in node_settings and node_settings["callback_groups"] is not None: + bundle["callback_groups"] = dict(node_settings["callback_groups"]) + if not bundle: + return "" + return yaml.safe_dump(bundle, default_flow_style=False, sort_keys=False) + + +def _build_sim_parameters(sim_entries: List[Dict[str, Any]]) -> Dict[str, str]: + """Emit one YAML-string ROS parameter per sim entry, named by its ``ros_parameter`` field. + + Each entry's content (minus the ``ros_parameter`` key itself) is yaml.safe_dumped and used as + the value of the ROS parameter. The simulator node parses the YAML in its on_configure. + """ + out: Dict[str, str] = {} + for entry in sim_entries: + if "ros_parameter" not in entry: + raise KeyError("Each sim entry must specify a `ros_parameter` for the simulator's settings parameter.") + param_name = entry["ros_parameter"] + body = {k: v for k, v in entry.items() if k != "ros_parameter"} + out[param_name] = yaml.safe_dump(body, default_flow_style=False, sort_keys=False) + return out def get_parameters_dict(node_settings: Dict) -> Dict: - """Returns the parameters dictionary corresponding to the settings of a node in the Obelisk architecture. + """Returns the parameters dictionary corresponding to the settings of a node. + + The Obelisk-internal sections (``publishers``, ``subscribers``, ``timers``, ``sim``, + ``callback_groups``) are bundled into a single ``obelisk_settings`` ROS parameter whose value is + a YAML string. User-defined ``params`` and ``params_path`` entries flow through as separate ROS + parameters, unchanged. Parameters: node_settings: the settings dictionary of an Obelisk node. @@ -168,32 +214,20 @@ def get_parameters_dict(node_settings: Dict) -> Dict: if node_settings is None: return {} - # parse settings for each component - if "callback_groups" in node_settings: - callback_group_settings = ",".join([f"{k}:{v}" for k, v in node_settings["callback_groups"].items()]) - else: - callback_group_settings = None - pub_settings_dict = ( - get_component_settings_subdict(node_settings, "publishers") if "publishers" in node_settings else {} - ) - sub_settings_dict = ( - get_component_settings_subdict(node_settings, "subscribers") if "subscribers" in node_settings else {} - ) - timer_settings_dict = get_component_settings_subdict(node_settings, "timers") if "timers" in node_settings else {} - sim_settings_dict = get_component_sim_settings_subdict(node_settings) if "sim" in node_settings else {} + parameters_dict: Dict[str, Any] = {} + + bundled = _build_obelisk_settings(node_settings) + if bundled: + parameters_dict["obelisk_settings"] = bundled + + if "sim" in node_settings and node_settings["sim"] is not None: + parameters_dict.update(_build_sim_parameters(node_settings["sim"])) - # create parameters dictionary for a node by combining all the settings - parameters_dict = ( - {"callback_group_settings": callback_group_settings} if callback_group_settings is not None else {} - ) if "params_path" in node_settings: parameters_dict["params_path"] = node_settings["params_path"] if "params" in node_settings: parameters_dict.update(node_settings["params"]) - parameters_dict.update(pub_settings_dict) - parameters_dict.update(sub_settings_dict) - parameters_dict.update(timer_settings_dict) - parameters_dict.update(sim_settings_dict) + return parameters_dict @@ -315,44 +349,28 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None for i, viz_node_settings in enumerate(node_settings): launch_actions += _single_viz_node_launch_actions(viz_node_settings, suffix=i) - if "viz_tool" not in settings or settings["viz_tool"] == "rviz": - # Setup Rviz - if "rviz_config" not in settings.keys(): - launch_actions += [ - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - ) - ] - else: - rviz_file_name = "rviz/" + settings["rviz_config"] - rviz_config_path = os.path.join(get_package_share_directory(settings["rviz_pkg"]), rviz_file_name) - launch_actions += [ - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", rviz_config_path], - ) - ] - elif settings["viz_tool"] == "foxglove": - # setup fox glove - xml_launch_file = os.path.join( - get_package_share_directory("foxglove_bridge"), "launch", "foxglove_bridge_launch.xml" - ) + # Setup Rviz + if "rviz_config" not in settings.keys(): launch_actions += [ - IncludeLaunchDescription( - FrontendLaunchDescriptionSource(xml_launch_file), - launch_arguments={ - "capabilities": "[clientPublish,parametersSubscribe,services,connectionGraph,assets]" - }.items(), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", ) ] else: - raise RuntimeError("Invalid setting for `viz_tool`. Must be either `rviz` of `foxglove`.") + rviz_file_name = "rviz/" + settings["rviz_config"] + rviz_config_path = os.path.join(get_package_share_directory(settings["rviz_pkg"]), rviz_file_name) + launch_actions += [ + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_config_path], + ) + ] return launch_actions diff --git a/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py b/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py deleted file mode 100644 index ba9bb6d8..00000000 --- a/obelisk/python/obelisk_py/zoo/control/example/leap_example_pos_setpoint_controller.py +++ /dev/null @@ -1,45 +0,0 @@ -from typing import Type - -import numpy as np -from obelisk_control_msgs.msg import PositionSetpoint -from obelisk_estimator_msgs.msg import EstimatedState -from rclpy.lifecycle import LifecycleState, TransitionCallbackReturn - -from obelisk_py.core.control import ObeliskController -from obelisk_py.core.obelisk_typing import ObeliskControlMsg, is_in_bound - - -class LeapExamplePositionSetpointController(ObeliskController): - """Example position setpoint controller.""" - - def __init__(self, node_name: str = "leap_example_position_setpoint_controller") -> None: - """Initialize the example position setpoint controller.""" - super().__init__(node_name, PositionSetpoint, EstimatedState) - - def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: - """Configure the controller.""" - super().on_configure(state) - self.joint_pos = None - return TransitionCallbackReturn.SUCCESS - - def update_x_hat(self, x_hat_msg: Type) -> None: - """Update the state estimate. - - Parameters: - x_hat_msg: The Obelisk message containing the state estimate. - """ - pass # do nothing - - def compute_control(self) -> Type: - """Compute the control signal for the LEAP hand. - - Returns: - The control message. - """ - # setting the message - position_setpoint_msg = PositionSetpoint() - position_setpoint_msg.u_mujoco = [(0.3 * np.sin(self.t)) for _ in range(16)] # example state-independent input - position_setpoint_msg.q_des = [(0.3 * np.sin(self.t)) for _ in range(16)] # example state-independent input - self.obk_publishers["pub_ctrl"].publish(position_setpoint_msg) - assert is_in_bound(type(position_setpoint_msg), ObeliskControlMsg) - return position_setpoint_msg # type: ignore diff --git a/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py b/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py index ebc11705..ae60ae9b 100644 --- a/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py +++ b/obelisk/python/obelisk_py/zoo/estimation/jointencoders_passthrough_estimator.py @@ -14,10 +14,9 @@ def __init__(self, node_name: str = "joint_encoders_passthrough_estimator") -> N """Initialize the joint encoders passthrough estimator.""" super().__init__(node_name, EstimatedState) self.register_obk_subscription( - "sub_sensor_setting", - self.joint_encoder_callback, # type: ignore - ObkJointEncoders, - key="sub_sensor", # key can be specified here or in the config file + key="sub_sensor", + callback=self.joint_encoder_callback, # type: ignore + msg_type=ObkJointEncoders, ) def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: diff --git a/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp b/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp index 42c36d24..22ad5e4a 100644 --- a/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp +++ b/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp @@ -5,16 +5,14 @@ #include int main(int argc, char* argv[]) { - std::string height_scan_config = "/home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml"; - std::string lidar_config = "/home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml"; - - // Allow config paths as arguments - if (argc >= 2) { - height_scan_config = argv[1]; - } - if (argc >= 3) { - lidar_config = argv[2]; + if (argc < 3) { + std::cerr << "Usage: ray_caster_demo \n" + << " Each argument is a path to a YAML config (same schema as the inline\n" + << " scan_config: blocks under sensor_settings in the obelisk launch YAMLs).\n"; + return 1; } + std::string height_scan_config = argv[1]; + std::string lidar_config = argv[2]; std::cout << "=== Height Scan Interface ===" << std::endl; { diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt index ee012ae4..a3e54593 100644 --- a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt +++ b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt @@ -19,7 +19,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ObkJointEncoders.msg" "msg/TrueSimState.msg" "msg/ObkImage.msg" - "msg/ObkImu.msg" "msg/ObkFramePose.msg" "msg/ObkForceSensor.msg" "msg/ObkScan.msg" diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkImu.msg b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkImu.msg deleted file mode 100644 index 323ad446..00000000 --- a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkImu.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Obelisk compliant message type to hold IMU information. -# This message type generally mimics the ROS2 sensor_msgs/msg/imu type. - -string MESSAGE_NAME="ObkImu" - -std_msgs/Header header - -geometry_msgs/Quaternion orientation - -# This is the covariance matrix for elements of the tangent space. -# This means that this represents a distribution about the identity element of a quaternion in the tangent space. -# To convert from the tangent space to a quaternion element you must use an exp map. -float64[9] orientation_covariance - - -geometry_msgs/Vector3 angular_velocity -float64[9] angular_velocity_covariance # Row major about x, y, z axes - - -geometry_msgs/Vector3 linear_acceleration -float64[9] linear_acceleration_covariance # Row major x, y z diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_composed.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_composed.yaml new file mode 100644 index 00000000..c0f7c4bc --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/dummy_composed.yaml @@ -0,0 +1,16 @@ +# Demo of the `include:` composition feature. +# +# Equivalent to dummy_cpp.yaml, but factored into reusable pieces: +# - dummy_composed_base.yaml — control + estimation + joystick (would also be reused +# by a hypothetical hardware variant) +# - dummy_composed_robot_sim.yaml — the sim-mode robot backend +# +# Run with: +# obk-launch config=dummy_composed.yaml +# +# The launch result should be identical to `obk-launch config=dummy_cpp.yaml` (modulo the run +# name `config:` below). +config: dummy_composed +include: + - dummy_composed_base.yaml + - dummy_composed_robot_sim.yaml diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_composed_base.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_composed_base.yaml new file mode 100644 index 00000000..dd7dd1d4 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/dummy_composed_base.yaml @@ -0,0 +1,54 @@ +# Shared dummy stack: controller + estimator + joystick. +# Reused by sim and (in the future) hardware variants. Pulled in via `include:` from a wrapper +# config like dummy_composed.yaml. +control: + - pkg: obelisk_control_cpp + executable: example_position_setpoint_controller + params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt + callback_groups: + test_cbg: MutuallyExclusiveCallbackGroup + params: + test_param: value_configured_in_yaml + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + - ros_parameter: joystick_feedback_setting + topic: /obelisk/dummy/joy_feedback + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + - ros_parameter: joystick_sub_setting + topic: /obelisk/dummy/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: test_cbg + +estimation: + - pkg: obelisk_estimation_cpp + executable: jointencoders_passthrough_estimator + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + msg_type: EstimatedState + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/dummy/joint_encoders + msg_type: JointEncoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None + +joystick: + on: True + pub_topic: /obelisk/dummy/joy + sub_topic: /obelisk/dummy/joy_feedback diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_composed_robot_sim.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_composed_robot_sim.yaml new file mode 100644 index 00000000..6f400024 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/dummy_composed_robot_sim.yaml @@ -0,0 +1,49 @@ +# Sim-mode robot backend for the dummy stack. Pairs with dummy_composed_base.yaml via include. +robot: + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + params: + ic_keyframe: ic + height_map_grid_size: [1., 1.] + height_map_grid_spacing: 0.1 + height_map_geom_group: [0] + subscribers: + - ros_parameter: sub_ctrl_setting + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: PoseStamped + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + - topic: /obelisk/dummy/odom + dt: 0.005 + msg_type: Odometry + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + tip_velocimeter: velocimeter + tip_gyro_sensor: gyro diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml index 96e83374..1f704d5a 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml @@ -1,109 +1,108 @@ config: dummy -onboard: - control: - - pkg: obelisk_control_cpp - executable: example_position_setpoint_controller - params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt - callback_groups: - test_cbg: MutuallyExclusiveCallbackGroup - params: - test_param: value_configured_in_yaml - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/dummy/ctrl - history_depth: 10 - callback_group: None - # ----- Joystick publisher ----- # - - ros_parameter: joystick_feedback_setting - topic: /obelisk/dummy/joy_feedback - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/dummy/est - history_depth: 10 - callback_group: None - # ----- Joystick subscriber ----- # - - ros_parameter: joystick_sub_setting - topic: /obelisk/dummy/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: test_cbg - estimation: - - pkg: obelisk_estimation_cpp - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/dummy/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - topic: /obelisk/dummy/joint_encoders - msg_type: JointEncoders - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - - is_simulated: True - pkg: obelisk_sim_cpp - executable: obelisk_mujoco_robot - params: - ic_keyframe: ic - height_map_grid_size: [1., 1.] - height_map_grid_spacing: 0.1 - height_map_geom_group: [0] - # camera_name: "fixed_cam" # You can select a camera to use here - subscribers: - - ros_parameter: sub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - num_steps_per_viz: 5 - robot_pkg: dummy_bot - model_xml_path: dummy.xml - sensor_settings: - - topic: /obelisk/dummy/joint_encoders - dt: 0.001 - msg_type: ObkJointEncoders - sensor_names: - joint_pos: jointpos - joint_vel: jointvel - - topic: /obelisk/dummy/imu - dt: 0.002 - msg_type: Imu - sensor_names: - tip_acc_sensor: accelerometer - tip_gyro_sensor: gyro - tip_frame_sensor: framequat - - topic: /obelisk/dummy/framepose - dt: 0.002 - msg_type: PoseStamped - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - - topic: /obelisk/dummy/odom - dt: 0.005 - msg_type: Odometry - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - tip_velocimeter: velocimeter - tip_gyro_sensor: gyro - - topic: /obelisk/dummy/scan_dots - dt: 0.1 - msg_type: GridCells - sensor_names: - tip_pos_sensor: framepos - joystick: - on: True - pub_topic: /obelisk/dummy/joy - sub_topic: /obelisk/dummy/joy_feedback +control: + - pkg: obelisk_control_cpp + executable: example_position_setpoint_controller + params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt + callback_groups: + test_cbg: MutuallyExclusiveCallbackGroup + params: + test_param: value_configured_in_yaml + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + # ----- Joystick publisher ----- # + - ros_parameter: joystick_feedback_setting + topic: /obelisk/dummy/joy_feedback + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + # ----- Joystick subscriber ----- # + - ros_parameter: joystick_sub_setting + topic: /obelisk/dummy/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: test_cbg +estimation: + - pkg: obelisk_estimation_cpp + executable: jointencoders_passthrough_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + msg_type: EstimatedState + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/dummy/joint_encoders + msg_type: JointEncoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + params: + ic_keyframe: ic + height_map_grid_size: [1., 1.] + height_map_grid_spacing: 0.1 + height_map_geom_group: [0] + # camera_name: "fixed_cam" # You can select a camera to use here + subscribers: + - ros_parameter: sub_ctrl_setting + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: PoseStamped + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + - topic: /obelisk/dummy/odom + dt: 0.005 + msg_type: Odometry + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + tip_velocimeter: velocimeter + tip_gyro_sensor: gyro + - topic: /obelisk/dummy/scan_dots + dt: 0.1 + msg_type: GridCells + sensor_names: + tip_pos_sensor: framepos +joystick: + on: True + pub_topic: /obelisk/dummy/joy + sub_topic: /obelisk/dummy/joy_feedback diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_scan.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_scan.yaml new file mode 100644 index 00000000..acb43215 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_scan.yaml @@ -0,0 +1,150 @@ +config: dummy_scan +# Same as dummy_cpp.yaml, plus three ray-caster sensors (height-scan, lidar, depth-camera) +# configured inline via `scan_config:` blocks (no external YAML file required). +# +# Run with: +# obk-launch config=dummy_cpp_scan.yaml auto_start=True bag=False +# +# Then in another shell: +# ros2 topic list # expect /obelisk/dummy/{height_scan,lidar,depth} +# ros2 topic hz /obelisk/dummy/lidar # confirm 10 Hz +# ros2 topic echo /obelisk/dummy/height_scan --once +control: + - pkg: obelisk_control_cpp + executable: example_position_setpoint_controller + params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt + params: + test_param: value_configured_in_yaml + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + - ros_parameter: joystick_feedback_setting + topic: /obelisk/dummy/joy_feedback + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + - ros_parameter: joystick_sub_setting + topic: /obelisk/dummy/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +estimation: + - pkg: obelisk_estimation_cpp + executable: jointencoders_passthrough_estimator + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/dummy/joint_encoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +robot: + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + params: + ic_keyframe: ic + subscribers: + - ros_parameter: sub_ctrl_setting + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + # ---- Standard MuJoCo sensors (joints + IMU) ---- + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.01 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + + # ---- Height-scan (ObkScan) using HeightScanInterface ---- + # Grid of downward rays, 1.6m x 0.8m at 0.1m resolution. Origin lifted 20m above + # the `tip` site so rays travel down to hit the floor / boxes regardless of robot pose. + - topic: /obelisk/dummy/height_scan + dt: 0.1 + msg_type: ObkScan + sensor_names: + tip_pos_sensor: framepos + scan_config: + site_name: tip + geom_groups: [0] + pattern: + type: height_scan + ordering: xy + resolution: 0.1 + size: [1.6, 0.8] + offset: + pos: [0.0, 0.0, 20.0] + + # ---- Lidar (PointCloud2) using LidarInterface ---- + # 16-channel spherical pattern emulating a small Velodyne-style sensor. + - topic: /obelisk/dummy/lidar + dt: 0.1 + msg_type: PointCloud2 + sensor_names: + tip_pos_sensor: framepos + scan_config: + site_name: tip + geom_groups: [0] + frame: world + max_distance: 10.0 + pattern: + type: lidar_scan + vertical_fov_range: [-15.0, 15.0] + horizontal_fov_range: [0.0, 360.0] + channels: 16 + horizontal_res: 2.0 + offset: + pos: [0.0, 0.0, 0.0] + rot: [1.0, 0.0, 0.0, 0.0] + + # ---- Depth camera (DepthImage) using DepthInterface ---- + # 32x24 pinhole-camera depth image, looking forward from the `tip` site. + - topic: /obelisk/dummy/depth + dt: 0.05 + msg_type: DepthImage + sensor_names: + tip_pos_sensor: framepos + scan_config: + site_name: tip + geom_groups: [0] + frame: world + pattern: + type: depth_camera + width: 32 + height: 24 + intrinsic_matrix: [25.9928, 0.0, 14.9936, + 0.0, 27.8703, 12.9966, + 0.0, 0.0, 1.0] + offset: + pos: [0.0, 0.0, 0.0] + rot: [1.0, 0.0, 0.0, 0.0] +joystick: + on: True + pub_topic: /obelisk/dummy/joy + sub_topic: /obelisk/dummy/joy_feedback diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml index 9694ab2e..f763368b 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml @@ -1,134 +1,132 @@ config: dummy -onboard: - control: - - pkg: obelisk_control_cpp - executable: example_position_setpoint_controller - params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - non_obelisk: False +control: + - pkg: obelisk_control_cpp + executable: example_position_setpoint_controller + params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt + # callback_groups: + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + history_depth: 10 + callback_group: None + non_obelisk: False + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + msg_type: EstimatedState + history_depth: 10 + callback_group: None + non_obelisk: False + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +estimation: + - pkg: obelisk_estimation_cpp + executable: jointencoders_passthrough_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + msg_type: EstimatedState + history_depth: 10 + callback_group: None + non_obelisk: False + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/dummy/joint_encoders + msg_type: JointEncoders + history_depth: 10 + callback_group: None + non_obelisk: False + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + # callback_groups: + params: + height_map_grid_size: [1., 1.] + height_map_grid_spacing: 0.1 + height_map_geom_group: [0] + subscribers: + - ros_parameter: sub_ctrl_setting + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + history_depth: 10 + callback_group: None + non_obelisk: False + sim: + - ros_parameter: mujoco_setting + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: PoseStamped + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + - topic: /obelisk/dummy/odom + dt: 0.005 + msg_type: Odometry + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + tip_velocimeter: velocimeter + tip_gyro_sensor: gyro + - topic: /obelisk/dummy/scan_dots + dt: 0.1 + msg_type: GridCells + sensor_names: + tip_pos_sensor: framepos + viz_geoms: + dt: 1.0 + dummy_box: box + dummy_box_2: box + dummy_sphere: sphere + +viz: + on: True + rviz_pkg: obelisk_ros + rviz_config: basic_obk_config.rviz + viz_nodes: + - pkg: obelisk_viz_cpp + executable: default_robot_viz + robot_pkg: dummy_bot + urdf: dummy_bot.urdf + robot_topic: robot_description subscribers: - - ros_parameter: sub_est_setting + - ros_parameter: sub_viz_est_setting topic: /obelisk/dummy/est - msg_type: EstimatedState history_depth: 10 callback_group: None non_obelisk: False - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - estimation: - - pkg: obelisk_estimation_cpp - executable: jointencoders_passthrough_estimator - # callback_groups: publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/dummy/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - non_obelisk: False - subscribers: - - ros_parameter: sub_sensor_setting - topic: /obelisk/dummy/joint_encoders - msg_type: JointEncoders + - ros_parameter: pub_viz_joint_setting + topic: joint_states history_depth: 10 callback_group: None - non_obelisk: False timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - - is_simulated: True - pkg: obelisk_sim_cpp - executable: obelisk_mujoco_robot - # callback_groups: - params: - height_map_grid_size: [1., 1.] - height_map_grid_spacing: 0.1 - height_map_geom_group: [0] - subscribers: - - ros_parameter: sub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 + - ros_parameter: timer_viz_joint_setting + timer_period_sec: 0.05 callback_group: None - non_obelisk: False - sim: - - ros_parameter: mujoco_setting - num_steps_per_viz: 5 - robot_pkg: dummy_bot - model_xml_path: dummy.xml - sensor_settings: - - topic: /obelisk/dummy/joint_encoders - dt: 0.001 - msg_type: ObkJointEncoders - sensor_names: - joint_pos: jointpos - joint_vel: jointvel - - topic: /obelisk/dummy/imu - dt: 0.002 - msg_type: Imu - sensor_names: - tip_acc_sensor: accelerometer - tip_gyro_sensor: gyro - tip_frame_sensor: framequat - - topic: /obelisk/dummy/framepose - dt: 0.002 - msg_type: PoseStamped - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - - topic: /obelisk/dummy/odom - dt: 0.005 - msg_type: Odometry - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - tip_velocimeter: velocimeter - tip_gyro_sensor: gyro - - topic: /obelisk/dummy/scan_dots - dt: 0.1 - msg_type: GridCells - sensor_names: - tip_pos_sensor: framepos - viz_geoms: - dt: 1.0 - dummy_box: box - dummy_box_2: box - dummy_sphere: sphere - - viz: - on: True - viz_tool: foxglove # rviz - rviz_pkg: obelisk_ros - rviz_config: basic_obk_config.rviz - viz_nodes: - - pkg: obelisk_viz_cpp - executable: default_robot_viz - robot_pkg: dummy_bot - urdf: dummy_bot.urdf - robot_topic: robot_description - subscribers: - - ros_parameter: sub_viz_est_setting - topic: /obelisk/dummy/est - history_depth: 10 - callback_group: None - non_obelisk: False - publishers: - - ros_parameter: pub_viz_joint_setting - topic: joint_states - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_viz_joint_setting - timer_period_sec: 0.05 - callback_group: None diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml index 6a5ff125..9e230bb0 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml @@ -1,106 +1,105 @@ config: dummy -onboard: - control: - - pkg: obelisk_control_cpp - executable: example_position_setpoint_controller - # callback_groups: - params: - test_param: value_configured_in_yaml - params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - key: "asdf" - history_depth: 10 - callback_group: None - non_obelisk: False - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/dummy/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - non_obelisk: False - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - estimation: - - pkg: obelisk_estimation_cpp - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/dummy/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - non_obelisk: False - subscribers: - - ros_parameter: sub_sensor_setting - # key: sub1 - topic: /obelisk/dummy/joint_encoders +control: + - pkg: obelisk_control_cpp + executable: example_position_setpoint_controller + # callback_groups: + params: + test_param: value_configured_in_yaml + params_path: /obelisk_ws/src/obelisk_ros/config/dummy_params.txt + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + key: "asdf" + history_depth: 10 + callback_group: None + non_obelisk: False + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + msg_type: EstimatedState + history_depth: 10 + callback_group: None + non_obelisk: False + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +estimation: + - pkg: obelisk_estimation_cpp + executable: jointencoders_passthrough_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + msg_type: EstimatedState + history_depth: 10 + callback_group: None + non_obelisk: False + subscribers: + - ros_parameter: sub_sensor_setting + # key: sub1 + topic: /obelisk/dummy/joint_encoders + msg_type: ObkJointEncoders + history_depth: 10 + callback_group: None + non_obelisk: False + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +sensing: + - pkg: obelisk_zed_cpp + executable: zed2_sensors + params: + params_path_pkg: obelisk_ros + pub_period: 0.01 + params_path: config/dummy_zed_params.yaml + # callback_groups: + publishers: + - ros_parameter: pub_img_setting + topic: /obelisk/dummy/zed2 + msg_type: ObkImage + history_depth: 10 + callback_group: None + non_obelisk: False +robot: + # `is_simulated` is critical for parsing correct package in the obelisk launch file + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + # callback_groups: + # publishers: + subscribers: + - ros_parameter: sub_ctrl_setting + # key: sub1 + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + history_depth: 10 + callback_group: None + non_obelisk: False + sim: + - ros_parameter: mujoco_setting + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 msg_type: ObkJointEncoders - history_depth: 10 - callback_group: None - non_obelisk: False - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - sensing: - - pkg: obelisk_zed_cpp - executable: zed2_sensors - params: - params_path_pkg: obelisk_ros - pub_period: 0.01 - params_path: config/dummy_zed_params.yaml - # callback_groups: - publishers: - - ros_parameter: pub_img_setting - topic: /obelisk/dummy/zed2 - msg_type: ObkImage - history_depth: 10 - callback_group: None - non_obelisk: False - robot: - # `is_simulated` is critical for parsing correct package in the obelisk launch file - - is_simulated: True - pkg: obelisk_sim_cpp - executable: obelisk_mujoco_robot - # callback_groups: - # publishers: - subscribers: - - ros_parameter: sub_ctrl_setting - # key: sub1 - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - non_obelisk: False - sim: - - ros_parameter: mujoco_setting - num_steps_per_viz: 5 - robot_pkg: dummy_bot - model_xml_path: dummy.xml - sensor_settings: - - topic: /obelisk/dummy/joint_encoders - dt: 0.001 - msg_type: ObkJointEncoders - sensor_names: - joint_pos: jointpos - joint_vel: jointvel - - topic: /obelisk/dummy/imu - dt: 0.002 - msg_type: Imu - sensor_names: - tip_acc_sensor: accelerometer - tip_gyro_sensor: gyro - tip_frame_sensor: framequat - - topic: /obelisk/dummy/framepose - dt: 0.002 - msg_type: ObkFramePose - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: ObkFramePose + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_py.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_py.yaml index 94493b45..86296ab8 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_py.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_py.yaml @@ -1,107 +1,106 @@ config: dummy -onboard: - control: - - pkg: obelisk_control_py - executable: example_position_setpoint_controller - # callback_groups: - params: - test_param: value_configured_in_yaml - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/dummy/ctrl - key: "asdf" - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/dummy/est - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - estimation: - - pkg: obelisk_estimation_py - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/dummy/est - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - # key: sub1 - topic: /obelisk/dummy/joint_encoders - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - - is_simulated: True - pkg: obelisk_sim_cpp - executable: obelisk_mujoco_robot - params: - ic_keyframe: ic - height_map_grid_size: [1., 1.] - height_map_grid_spacing: 0.1 - height_map_geom_group: [0] - # camera_name: "fixed_cam" # You can select a camera to use here - publishers: - - ros_parameter: pub_true_sim_state_setting - topic: /obelisk/dummy/true_sim_state - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_true_sim_state_setting - history_depth: 10 - timer_period_sec: 0.01 - callback_group: None - subscribers: - - ros_parameter: sub_ctrl_setting - topic: /obelisk/dummy/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - num_steps_per_viz: 5 - robot_pkg: dummy_bot - model_xml_path: dummy.xml - sensor_settings: - - topic: /obelisk/dummy/joint_encoders - dt: 0.001 - msg_type: ObkJointEncoders - sensor_names: - joint_pos: jointpos - joint_vel: jointvel - - topic: /obelisk/dummy/imu - dt: 0.002 - msg_type: ObkImu - sensor_names: - tip_acc_sensor: accelerometer - tip_gyro_sensor: gyro - tip_frame_sensor: framequat - - topic: /obelisk/dummy/framepose - dt: 0.002 - msg_type: PoseStamped - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - - topic: /obelisk/dummy/odom - dt: 0.005 - msg_type: Odometry - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat - tip_velocimeter: velocimeter - tip_gyro_sensor: gyro - - topic: /obelisk/dummy/scan_dots - dt: 0.1 - msg_type: GridCells - sensor_names: - tip_pos_sensor: framepos +control: + - pkg: obelisk_control_py + executable: example_position_setpoint_controller + # callback_groups: + params: + test_param: value_configured_in_yaml + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + key: "asdf" + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +estimation: + - pkg: obelisk_estimation_py + executable: jointencoders_passthrough_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + # key: sub1 + topic: /obelisk/dummy/joint_encoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + - is_simulated: True + pkg: obelisk_sim_cpp + executable: obelisk_mujoco_robot + params: + ic_keyframe: ic + height_map_grid_size: [1., 1.] + height_map_grid_spacing: 0.1 + height_map_geom_group: [0] + # camera_name: "fixed_cam" # You can select a camera to use here + publishers: + - ros_parameter: pub_true_sim_state_setting + topic: /obelisk/dummy/true_sim_state + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_true_sim_state_setting + history_depth: 10 + timer_period_sec: 0.01 + callback_group: None + subscribers: + - ros_parameter: sub_ctrl_setting + topic: /obelisk/dummy/ctrl + msg_type: PositionSetpoint + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: PoseStamped + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + - topic: /obelisk/dummy/odom + dt: 0.005 + msg_type: Odometry + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat + tip_velocimeter: velocimeter + tip_gyro_sensor: gyro + - topic: /obelisk/dummy/scan_dots + dt: 0.1 + msg_type: GridCells + sensor_names: + tip_pos_sensor: framepos diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_py_zed.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_py_zed.yaml index aa6c9e15..a51e08e6 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_py_zed.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_py_zed.yaml @@ -1,91 +1,90 @@ config: dummy -onboard: - control: - - pkg: obelisk_control_py - executable: example_position_setpoint_controller - # callback_groups: - params: - test_param: value_configured_in_yaml - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/dummy/ctrl - key: "asdf" - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/dummy/est - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - estimation: - - pkg: obelisk_estimation_py - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/dummy/est - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - # key: sub1 - topic: /obelisk/dummy/joint_encoders - history_depth: 10 - callback_group: None - # TODO(ahl): make a new estimator object that has a zed camera for testing - # - ros_parameter: sub_zed_setting - # topic: /obelisk/dummy/zedmini/img - # msg_type: ObkImage - # history_depth: 1 - # callback_group: sub_cbg - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - # `is_simulated` is critical for parsing correct package in the obelisk launch file - - is_simulated: True - pkg: obelisk_sim_py - executable: obelisk_mujoco_robot - params: - ic_keyframe: other - # callback_groups: - # publishers: - subscribers: - - ros_parameter: sub_ctrl_setting - # key: sub1 - topic: /obelisk/dummy/ctrl - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - n_u: 1 - time_step: 0.002 - num_steps_per_viz: 5 - robot_pkg: dummy_bot - model_xml_path: dummy.xml - sensor_settings: - - topic: /obelisk/dummy/joint_encoders - dt: 0.001 - msg_type: ObkJointEncoders - sensor_names: - joint_pos: jointpos - joint_vel: jointvel - - topic: /obelisk/dummy/imu - dt: 0.002 - msg_type: ObkImu - sensor_names: - tip_acc_sensor: accelerometer - tip_gyro_sensor: gyro - tip_frame_sensor: framequat - - topic: /obelisk/dummy/framepose - dt: 0.002 - msg_type: ObkFramePose - sensor_names: - tip_pos_sensor: framepos - tip_orientation_sensor: framequat +control: + - pkg: obelisk_control_py + executable: example_position_setpoint_controller + # callback_groups: + params: + test_param: value_configured_in_yaml + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/dummy/ctrl + key: "asdf" + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +estimation: + - pkg: obelisk_estimation_py + executable: jointencoders_passthrough_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/dummy/est + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + # key: sub1 + topic: /obelisk/dummy/joint_encoders + history_depth: 10 + callback_group: None + # TODO(ahl): make a new estimator object that has a zed camera for testing + # - ros_parameter: sub_zed_setting + # topic: /obelisk/dummy/zedmini/img + # msg_type: ObkImage + # history_depth: 1 + # callback_group: sub_cbg + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + # `is_simulated` is critical for parsing correct package in the obelisk launch file + - is_simulated: True + pkg: obelisk_sim_py + executable: obelisk_mujoco_robot + params: + ic_keyframe: other + # callback_groups: + # publishers: + subscribers: + - ros_parameter: sub_ctrl_setting + # key: sub1 + topic: /obelisk/dummy/ctrl + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + n_u: 1 + time_step: 0.002 + num_steps_per_viz: 5 + robot_pkg: dummy_bot + model_xml_path: dummy.xml + sensor_settings: + - topic: /obelisk/dummy/joint_encoders + dt: 0.001 + msg_type: ObkJointEncoders + sensor_names: + joint_pos: jointpos + joint_vel: jointvel + - topic: /obelisk/dummy/imu + dt: 0.002 + msg_type: Imu + sensor_names: + tip_acc_sensor: accelerometer + tip_gyro_sensor: gyro + tip_frame_sensor: framequat + - topic: /obelisk/dummy/framepose + dt: 0.002 + msg_type: ObkFramePose + sensor_names: + tip_pos_sensor: framepos + tip_orientation_sensor: framequat diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index b6d9658e..07bf8aca 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -1,287 +1,285 @@ config: g1_example -onboard: - control: - - pkg: obelisk_unitree_cpp - params: - robot_str: G1 - executable: unitree_example_controller - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/g1/ctrl - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/g1/est_state - history_depth: 10 - callback_group: None - # ----- Joystick subscriber ----- # - - ros_parameter: joystick_sub_setting - topic: /obelisk/g1/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - # ----- High Level/Execution FSM Controller ----- # - - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_joystick - # callback_groups: - publishers: - # ----- Execution FSM ----- # - - ros_parameter: pub_exec_fsm_setting - topic: /obelisk/g1/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: pub_ctrl_setting - topic: /obelisk/g1/vel_cmd - history_depth: 10 - callback_group: None - # ----- Joystick Passthrough ----- # - - ros_parameter: pub_joy_passthrough_setting - topic: /obelisk/g1/joy_passthrough - history_depth: 10 - callback_group: None - subscribers: +control: + - pkg: obelisk_unitree_cpp + params: + robot_str: G1 + executable: unitree_example_controller + # callback_groups: + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/g1/ctrl + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/g1/est_state + history_depth: 10 + callback_group: None # ----- Joystick subscriber ----- # - - ros_parameter: sub_est_setting - topic: /obelisk/g1/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.1 - callback_group: None - estimation: - - pkg: obelisk_unitree_cpp - executable: unitree_example_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/g1/est_state - msg_type: EstimatedState - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - topic: /obelisk/g1/joint_encoders + - ros_parameter: joystick_sub_setting + topic: /obelisk/g1/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +# ----- High Level/Execution FSM Controller ----- # + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + # callback_groups: + publishers: + # ----- Execution FSM ----- # + - ros_parameter: pub_exec_fsm_setting + topic: /obelisk/g1/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: pub_ctrl_setting + topic: /obelisk/g1/vel_cmd + history_depth: 10 + callback_group: None + # ----- Joystick Passthrough ----- # + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/g1/joy_passthrough + history_depth: 10 + callback_group: None + subscribers: + # ----- Joystick subscriber ----- # + - ros_parameter: sub_est_setting + topic: /obelisk/g1/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.1 + callback_group: None +estimation: + - pkg: obelisk_unitree_cpp + executable: unitree_example_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/g1/est_state + msg_type: EstimatedState + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/g1/joint_encoders + msg_type: ObkJointEncoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + # === simulation === + # - is_simulated: True + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_sim + # params: + # ic_keyframe: standing + # === hardware === + - is_simulated: False + pkg: obelisk_unitree_cpp + executable: obelisk_unitree_g1_hardware + params: + # network_interface_name: enx6c1ff724e92e + default_joint_names: [ + "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", + "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", + "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", + "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", + "waist_yaw_joint", + "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", + "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", + "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", + "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", + ] + user_pose: [ + -0.25, 0, 0, + 0.46, -0.25, 0, + -0.25, 0, 0, + 0.46, -0.25, 0, + 0, + 0.07, 0.24, 0, 1.39, + 0, 0, 0, + 0.07, -0.24, 0, 1.39, + 0, 0, 0 + ] + default_kp: [ + 100., 100., 100., # L hip + 150., 100., 100., # L knee, ankle + 100., 100., 100., # R hip + 150., 100., 100., # R knee, ankle + 100., # Waist + 50., 50., 40., 40., # L shoulder, elbow + 30., 30., 30., # L wrist + 50., 50., 40., 40., # R shoulder, elbow + 30., 30., 30., # R wrist + ] + default_kd: [ + 2., 2., 2., # L hip + 4., 2., 2., # L knee, ankle + 2., 2., 2., # R hip + 4., 2., 2., # R knee, ankle + 2., # Waist + 2., 2., 2., 2., # L shoulder, elbow + 2., 2., 2., # L wrist + 2., 2., 2., 2., # R shoulder, elbow + 2., 2., 2., # R wrist + ] + default_kd_damping: [ + 10., 10., 10., # L hip + 10., 10., 10., # L knee, ankle + 10., 10., 10., # R hip + 10., 10., 10., # R knee, ankle + 10., # Waist + 10., 10., 10., 10., # L shoulder, elbow + 10., 10., 10., # L wrist + 10., 10., 10., 10., # R shoulder, elbow + 10., 10., 10., # R wrist + ] + # ================== + # callback_groups: + publishers: + # ----- Joints ----- # + - ros_parameter: pub_sensor_setting + topic: /obelisk/g1/joint_encoders + history_depth: 10 + # ----- IMU ----- # + - ros_parameter: pub_imu_setting + topic: /obelisk/g1/pelvis_imu + history_depth: 10 + # ----- Odom ----- # + - ros_parameter: pub_odom_setting + topic: /obelisk/g1/odom + history_depth: 10 + subscribers: + # ----- Control ----- # + - ros_parameter: sub_ctrl_setting + topic: /obelisk/g1/ctrl + history_depth: 10 + callback_group: None + # ----- Execution FSM ----- # + - ros_parameter: sub_fsm_setting + topic: /obelisk/g1/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: sub_vel_cmd_setting + topic: /obelisk/g1/vel_cmd + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + n_u: 16 + time_step: 0.002 + num_steps_per_viz: 5 + robot_pkg: g1_description + # model_xml_path: g1_27dof_fixed_base.xml + # model_xml_path: g1_27dof.xml + model_xml_path: g1_basic_scene.xml + sensor_settings: + - topic: /obelisk/g1/joint_encoders + dt: 0.002 msg_type: ObkJointEncoders - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - # === simulation === - # - is_simulated: True - # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_sim - # params: - # ic_keyframe: standing - # === hardware === - - is_simulated: False - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_g1_hardware - params: - # network_interface_name: enx6c1ff724e92e - default_joint_names: [ - "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", - "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", - "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", - "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", - "waist_yaw_joint", - "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", - "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", - "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", - ] - user_pose: [ - -0.25, 0, 0, - 0.46, -0.25, 0, - -0.25, 0, 0, - 0.46, -0.25, 0, - 0, - 0.07, 0.24, 0, 1.39, - 0, 0, 0, - 0.07, -0.24, 0, 1.39, - 0, 0, 0 - ] - default_kp: [ - 100., 100., 100., # L hip - 150., 100., 100., # L knee, ankle - 100., 100., 100., # R hip - 150., 100., 100., # R knee, ankle - 100., # Waist - 50., 50., 40., 40., # L shoulder, elbow - 30., 30., 30., # L wrist - 50., 50., 40., 40., # R shoulder, elbow - 30., 30., 30., # R wrist - ] - default_kd: [ - 2., 2., 2., # L hip - 4., 2., 2., # L knee, ankle - 2., 2., 2., # R hip - 4., 2., 2., # R knee, ankle - 2., # Waist - 2., 2., 2., 2., # L shoulder, elbow - 2., 2., 2., # L wrist - 2., 2., 2., 2., # R shoulder, elbow - 2., 2., 2., # R wrist - ] - default_kd_damping: [ - 10., 10., 10., # L hip - 10., 10., 10., # L knee, ankle - 10., 10., 10., # R hip - 10., 10., 10., # R knee, ankle - 10., # Waist - 10., 10., 10., 10., # L shoulder, elbow - 10., 10., 10., # L wrist - 10., 10., 10., 10., # R shoulder, elbow - 10., 10., 10., # R wrist - ] - # ================== - # callback_groups: - publishers: - # ----- Joints ----- # - - ros_parameter: pub_sensor_setting - topic: /obelisk/g1/joint_encoders - history_depth: 10 - # ----- IMU ----- # - - ros_parameter: pub_imu_setting - topic: /obelisk/g1/pelvis_imu - history_depth: 10 - # ----- Odom ----- # - - ros_parameter: pub_odom_setting - topic: /obelisk/g1/odom - history_depth: 10 - subscribers: - # ----- Control ----- # - - ros_parameter: sub_ctrl_setting - topic: /obelisk/g1/ctrl - history_depth: 10 - callback_group: None - # ----- Execution FSM ----- # - - ros_parameter: sub_fsm_setting - topic: /obelisk/g1/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: sub_vel_cmd_setting - topic: /obelisk/g1/vel_cmd - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - n_u: 16 - time_step: 0.002 - num_steps_per_viz: 5 - robot_pkg: g1_description - # model_xml_path: g1_27dof_fixed_base.xml - # model_xml_path: g1_27dof.xml - model_xml_path: g1_basic_scene.xml - sensor_settings: - - topic: /obelisk/g1/joint_encoders - dt: 0.002 - msg_type: ObkJointEncoders - sensor_names: - # ---------- Joint Positions ---------- # - left_hip_pitch_joint_pos_sensor: jointpos - left_hip_roll_joint_pos_sensor: jointpos - left_hip_yaw_joint_pos_sensor: jointpos - left_knee_joint_pos_sensor: jointpos - left_ankle_pitch_joint_pos_sensor: jointpos - left_ankle_roll_joint_pos_sensor: jointpos + sensor_names: + # ---------- Joint Positions ---------- # + left_hip_pitch_joint_pos_sensor: jointpos + left_hip_roll_joint_pos_sensor: jointpos + left_hip_yaw_joint_pos_sensor: jointpos + left_knee_joint_pos_sensor: jointpos + left_ankle_pitch_joint_pos_sensor: jointpos + left_ankle_roll_joint_pos_sensor: jointpos - right_hip_pitch_joint_pos_sensor: jointpos - right_hip_roll_joint_pos_sensor: jointpos - right_hip_yaw_joint_pos_sensor: jointpos - right_knee_joint_pos_sensor: jointpos - right_ankle_pitch_joint_pos_sensor: jointpos - right_ankle_roll_joint_pos_sensor: jointpos + right_hip_pitch_joint_pos_sensor: jointpos + right_hip_roll_joint_pos_sensor: jointpos + right_hip_yaw_joint_pos_sensor: jointpos + right_knee_joint_pos_sensor: jointpos + right_ankle_pitch_joint_pos_sensor: jointpos + right_ankle_roll_joint_pos_sensor: jointpos - waist_yaw_joint_pos_sensor: jointpos + waist_yaw_joint_pos_sensor: jointpos - left_shoulder_pitch_joint_pos_sensor: jointpos - left_shoulder_roll_joint_pos_sensor: jointpos - left_shoulder_yaw_joint_pos_sensor: jointpos - left_elbow_joint_pos_sensor: jointpos - left_wrist_roll_joint_pos_sensor: jointpos - left_wrist_pitch_joint_pos_sensor: jointpos - left_wrist_yaw_joint_pos_sensor: jointpos + left_shoulder_pitch_joint_pos_sensor: jointpos + left_shoulder_roll_joint_pos_sensor: jointpos + left_shoulder_yaw_joint_pos_sensor: jointpos + left_elbow_joint_pos_sensor: jointpos + left_wrist_roll_joint_pos_sensor: jointpos + left_wrist_pitch_joint_pos_sensor: jointpos + left_wrist_yaw_joint_pos_sensor: jointpos - right_shoulder_pitch_joint_pos_sensor: jointpos - right_shoulder_roll_joint_pos_sensor: jointpos - right_shoulder_yaw_joint_pos_sensor: jointpos - right_elbow_joint_pos_sensor: jointpos - right_wrist_roll_joint_pos_sensor: jointpos - right_wrist_pitch_joint_pos_sensor: jointpos - right_wrist_yaw_joint_pos_sensor: jointpos + right_shoulder_pitch_joint_pos_sensor: jointpos + right_shoulder_roll_joint_pos_sensor: jointpos + right_shoulder_yaw_joint_pos_sensor: jointpos + right_elbow_joint_pos_sensor: jointpos + right_wrist_roll_joint_pos_sensor: jointpos + right_wrist_pitch_joint_pos_sensor: jointpos + right_wrist_yaw_joint_pos_sensor: jointpos - # ---------- Joint Velocities ---------- # - left_hip_pitch_joint_vel_sensor: jointvel - left_hip_roll_joint_vel_sensor: jointvel - left_hip_yaw_joint_vel_sensor: jointvel - left_knee_joint_vel_sensor: jointvel - left_ankle_pitch_joint_vel_sensor: jointvel - left_ankle_roll_joint_vel_sensor: jointvel + # ---------- Joint Velocities ---------- # + left_hip_pitch_joint_vel_sensor: jointvel + left_hip_roll_joint_vel_sensor: jointvel + left_hip_yaw_joint_vel_sensor: jointvel + left_knee_joint_vel_sensor: jointvel + left_ankle_pitch_joint_vel_sensor: jointvel + left_ankle_roll_joint_vel_sensor: jointvel - right_hip_pitch_joint_vel_sensor: jointvel - right_hip_roll_joint_vel_sensor: jointvel - right_hip_yaw_joint_vel_sensor: jointvel - right_knee_joint_vel_sensor: jointvel - right_ankle_pitch_joint_vel_sensor: jointvel - right_ankle_roll_joint_vel_sensor: jointvel + right_hip_pitch_joint_vel_sensor: jointvel + right_hip_roll_joint_vel_sensor: jointvel + right_hip_yaw_joint_vel_sensor: jointvel + right_knee_joint_vel_sensor: jointvel + right_ankle_pitch_joint_vel_sensor: jointvel + right_ankle_roll_joint_vel_sensor: jointvel - waist_yaw_joint_vel_sensor: jointvel + waist_yaw_joint_vel_sensor: jointvel - left_shoulder_pitch_joint_vel_sensor: jointvel - left_shoulder_roll_joint_vel_sensor: jointvel - left_shoulder_yaw_joint_vel_sensor: jointvel - left_elbow_joint_vel_sensor: jointvel - left_wrist_roll_joint_vel_sensor: jointvel - left_wrist_pitch_joint_vel_sensor: jointvel - left_wrist_yaw_joint_vel_sensor: jointvel + left_shoulder_pitch_joint_vel_sensor: jointvel + left_shoulder_roll_joint_vel_sensor: jointvel + left_shoulder_yaw_joint_vel_sensor: jointvel + left_elbow_joint_vel_sensor: jointvel + left_wrist_roll_joint_vel_sensor: jointvel + left_wrist_pitch_joint_vel_sensor: jointvel + left_wrist_yaw_joint_vel_sensor: jointvel - right_shoulder_pitch_joint_vel_sensor: jointvel - right_shoulder_roll_joint_vel_sensor: jointvel - right_shoulder_yaw_joint_vel_sensor: jointvel - right_elbow_joint_vel_sensor: jointvel - right_wrist_roll_joint_vel_sensor: jointvel - right_wrist_pitch_joint_vel_sensor: jointvel - right_wrist_yaw_joint_vel_sensor: jointvel - timers: - - ros_parameter: timer_sensor_setting - timer_period_sec: 0.02 + right_shoulder_pitch_joint_vel_sensor: jointvel + right_shoulder_roll_joint_vel_sensor: jointvel + right_shoulder_yaw_joint_vel_sensor: jointvel + right_elbow_joint_vel_sensor: jointvel + right_wrist_roll_joint_vel_sensor: jointvel + right_wrist_pitch_joint_vel_sensor: jointvel + right_wrist_yaw_joint_vel_sensor: jointvel + timers: + - ros_parameter: timer_sensor_setting + timer_period_sec: 0.02 + callback_group: None + - ros_parameter: timer_logging_setting + timer_period_sec: 0.2 + callback_group: None +viz: + on: True + viz_nodes: + - pkg: obelisk_viz_cpp + executable: default_robot_viz + robot_pkg: g1_description + urdf: g1.urdf + robot_topic: robot_description + subscribers: + - ros_parameter: sub_viz_est_setting + topic: /obelisk/g1/est_state + history_depth: 10 + callback_group: None + non_obelisk: False + publishers: + - ros_parameter: pub_viz_joint_setting + topic: joint_states + history_depth: 10 callback_group: None - - ros_parameter: timer_logging_setting - timer_period_sec: 0.2 + timers: + - ros_parameter: timer_viz_joint_setting + timer_period_sec: 0.01 callback_group: None - viz: - on: True - viz_tool: foxglove - viz_nodes: - - pkg: obelisk_viz_cpp - executable: default_robot_viz - robot_pkg: g1_description - urdf: g1.urdf - robot_topic: robot_description - subscribers: - - ros_parameter: sub_viz_est_setting - topic: /obelisk/g1/est_state - history_depth: 10 - callback_group: None - non_obelisk: False - publishers: - - ros_parameter: pub_viz_joint_setting - topic: joint_states - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_viz_joint_setting - timer_period_sec: 0.01 - callback_group: None - joystick: - on: True - pub_topic: /obelisk/g1/joy - sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file +joystick: + on: True + pub_topic: /obelisk/g1/joy + sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml index 45bd99f6..d4b7ad8d 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -1,319 +1,351 @@ config: g1_example -onboard: - control: - - pkg: obelisk_unitree_cpp - params: - robot_str: G1 - executable: unitree_example_controller - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/g1/ctrl - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/g1/est_state - history_depth: 10 - callback_group: None - # ----- Joystick subscriber ----- # - - ros_parameter: joystick_sub_setting - topic: /obelisk/g1/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - # ----- High Level/Execution FSM Controller ----- # - - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_joystick - # callback_groups: - publishers: - # ----- Execution FSM ----- # - - ros_parameter: pub_exec_fsm_setting - topic: /obelisk/g1/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: pub_ctrl_setting - topic: /obelisk/g1/vel_cmd - history_depth: 10 - callback_group: None - # ----- Joystick Passthrough ----- # - - ros_parameter: pub_joy_passthrough_setting - topic: /obelisk/g1/joy_passthrough - history_depth: 10 - callback_group: None - subscribers: +control: + - pkg: obelisk_unitree_cpp + params: + robot_str: G1 + executable: unitree_example_controller + # callback_groups: + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/g1/ctrl + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/g1/est_state + history_depth: 10 + callback_group: None # ----- Joystick subscriber ----- # - - ros_parameter: sub_est_setting - topic: /obelisk/g1/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.1 - callback_group: None - estimation: - - pkg: obelisk_unitree_cpp - executable: unitree_example_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/g1/est_state - msg_type: EstimatedState - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - topic: /obelisk/g1/joint_encoders + - ros_parameter: joystick_sub_setting + topic: /obelisk/g1/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +# ----- High Level/Execution FSM Controller ----- # + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + # callback_groups: + publishers: + # ----- Execution FSM ----- # + - ros_parameter: pub_exec_fsm_setting + topic: /obelisk/g1/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: pub_ctrl_setting + topic: /obelisk/g1/vel_cmd + history_depth: 10 + callback_group: None + # ----- Joystick Passthrough ----- # + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/g1/joy_passthrough + history_depth: 10 + callback_group: None + subscribers: + # ----- Joystick subscriber ----- # + - ros_parameter: sub_est_setting + topic: /obelisk/g1/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.1 + callback_group: None +estimation: + - pkg: obelisk_unitree_cpp + executable: unitree_example_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/g1/est_state + msg_type: EstimatedState + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/g1/joint_encoders + msg_type: ObkJointEncoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + # === simulation === + - is_simulated: True + pkg: obelisk_unitree_cpp + executable: obelisk_unitree_sim + params: + ic_keyframe: standing + # === hardware === + # - is_simulated: False + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_g1_hardware + # params: + # # network_interface_name: enx6c1ff724e92e + # default_joint_names: [ + # "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", + # "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", + # "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", + # "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", + # "waist_yaw_joint", + # "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", + # "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", + # "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", + # "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", + # ] + # user_pose: [ + # -0.25, 0, 0, + # 0.46, -0.25, 0, + # -0.25, 0, 0, + # 0.46, -0.25, 0, + # 0, + # 0.07, 0.24, 0, 1.39, + # 0, 0, 0, + # 0.07, -0.24, 0, 1.39, + # 0, 0, 0 + # ] + # default_kp: [ + # 100., 100., 100., # L hip + # 150., 100., 100., # L knee, ankle + # 100., 100., 100., # R hip + # 150., 100., 100., # R knee, ankle + # 100., # Waist + # 50., 50., 40., 40., # L shoulder, elbow + # 30., 30., 30., # L wrist + # 50., 50., 40., 40., # R shoulder, elbow + # 30., 30., 30., # R wrist + # ] + # default_kd: [ + # 2., 2., 2., # L hip + # 4., 2., 2., # L knee, ankle + # 2., 2., 2., # R hip + # 4., 2., 2., # R knee, ankle + # 2., # Waist + # 2., 2., 2., 2., # L shoulder, elbow + # 2., 2., 2., # L wrist + # 2., 2., 2., 2., # R shoulder, elbow + # 2., 2., 2., # R wrist + # ] + # default_kd_damping: [ + # 10., 10., 10., # L hip + # 10., 10., 10., # L knee, ankle + # 10., 10., 10., # R hip + # 10., 10., 10., # R knee, ankle + # 10., # Waist + # 10., 10., 10., 10., # L shoulder, elbow + # 10., 10., 10., # L wrist + # 10., 10., 10., 10., # R shoulder, elbow + # 10., 10., 10., # R wrist + # ] + # ================== + # callback_groups: + publishers: + # ----- Joints ----- # + - ros_parameter: pub_sensor_setting + topic: /obelisk/g1/joint_encoders + history_depth: 10 + # ----- IMU ----- # + - ros_parameter: pub_imu_setting + topic: /obelisk/g1/pelvis_imu + history_depth: 10 + # ----- Odom ----- # + - ros_parameter: pub_odom_setting + topic: /obelisk/g1/odom + history_depth: 10 + subscribers: + # ----- Control ----- # + - ros_parameter: sub_ctrl_setting + topic: /obelisk/g1/ctrl + history_depth: 10 + callback_group: None + # ----- Execution FSM ----- # + - ros_parameter: sub_fsm_setting + topic: /obelisk/g1/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: sub_vel_cmd_setting + topic: /obelisk/g1/vel_cmd + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + n_u: 16 + time_step: 0.002 + num_steps_per_viz: 5 + robot_pkg: g1_description + # model_xml_path: g1_27dof_fixed_base.xml + # model_xml_path: g1_27dof.xml + model_xml_path: g1_basic_scene.xml + sensor_settings: + # - topic: /obelisk/g1/raycast + # dt: 0.1 + # msg_type: ObkScan + # sensor_names: + # scan_pos_sensor: framepos + # scan_config: + # site_name: pelvis_mocap_site + # geom_groups: [2] + # pattern: + # type: height_scan + # ordering: xy + # resolution: 0.1 + # size: [1.6, 0.8] + # offset: + # pos: [0.08, 0.0, 20.0] + - topic: /cloud_registered_body + dt: 0.1 + msg_type: PointCloud2 + sensor_names: + scan_pos_sensor: framepos + scan_config: + site_name: head_lidar_site + geom_groups: [2] + viz_decimation: 10 + frame: body + pattern: + type: lidar_scan + vertical_fov_range: [0.0, 51.0] + horizontal_fov_range: [0.0, 360.0] + channels: 51 + horizontal_res: 1.0 + offset: + pos: [0.0, 0.0, 0.0] + rot: [1.0, 0.0, 0.0, 0.0] + # - topic: /depth/image_processed + # dt: 0.033 + # msg_type: DepthImage + # sensor_names: + # scan_pos_sensor: framepos + # scan_config: + # site_name: pelvis_mocap_site + # geom_groups: [0, 1, 2] + # viz_decimation: 1 + # frame: world + # pattern: + # type: depth_camera + # width: 30 + # height: 26 + # intrinsic_matrix: [25.9928, 0.0, 14.9936, 0.0, 27.8703, 12.9966, 0.0, 0.0, 1.0] + # offset: + # pos: [0.153246, 0.0, 0.106799] + # rot: [0.843391, 0.0, 0.537300, 0.0] + - topic: /obelisk/g1/joint_encoders + dt: 0.002 msg_type: ObkJointEncoders - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - # === simulation === - - is_simulated: True - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_sim - params: - ic_keyframe: standing - # === hardware === - # - is_simulated: False - # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_g1_hardware - # params: - # # network_interface_name: enx6c1ff724e92e - # default_joint_names: [ - # "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", - # "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", - # "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", - # "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", - # "waist_yaw_joint", - # "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", - # "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", - # "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", - # "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", - # ] - # user_pose: [ - # -0.25, 0, 0, - # 0.46, -0.25, 0, - # -0.25, 0, 0, - # 0.46, -0.25, 0, - # 0, - # 0.07, 0.24, 0, 1.39, - # 0, 0, 0, - # 0.07, -0.24, 0, 1.39, - # 0, 0, 0 - # ] - # default_kp: [ - # 100., 100., 100., # L hip - # 150., 100., 100., # L knee, ankle - # 100., 100., 100., # R hip - # 150., 100., 100., # R knee, ankle - # 100., # Waist - # 50., 50., 40., 40., # L shoulder, elbow - # 30., 30., 30., # L wrist - # 50., 50., 40., 40., # R shoulder, elbow - # 30., 30., 30., # R wrist - # ] - # default_kd: [ - # 2., 2., 2., # L hip - # 4., 2., 2., # L knee, ankle - # 2., 2., 2., # R hip - # 4., 2., 2., # R knee, ankle - # 2., # Waist - # 2., 2., 2., 2., # L shoulder, elbow - # 2., 2., 2., # L wrist - # 2., 2., 2., 2., # R shoulder, elbow - # 2., 2., 2., # R wrist - # ] - # default_kd_damping: [ - # 10., 10., 10., # L hip - # 10., 10., 10., # L knee, ankle - # 10., 10., 10., # R hip - # 10., 10., 10., # R knee, ankle - # 10., # Waist - # 10., 10., 10., 10., # L shoulder, elbow - # 10., 10., 10., # L wrist - # 10., 10., 10., 10., # R shoulder, elbow - # 10., 10., 10., # R wrist - # ] - # ================== - # callback_groups: - publishers: - # ----- Joints ----- # - - ros_parameter: pub_sensor_setting - topic: /obelisk/g1/joint_encoders - history_depth: 10 - # ----- IMU ----- # - - ros_parameter: pub_imu_setting - topic: /obelisk/g1/pelvis_imu - history_depth: 10 - # ----- Odom ----- # - - ros_parameter: pub_odom_setting - topic: /obelisk/g1/odom - history_depth: 10 - subscribers: - # ----- Control ----- # - - ros_parameter: sub_ctrl_setting - topic: /obelisk/g1/ctrl - history_depth: 10 - callback_group: None - # ----- Execution FSM ----- # - - ros_parameter: sub_fsm_setting - topic: /obelisk/g1/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: sub_vel_cmd_setting - topic: /obelisk/g1/vel_cmd - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - n_u: 16 - time_step: 0.002 - num_steps_per_viz: 5 - robot_pkg: g1_description - # model_xml_path: g1_27dof_fixed_base.xml - # model_xml_path: g1_27dof.xml - model_xml_path: g1_basic_scene.xml - sensor_settings: - # - topic: /obelisk/g1/raycast - # dt: 0.1 - # msg_type: ObkScan - # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml - # sensor_names: - # scan_pos_sensor: framepos - - topic: /cloud_registered_body - dt: 0.1 - msg_type: PointCloud2 - config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml - sensor_names: - scan_pos_sensor: framepos - # - topic: /depth/image_processed - # dt: 0.033 - # msg_type: DepthImage - # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/depth_config.yaml - # sensor_names: - # scan_pos_sensor: framepos - - topic: /obelisk/g1/joint_encoders - dt: 0.002 - msg_type: ObkJointEncoders - sensor_names: - # ---------- Joint Positions ---------- # - left_hip_pitch_joint_pos_sensor: jointpos - left_hip_roll_joint_pos_sensor: jointpos - left_hip_yaw_joint_pos_sensor: jointpos - left_knee_joint_pos_sensor: jointpos - left_ankle_pitch_joint_pos_sensor: jointpos - left_ankle_roll_joint_pos_sensor: jointpos + sensor_names: + # ---------- Joint Positions ---------- # + left_hip_pitch_joint_pos_sensor: jointpos + left_hip_roll_joint_pos_sensor: jointpos + left_hip_yaw_joint_pos_sensor: jointpos + left_knee_joint_pos_sensor: jointpos + left_ankle_pitch_joint_pos_sensor: jointpos + left_ankle_roll_joint_pos_sensor: jointpos - right_hip_pitch_joint_pos_sensor: jointpos - right_hip_roll_joint_pos_sensor: jointpos - right_hip_yaw_joint_pos_sensor: jointpos - right_knee_joint_pos_sensor: jointpos - right_ankle_pitch_joint_pos_sensor: jointpos - right_ankle_roll_joint_pos_sensor: jointpos + right_hip_pitch_joint_pos_sensor: jointpos + right_hip_roll_joint_pos_sensor: jointpos + right_hip_yaw_joint_pos_sensor: jointpos + right_knee_joint_pos_sensor: jointpos + right_ankle_pitch_joint_pos_sensor: jointpos + right_ankle_roll_joint_pos_sensor: jointpos - waist_yaw_joint_pos_sensor: jointpos + waist_yaw_joint_pos_sensor: jointpos - left_shoulder_pitch_joint_pos_sensor: jointpos - left_shoulder_roll_joint_pos_sensor: jointpos - left_shoulder_yaw_joint_pos_sensor: jointpos - left_elbow_joint_pos_sensor: jointpos - left_wrist_roll_joint_pos_sensor: jointpos - left_wrist_pitch_joint_pos_sensor: jointpos - left_wrist_yaw_joint_pos_sensor: jointpos + left_shoulder_pitch_joint_pos_sensor: jointpos + left_shoulder_roll_joint_pos_sensor: jointpos + left_shoulder_yaw_joint_pos_sensor: jointpos + left_elbow_joint_pos_sensor: jointpos + left_wrist_roll_joint_pos_sensor: jointpos + left_wrist_pitch_joint_pos_sensor: jointpos + left_wrist_yaw_joint_pos_sensor: jointpos - right_shoulder_pitch_joint_pos_sensor: jointpos - right_shoulder_roll_joint_pos_sensor: jointpos - right_shoulder_yaw_joint_pos_sensor: jointpos - right_elbow_joint_pos_sensor: jointpos - right_wrist_roll_joint_pos_sensor: jointpos - right_wrist_pitch_joint_pos_sensor: jointpos - right_wrist_yaw_joint_pos_sensor: jointpos + right_shoulder_pitch_joint_pos_sensor: jointpos + right_shoulder_roll_joint_pos_sensor: jointpos + right_shoulder_yaw_joint_pos_sensor: jointpos + right_elbow_joint_pos_sensor: jointpos + right_wrist_roll_joint_pos_sensor: jointpos + right_wrist_pitch_joint_pos_sensor: jointpos + right_wrist_yaw_joint_pos_sensor: jointpos - # ---------- Joint Velocities ---------- # - left_hip_pitch_joint_vel_sensor: jointvel - left_hip_roll_joint_vel_sensor: jointvel - left_hip_yaw_joint_vel_sensor: jointvel - left_knee_joint_vel_sensor: jointvel - left_ankle_pitch_joint_vel_sensor: jointvel - left_ankle_roll_joint_vel_sensor: jointvel + # ---------- Joint Velocities ---------- # + left_hip_pitch_joint_vel_sensor: jointvel + left_hip_roll_joint_vel_sensor: jointvel + left_hip_yaw_joint_vel_sensor: jointvel + left_knee_joint_vel_sensor: jointvel + left_ankle_pitch_joint_vel_sensor: jointvel + left_ankle_roll_joint_vel_sensor: jointvel - right_hip_pitch_joint_vel_sensor: jointvel - right_hip_roll_joint_vel_sensor: jointvel - right_hip_yaw_joint_vel_sensor: jointvel - right_knee_joint_vel_sensor: jointvel - right_ankle_pitch_joint_vel_sensor: jointvel - right_ankle_roll_joint_vel_sensor: jointvel + right_hip_pitch_joint_vel_sensor: jointvel + right_hip_roll_joint_vel_sensor: jointvel + right_hip_yaw_joint_vel_sensor: jointvel + right_knee_joint_vel_sensor: jointvel + right_ankle_pitch_joint_vel_sensor: jointvel + right_ankle_roll_joint_vel_sensor: jointvel - waist_yaw_joint_vel_sensor: jointvel + waist_yaw_joint_vel_sensor: jointvel - left_shoulder_pitch_joint_vel_sensor: jointvel - left_shoulder_roll_joint_vel_sensor: jointvel - left_shoulder_yaw_joint_vel_sensor: jointvel - left_elbow_joint_vel_sensor: jointvel - left_wrist_roll_joint_vel_sensor: jointvel - left_wrist_pitch_joint_vel_sensor: jointvel - left_wrist_yaw_joint_vel_sensor: jointvel + left_shoulder_pitch_joint_vel_sensor: jointvel + left_shoulder_roll_joint_vel_sensor: jointvel + left_shoulder_yaw_joint_vel_sensor: jointvel + left_elbow_joint_vel_sensor: jointvel + left_wrist_roll_joint_vel_sensor: jointvel + left_wrist_pitch_joint_vel_sensor: jointvel + left_wrist_yaw_joint_vel_sensor: jointvel - right_shoulder_pitch_joint_vel_sensor: jointvel - right_shoulder_roll_joint_vel_sensor: jointvel - right_shoulder_yaw_joint_vel_sensor: jointvel - right_elbow_joint_vel_sensor: jointvel - right_wrist_roll_joint_vel_sensor: jointvel - right_wrist_pitch_joint_vel_sensor: jointvel - right_wrist_yaw_joint_vel_sensor: jointvel - # ---------- Fast LIO ----------- # - - topic: /livox/imu - dt: 0.005 - msg_type: Imu - sensor_names: - lidar_imu_acc_sensor: accelerometer - lidar_imu_gyro_sensor: gyro - lidar_imu_quat_sensor: framequat - - topic: /Odometry - dt: 0.1 - msg_type: Odometry - sensor_names: - head_lidar_pos_sensor: framepos - head_lidar_orientation_sensor: framequat - timers: - - ros_parameter: timer_sensor_setting - timer_period_sec: 0.02 + right_shoulder_pitch_joint_vel_sensor: jointvel + right_shoulder_roll_joint_vel_sensor: jointvel + right_shoulder_yaw_joint_vel_sensor: jointvel + right_elbow_joint_vel_sensor: jointvel + right_wrist_roll_joint_vel_sensor: jointvel + right_wrist_pitch_joint_vel_sensor: jointvel + right_wrist_yaw_joint_vel_sensor: jointvel + # ---------- Fast LIO ----------- # + - topic: /livox/imu + dt: 0.005 + msg_type: Imu + sensor_names: + lidar_imu_acc_sensor: accelerometer + lidar_imu_gyro_sensor: gyro + lidar_imu_quat_sensor: framequat + - topic: /Odometry + dt: 0.1 + msg_type: Odometry + sensor_names: + head_lidar_pos_sensor: framepos + head_lidar_orientation_sensor: framequat + timers: + - ros_parameter: timer_sensor_setting + timer_period_sec: 0.02 + callback_group: None + - ros_parameter: timer_logging_setting + timer_period_sec: 0.2 + callback_group: None +viz: + on: True + viz_nodes: + - pkg: obelisk_viz_cpp + executable: default_robot_viz + robot_pkg: g1_description + urdf: g1.urdf + robot_topic: robot_description + subscribers: + - ros_parameter: sub_viz_est_setting + topic: /obelisk/g1/est_state + history_depth: 10 + callback_group: None + non_obelisk: False + publishers: + - ros_parameter: pub_viz_joint_setting + topic: joint_states + history_depth: 10 callback_group: None - - ros_parameter: timer_logging_setting - timer_period_sec: 0.2 + timers: + - ros_parameter: timer_viz_joint_setting + timer_period_sec: 0.01 callback_group: None - viz: - on: True - viz_tool: foxglove - viz_nodes: - - pkg: obelisk_viz_cpp - executable: default_robot_viz - robot_pkg: g1_description - urdf: g1.urdf - robot_topic: robot_description - subscribers: - - ros_parameter: sub_viz_est_setting - topic: /obelisk/g1/est_state - history_depth: 10 - callback_group: None - non_obelisk: False - publishers: - - ros_parameter: pub_viz_joint_setting - topic: joint_states - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_viz_joint_setting - timer_period_sec: 0.01 - callback_group: None - joystick: - on: True - pub_topic: /obelisk/g1/joy - sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file +joystick: + on: True + pub_topic: /obelisk/g1/joy + sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index bbe1681f..b2037f33 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -1,233 +1,231 @@ config: go2_example -onboard: - control: - # ----- Low Level Controller ----- # - - pkg: obelisk_unitree_cpp - params: - robot_str: Go2 - executable: unitree_example_controller - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/go2/ctrl - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/go2/est_state - history_depth: 10 - callback_group: None - # ----- Joystick subscriber ----- # - - ros_parameter: joystick_sub_setting - topic: /obelisk/go2/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - # ----- High Level/Execution FSM Controller ----- # - - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_joystick - # params: - # v_x_scale: 1.0 - # v_y_scale: 0.5 - # w_z_scale: 1.0 - # callback_groups: - publishers: - # ----- Execution FSM ----- # - - ros_parameter: pub_exec_fsm_setting - topic: /obelisk/go2/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: pub_ctrl_setting - topic: /obelisk/go2/vel_cmd - history_depth: 10 - callback_group: None - subscribers: +control: +# ----- Low Level Controller ----- # + - pkg: obelisk_unitree_cpp + params: + robot_str: Go2 + executable: unitree_example_controller + # callback_groups: + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/go2/ctrl + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/go2/est_state + history_depth: 10 + callback_group: None # ----- Joystick subscriber ----- # - - ros_parameter: sub_est_setting - topic: /obelisk/go2/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 100 # Control callback is not being used - callback_group: None - estimation: - - pkg: obelisk_unitree_cpp - executable: unitree_go2_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/go2/est_state - msg_type: EstimatedState - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - topic: /obelisk/go2/joint_encoders + - ros_parameter: joystick_sub_setting + topic: /obelisk/go2/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None +# ----- High Level/Execution FSM Controller ----- # + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + # params: + # v_x_scale: 1.0 + # v_y_scale: 0.5 + # w_z_scale: 1.0 + # callback_groups: + publishers: + # ----- Execution FSM ----- # + - ros_parameter: pub_exec_fsm_setting + topic: /obelisk/go2/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: pub_ctrl_setting + topic: /obelisk/go2/vel_cmd + history_depth: 10 + callback_group: None + subscribers: + # ----- Joystick subscriber ----- # + - ros_parameter: sub_est_setting + topic: /obelisk/go2/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 100 # Control callback is not being used + callback_group: None +estimation: + - pkg: obelisk_unitree_cpp + executable: unitree_go2_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/go2/est_state + msg_type: EstimatedState + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/go2/joint_encoders + msg_type: ObkJointEncoders + history_depth: 10 + callback_group: None + - ros_parameter: sub_imu_setting + topic: /obelisk/go2/torso_imu + msg_type: ObkIMU + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None +# sensing: +robot: + # === simulation === + # - is_simulated: True + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_sim + # params: + # ic_keyframe: standing + # === hardware === + - is_simulated: False + pkg: obelisk_unitree_cpp + executable: obelisk_unitree_go2_hardware + params: + network_interface_name: enx6c1ff724e92e + default_kp: [ + 60., 60., 60., # FR + 60., 60., 60., # FL + 60., 60., 60., # RR + 60., 60., 60., # RL + ] + default_kd: [ + 5., 5., 5., # FR + 5., 5., 5., # FL + 5., 5., 5., # RR + 5., 5., 5., # RL + ] + default_kd_damping: [ + 15., 15., 15., # FR + 15., 15., 15., # FL + 15., 15., 15., # RR + 15., 15., 15., # RL + ] + # ================== + # callback_groups: + publishers: + # ----- Joints ----- # + - ros_parameter: pub_sensor_setting + topic: /obelisk/go2/joint_encoders + history_depth: 10 + # ----- IMU ----- # + - ros_parameter: pub_imu_setting + topic: /obelisk/go2/torso_imu + history_depth: 10 + # ----- Force Sensors ----- # + - ros_parameter: pub_force_sensor_setting + topic: /obelisk/go2/feet_force + history_depth: 10 + # ----- Odom ----- # + # - ros_parameter: pub_odom_setting + # topic: /obelisk/go2/odom + # history_depth: 10 + subscribers: + # ----- Control ----- # + - ros_parameter: sub_ctrl_setting + topic: /obelisk/go2/ctrl + history_depth: 10 + callback_group: None + # ----- Execution FSM ----- # + - ros_parameter: sub_fsm_setting + topic: /obelisk/go2/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: sub_vel_cmd_setting + topic: /obelisk/go2/vel_cmd + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + n_u: 16 + time_step: 0.002 + num_steps_per_viz: 5 + robot_pkg: go2_description + # model_xml_path: go2_fixed_base.xml + model_xml_path: go2_scene.xml + sensor_settings: + - topic: /obelisk/go2/joint_encoders + dt: 0.002 msg_type: ObkJointEncoders - history_depth: 10 - callback_group: None - - ros_parameter: sub_imu_setting - topic: /obelisk/go2/torso_imu - msg_type: ObkIMU - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - # === simulation === - # - is_simulated: True - # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_sim - # params: - # ic_keyframe: standing - # === hardware === - - is_simulated: False - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_go2_hardware - params: - network_interface_name: enx6c1ff724e92e - default_kp: [ - 60., 60., 60., # FR - 60., 60., 60., # FL - 60., 60., 60., # RR - 60., 60., 60., # RL - ] - default_kd: [ - 5., 5., 5., # FR - 5., 5., 5., # FL - 5., 5., 5., # RR - 5., 5., 5., # RL - ] - default_kd_damping: [ - 15., 15., 15., # FR - 15., 15., 15., # FL - 15., 15., 15., # RR - 15., 15., 15., # RL - ] - # ================== - # callback_groups: - publishers: - # ----- Joints ----- # - - ros_parameter: pub_sensor_setting - topic: /obelisk/go2/joint_encoders - history_depth: 10 - # ----- IMU ----- # - - ros_parameter: pub_imu_setting - topic: /obelisk/go2/torso_imu - history_depth: 10 - # ----- Force Sensors ----- # - - ros_parameter: pub_force_sensor_setting - topic: /obelisk/go2/feet_force - history_depth: 10 - # ----- Odom ----- # - # - ros_parameter: pub_odom_setting - # topic: /obelisk/go2/odom - # history_depth: 10 - subscribers: - # ----- Control ----- # - - ros_parameter: sub_ctrl_setting - topic: /obelisk/go2/ctrl - history_depth: 10 - callback_group: None - # ----- Execution FSM ----- # - - ros_parameter: sub_fsm_setting - topic: /obelisk/go2/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: sub_vel_cmd_setting - topic: /obelisk/go2/vel_cmd - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - n_u: 16 - time_step: 0.002 - num_steps_per_viz: 5 - robot_pkg: go2_description - # model_xml_path: go2_fixed_base.xml - model_xml_path: go2_scene.xml - sensor_settings: - - topic: /obelisk/go2/joint_encoders - dt: 0.002 - msg_type: ObkJointEncoders - sensor_names: - # ---------- Joint Positions ---------- # - FR_hip_joint_pos_sensor: jointpos - FR_thigh_joint_pos_sensor: jointpos - FR_calf_joint_pos_sensor: jointpos + sensor_names: + # ---------- Joint Positions ---------- # + FR_hip_joint_pos_sensor: jointpos + FR_thigh_joint_pos_sensor: jointpos + FR_calf_joint_pos_sensor: jointpos - FL_hip_joint_pos_sensor: jointpos - FL_thigh_joint_pos_sensor: jointpos - FL_calf_joint_pos_sensor: jointpos + FL_hip_joint_pos_sensor: jointpos + FL_thigh_joint_pos_sensor: jointpos + FL_calf_joint_pos_sensor: jointpos - RR_hip_joint_pos_sensor: jointpos - RR_thigh_joint_pos_sensor: jointpos - RR_calf_joint_pos_sensor: jointpos + RR_hip_joint_pos_sensor: jointpos + RR_thigh_joint_pos_sensor: jointpos + RR_calf_joint_pos_sensor: jointpos - RL_hip_joint_pos_sensor: jointpos - RL_thigh_joint_pos_sensor: jointpos - RL_calf_joint_pos_sensor: jointpos - # ---------- Joint Velocities ---------- # - FR_hip_joint_vel_sensor: jointvel - FR_thigh_joint_vel_sensor: jointvel - FR_calf_joint_vel_sensor: jointvel + RL_hip_joint_pos_sensor: jointpos + RL_thigh_joint_pos_sensor: jointpos + RL_calf_joint_pos_sensor: jointpos + # ---------- Joint Velocities ---------- # + FR_hip_joint_vel_sensor: jointvel + FR_thigh_joint_vel_sensor: jointvel + FR_calf_joint_vel_sensor: jointvel - FL_hip_joint_vel_sensor: jointvel - FL_thigh_joint_vel_sensor: jointvel - FL_calf_joint_vel_sensor: jointvel + FL_hip_joint_vel_sensor: jointvel + FL_thigh_joint_vel_sensor: jointvel + FL_calf_joint_vel_sensor: jointvel - RR_hip_joint_vel_sensor: jointvel - RR_thigh_joint_vel_sensor: jointvel - RR_calf_joint_vel_sensor: jointvel + RR_hip_joint_vel_sensor: jointvel + RR_thigh_joint_vel_sensor: jointvel + RR_calf_joint_vel_sensor: jointvel - RL_hip_joint_vel_sensor: jointvel - RL_thigh_joint_vel_sensor: jointvel - RL_calf_joint_vel_sensor: jointvel - # ---------- Torso IMU ---------- # - - topic: /obelisk/go2/torso_imu - dt: 0.001 - msg_type: ObkImu - sensor_names: - imu_acc_sensor: accelerometer - imu_gyro_sensor: gyro - imu_quat_sensor: framequat - timers: - - ros_parameter: timer_sensor_setting - timer_period_sec: 0.02 + RL_hip_joint_vel_sensor: jointvel + RL_thigh_joint_vel_sensor: jointvel + RL_calf_joint_vel_sensor: jointvel + # ---------- Torso IMU ---------- # + - topic: /obelisk/go2/torso_imu + dt: 0.001 + msg_type: Imu + sensor_names: + imu_acc_sensor: accelerometer + imu_gyro_sensor: gyro + imu_quat_sensor: framequat + timers: + - ros_parameter: timer_sensor_setting + timer_period_sec: 0.02 + callback_group: None + - ros_parameter: timer_logging_setting + timer_period_sec: 0.2 + callback_group: None +viz: + on: True + viz_nodes: + - pkg: obelisk_viz_cpp + executable: default_robot_viz + robot_pkg: go2_description + urdf: go2.urdf + robot_topic: robot_description + subscribers: + - ros_parameter: sub_viz_est_setting + topic: /obelisk/go2/est_state + history_depth: 10 + callback_group: None + non_obelisk: False + publishers: + - ros_parameter: pub_viz_joint_setting + topic: joint_states + history_depth: 10 callback_group: None - - ros_parameter: timer_logging_setting - timer_period_sec: 0.2 + timers: + - ros_parameter: timer_viz_joint_setting + timer_period_sec: 0.01 callback_group: None - viz: - on: True - viz_tool: foxglove - viz_nodes: - - pkg: obelisk_viz_cpp - executable: default_robot_viz - robot_pkg: go2_description - urdf: go2.urdf - robot_topic: robot_description - subscribers: - - ros_parameter: sub_viz_est_setting - topic: /obelisk/go2/est_state - history_depth: 10 - callback_group: None - non_obelisk: False - publishers: - - ros_parameter: pub_viz_joint_setting - topic: joint_states - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_viz_joint_setting - timer_period_sec: 0.01 - callback_group: None - joystick: - on: True - pub_topic: /obelisk/go2/joy - sub_topic: /obelisk/go2/joy_feedback +joystick: + on: True + pub_topic: /obelisk/go2/joy + sub_topic: /obelisk/go2/joy_feedback diff --git a/obelisk_ws/src/obelisk_ros/config/leap_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/leap_cpp.yaml deleted file mode 100644 index 7f79ddff..00000000 --- a/obelisk_ws/src/obelisk_ros/config/leap_cpp.yaml +++ /dev/null @@ -1,114 +0,0 @@ -config: leap -onboard: - control: - - pkg: obelisk_leap_cpp - executable: leap_example_controller - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - # key: pub1 - topic: /obelisk/leap/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - non_obelisk: False - subscribers: - - ros_parameter: sub_est_setting - # key: sub1 - topic: /obelisk/leap/est - msg_type: EstimatedState - history_depth: 10 - # callback_key: sub_callback1 - callback_group: None - non_obelisk: False - timers: - - ros_parameter: timer_ctrl_setting - # key: timer1 - timer_period_sec: 0.001 - callback_group: None - # callback_key: timer_callback1 - estimation: - - pkg: obelisk_estimation_cpp - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - # key: pub1 - topic: /obelisk/leap/est - msg_type: EstimatedState - history_depth: 10 - callback_group: None - non_obelisk: False - subscribers: - - ros_parameter: sub_sensor_setting - # key: sub1 - topic: /obelisk/leap/joint_encoders - msg_type: ObkJointEncoders - history_depth: 10 - # callback_key: sub_callback1 - callback_group: None - non_obelisk: False - timers: - - ros_parameter: timer_est_setting - # key: timer1 - timer_period_sec: 0.001 - callback_group: None - # callback_key: timer_callback1 - # sensing: - robot: - # === simulation === - - is_simulated: True - pkg: obelisk_sim_cpp - executable: obelisk_mujoco_robot - # === hardware === - # - is_simulated: False - # pkg: obelisk_leap_cpp - # executable: obelisk_leap_robot - # ================== - # callback_groups: - publishers: - - ros_parameter: pub_sensor_setting - topic: /obelisk/leap/joint_encoders - msg_type: ObkJointEncoders - history_depth: 10 - callback_group: None - non_obelisk: False - subscribers: - - ros_parameter: sub_ctrl_setting - topic: /obelisk/leap/ctrl - msg_type: PositionSetpoint - history_depth: 10 - callback_group: None - non_obelisk: False - sim: - - ros_parameter: mujoco_setting - n_u: 16 - time_step: 0.002 - num_steps_per_viz: 5 - robot_pkg: leap_rh - model_xml_path: leap_palm_up.xml - sensor_settings: - - topic: /obelisk/leap/joint_encoders - dt: 0.002 - msg_type: ObkJointEncoders - sensor_names: - if_mcp_sensor: jointpos - if_rot_sensor: jointpos - if_pip_sensor: jointpos - if_dip_sensor: jointpos - mf_mcp_sensor: jointpos - mf_rot_sensor: jointpos - mf_pip_sensor: jointpos - mf_dip_sensor: jointpos - rf_mcp_sensor: jointpos - rf_rot_sensor: jointpos - rf_pip_sensor: jointpos - rf_dip_sensor: jointpos - th_cmc_sensor: jointpos - th_axl_sensor: jointpos - th_mcp_sensor: jointpos - th_ipl_sensor: jointpos - timers: - - ros_parameter: timer_sensor_setting - timer_period_sec: 0.02 - callback_group: None diff --git a/obelisk_ws/src/obelisk_ros/config/leap_py.yaml b/obelisk_ws/src/obelisk_ros/config/leap_py.yaml deleted file mode 100644 index 9167bac9..00000000 --- a/obelisk_ws/src/obelisk_ros/config/leap_py.yaml +++ /dev/null @@ -1,94 +0,0 @@ -config: leap -onboard: - control: - - pkg: obelisk_leap_py - executable: leap_example_controller - # callback_groups: - publishers: - - ros_parameter: pub_ctrl_setting - topic: /obelisk/leap/ctrl - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_est_setting - topic: /obelisk/leap/est - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 0.001 - callback_group: None - estimation: - - pkg: obelisk_estimation_py - executable: jointencoders_passthrough_estimator - # callback_groups: - publishers: - - ros_parameter: pub_est_setting - topic: /obelisk/leap/est - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_sensor_setting - # key: sub1 - topic: /obelisk/leap/joint_encoders - history_depth: 10 - callback_group: None - timers: - - ros_parameter: timer_est_setting - timer_period_sec: 0.001 - callback_group: None - # sensing: - robot: - # === simulation === - - is_simulated: True - pkg: obelisk_sim_py - executable: obelisk_mujoco_robot - # === hardware === - # - is_simulated: False - # pkg: obelisk_leap_py - # executable: obelisk_leap_robot - # ================== - # callback_groups: - publishers: # for hardware only - - ros_parameter: pub_sensor_setting - topic: /obelisk/leap/joint_encoders - history_depth: 10 - callback_group: None - subscribers: - - ros_parameter: sub_ctrl_setting - # key: sub1 - topic: /obelisk/leap/ctrl - history_depth: 10 - callback_group: None - sim: - - ros_parameter: mujoco_setting - n_u: 16 - time_step: 0.002 - num_steps_per_viz: 5 - robot_pkg: leap_rh - model_xml_path: leap_palm_up.xml - sensor_settings: - - topic: /obelisk/leap/joint_encoders - dt: 0.002 - msg_type: ObkJointEncoders - sensor_names: - if_mcp_sensor: jointpos - if_rot_sensor: jointpos - if_pip_sensor: jointpos - if_dip_sensor: jointpos - mf_mcp_sensor: jointpos - mf_rot_sensor: jointpos - mf_pip_sensor: jointpos - mf_dip_sensor: jointpos - rf_mcp_sensor: jointpos - rf_rot_sensor: jointpos - rf_pip_sensor: jointpos - rf_dip_sensor: jointpos - th_cmc_sensor: jointpos - th_axl_sensor: jointpos - th_mcp_sensor: jointpos - th_ipl_sensor: jointpos - timers: - - ros_parameter: timer_sensor_setting - timer_period_sec: 0.02 - callback_group: None diff --git a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml index 44bc36ee..693d193d 100644 --- a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml +++ b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml @@ -1,35 +1,34 @@ config: unitree_fsm_debug -onboard: - control: - # ----- High Level/Execution FSM Controller ----- # - - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_joystick - # callback_groups: - publishers: - # ----- Execution FSM ----- # - - ros_parameter: pub_exec_fsm_setting - topic: /obelisk/fsm_test/exec_fsm - history_depth: 10 - callback_group: None - # ----- High Level Control ----- # - - ros_parameter: pub_ctrl_setting - topic: /obelisk/fsm_test/high_level_ctrl - history_depth: 10 - callback_group: None - # ----- Joystick Passthrough ----- # - - ros_parameter: pub_joy_passthrough_setting - topic: /obelisk/fsm_test/joy_passthrough - history_depth: 10 - callback_group: None - subscribers: - # ----- Joystick subscriber ----- # - - ros_parameter: sub_est_setting - topic: /obelisk/fsm_test/joy - timers: - - ros_parameter: timer_ctrl_setting - timer_period_sec: 100 # Control callback is not being used - callback_group: None - joystick: - on: True - pub_topic: /obelisk/fsm_test/joy - sub_topic: /obelisk/fsm_test/joy_feedback \ No newline at end of file +control: +# ----- High Level/Execution FSM Controller ----- # + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + # callback_groups: + publishers: + # ----- Execution FSM ----- # + - ros_parameter: pub_exec_fsm_setting + topic: /obelisk/fsm_test/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: pub_ctrl_setting + topic: /obelisk/fsm_test/high_level_ctrl + history_depth: 10 + callback_group: None + # ----- Joystick Passthrough ----- # + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/fsm_test/joy_passthrough + history_depth: 10 + callback_group: None + subscribers: + # ----- Joystick subscriber ----- # + - ros_parameter: sub_est_setting + topic: /obelisk/fsm_test/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 100 # Control callback is not being used + callback_group: None +joystick: + on: True + pub_topic: /obelisk/fsm_test/joy + sub_topic: /obelisk/fsm_test/joy_feedback \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py b/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py index d5a5c337..ba9a265d 100644 --- a/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py +++ b/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py @@ -28,20 +28,17 @@ def launch_args_setup(context: launch.LaunchContext, *args: List, **kwargs: Dict # expose path to config file config_file_path = LaunchConfiguration("config_file_path") - device_name = LaunchConfiguration("device_name") auto_start = LaunchConfiguration("auto_start") bag = LaunchConfiguration("bag") config_file_path_arg = DeclareLaunchArgument("config_file_path") - device_name_arg = DeclareLaunchArgument("device_name") auto_start_arg = DeclareLaunchArgument("auto_start", default_value="True") - bag_arg = DeclareLaunchArgument("bag", default_value="True") + bag_arg = DeclareLaunchArgument("bag", default_value="False") - launch_actions += [config_file_path_arg, device_name_arg, auto_start_arg, bag_arg] + launch_actions += [config_file_path_arg, auto_start_arg, bag_arg] launch_args.update( { "config_file_path": config_file_path, - "device_name": device_name, "auto_start": auto_start, "bag": bag, } @@ -57,7 +54,6 @@ def obelisk_setup(context: launch.LaunchContext, launch_args: Dict) -> List: # parsing the launch arguments config_file_path = context.launch_configurations.get("config_file_path") - device_name = context.launch_configurations.get("device_name") auto_start = context.launch_configurations.get("auto_start") auto_start = "true" if auto_start is None else auto_start.lower() bag = context.launch_configurations.get("bag") @@ -65,8 +61,10 @@ def obelisk_setup(context: launch.LaunchContext, launch_args: Dict) -> List: full_config_dict = load_config_file(config_file_path) config_name = full_config_dict["config"] - obelisk_config = full_config_dict[device_name] # grab the settings associated with the device - logger.info(f"Bringing up the Obelisk nodes on device {device_name}...") + # The full config is a flat dict at the top level: keys include `config` (the run name) + # plus any of `control`, `estimation`, `robot`, `sensing`, `viz`, `joystick`. + obelisk_config = full_config_dict + logger.info(f"Bringing up the Obelisk nodes for config '{config_name}'...") # checks - we must at least have these 3 components # New -> no longer require control/estimation/robot diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/leap_example_controller.py b/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/leap_example_controller.py deleted file mode 100644 index 2b8f3cd8..00000000 --- a/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/leap_example_controller.py +++ /dev/null @@ -1,15 +0,0 @@ -from typing import List, Optional - -from rclpy.executors import SingleThreadedExecutor - -from obelisk_py.core.utils.ros import spin_obelisk -from obelisk_py.zoo.control.example.leap_example_pos_setpoint_controller import LeapExamplePositionSetpointController - - -def main(args: Optional[List] = None) -> None: - """Main entrypoint.""" - spin_obelisk(args, LeapExamplePositionSetpointController, SingleThreadedExecutor) - - -if __name__ == "__main__": - main() diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/obelisk_leap_robot.py b/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/obelisk_leap_robot.py deleted file mode 100644 index 0be1d989..00000000 --- a/obelisk_ws/src/python/hardware/obelisk_leap_py/obelisk_leap_py/obelisk_leap_robot.py +++ /dev/null @@ -1,15 +0,0 @@ -from typing import List, Optional - -from rclpy.executors import SingleThreadedExecutor - -from obelisk_py.core.utils.ros import spin_obelisk -from obelisk_py.zoo.hardware.robots.leap.leap_hand_interface import ObeliskLeapRobot - - -def main(args: Optional[List] = None) -> None: - """Main entrypoint.""" - spin_obelisk(args, ObeliskLeapRobot, SingleThreadedExecutor) - - -if __name__ == "__main__": - main() diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/package.xml b/obelisk_ws/src/python/hardware/obelisk_leap_py/package.xml deleted file mode 100644 index 26e33387..00000000 --- a/obelisk_ws/src/python/hardware/obelisk_leap_py/package.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - obelisk_leap_py - 0.0.1 - Python Obelisk ROS2 package for LEAP hand control. - gavinhua - alberthli - MIT - - - ament_python - - diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/resource/obelisk_leap_py b/obelisk_ws/src/python/hardware/obelisk_leap_py/resource/obelisk_leap_py deleted file mode 100644 index e69de29b..00000000 diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/setup.cfg b/obelisk_ws/src/python/hardware/obelisk_leap_py/setup.cfg deleted file mode 100644 index 599ce320..00000000 --- a/obelisk_ws/src/python/hardware/obelisk_leap_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/obelisk_leap_py -[install] -install_scripts=$base/lib/obelisk_leap_py diff --git a/obelisk_ws/src/python/hardware/obelisk_leap_py/setup.py b/obelisk_ws/src/python/hardware/obelisk_leap_py/setup.py deleted file mode 100644 index 336389cd..00000000 --- a/obelisk_ws/src/python/hardware/obelisk_leap_py/setup.py +++ /dev/null @@ -1,34 +0,0 @@ -import os -from glob import glob -from warnings import simplefilter - -from setuptools import SetuptoolsDeprecationWarning, setup - -simplefilter("ignore", category=SetuptoolsDeprecationWarning) - -package_name = "obelisk_leap_py" - -setup( - name=package_name, - version="0.0.1", - packages=[package_name], - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name, "launch"), glob(os.path.join("launch", "*launch.[pxy][yma]*"))), - (os.path.join("share", package_name, "config"), glob(os.path.join("config", "*.yaml"))), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="gavinhua", - maintainer_email="ghua@caltech.edu", - description="Python Obelisk ROS2 package for simulation.", - license="MIT", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "obelisk_leap_robot = obelisk_leap_py.obelisk_leap_robot:main", - "leap_example_controller = obelisk_leap_py.leap_example_controller:main", - ], - }, -) diff --git a/scripts/add_cuda_cmake_prefix_path.sh b/scripts/add_cuda_cmake_prefix_path.sh deleted file mode 100644 index c7545776..00000000 --- a/scripts/add_cuda_cmake_prefix_path.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -export CMAKE_PREFIX_PATH="$CMAKE_PREFIX_PATH:$PIXI_PROJECT_ROOT/.pixi/envs/$PIXI_ENVIRONMENT_NAME/targets/x86_64-linux" diff --git a/scripts/build_obelisk.sh b/scripts/build_obelisk.sh index e41935aa..e8f734df 100644 --- a/scripts/build_obelisk.sh +++ b/scripts/build_obelisk.sh @@ -1,6 +1,5 @@ #!/bin/bash -leap=false zed=false verbose=false @@ -18,7 +17,7 @@ for arg in "$@"; do *) # Unknown option echo "Unknown option: $arg" - echo "Usage: $0 [--leap] [--zed] [--verbose]" + echo "Usage: $0 [--zed] [--verbose]" exit 1 ;; esac @@ -38,6 +37,13 @@ fi # building Obelisk packages OBELISK_ROOT=$(dirname $(dirname $(readlink -f ${BASH_SOURCE[0]}))) +# Ensure the local obelisk_py is the one Python imports. The Docker image installs obelisk_py from +# GitHub main during image build (because OBELISK_ROOT isn't bind-mounted yet), so without this +# step launch_utils.py would be stale relative to the live source tree. +echo -e "\033[1;32mInstalling obelisk_py editable from local source...\033[0m" +pip install -e $OBELISK_ROOT/obelisk/python --quiet || \ + pip install --user -e $OBELISK_ROOT/obelisk/python --quiet + echo -e "\033[1;32mBuilding Obelisk ROS messages...\033[0m" curr_dir=$(pwd) cd $OBELISK_ROOT/obelisk_ws diff --git a/scripts/clean_obelisk.sh b/scripts/clean_obelisk.sh index 3d825269..52b589f0 100644 --- a/scripts/clean_obelisk.sh +++ b/scripts/clean_obelisk.sh @@ -4,15 +4,10 @@ rm -rf $OBELISK_ROOT/obelisk/cpp/build # removes the build/install/log directories of the obelisk_ws -if [ -z "$PIXI_ENVIRONMENT_NAME" ]; then - if [ -z "$OBELISK_ROOT" ]; then - echo -e "\033[1;31mOBELISK_ROOT is not set. Run dev_setup.sh first!\033[0m" - exit 0 - fi - rm -rf $OBELISK_ROOT/obelisk_ws/build $OBELISK_ROOT/obelisk_ws/install $OBELISK_ROOT/obelisk_ws/log - -else - pixi run ros-clean +if [ -z "$OBELISK_ROOT" ]; then + echo -e "\033[1;31mOBELISK_ROOT is not set. Run setup.sh first!\033[0m" + exit 0 fi +rm -rf $OBELISK_ROOT/obelisk_ws/build $OBELISK_ROOT/obelisk_ws/install $OBELISK_ROOT/obelisk_ws/log echo -e "\033[1;32mDeleted the build/install/log dirs under obelisk_ws!\033[0m" diff --git a/scripts/config_groups.sh b/scripts/config_groups.sh index a29e1c4a..366a7946 100644 --- a/scripts/config_groups.sh +++ b/scripts/config_groups.sh @@ -1,7 +1,6 @@ #!/bin/bash # --- script flags --- # -leap=false zed=false for arg in "$@"; do diff --git a/scripts/docker_setup.sh b/scripts/docker_setup.sh index 06500904..df691ed9 100644 --- a/scripts/docker_setup.sh +++ b/scripts/docker_setup.sh @@ -3,13 +3,10 @@ docker_install=false docker_basic=false docker_cyclone_perf=false -docker_leap=false docker_zed=false -docker_pixi=false docker_unitree=false docker_mujoco=false -docker_group_leap=false docker_group_zed=false for arg in "$@"; do @@ -185,9 +182,9 @@ if [ "$docker_unitree" = true ]; then echo "OBELISK_DOCKER_UNITREE=true" >> $env_file export OBELISK_DOCKER_UNITREE=true else - echo -e "\033[1;33mSetting OBELISK_DOCKER_PIXI=false!\033[0m" - echo "OBELISK_DOCKER_PIXI=false" >> $env_file - export OBELISK_DOCKER_PIXI=false + echo -e "\033[1;33mSetting OBELISK_DOCKER_UNITREE=false!\033[0m" + echo "OBELISK_DOCKER_UNITREE=false" >> $env_file + export OBELISK_DOCKER_UNITREE=false fi if [ "$docker_mujoco" = true ]; then diff --git a/scripts/install_sys_deps.sh b/scripts/install_sys_deps.sh index 84266a3c..13fa8572 100644 --- a/scripts/install_sys_deps.sh +++ b/scripts/install_sys_deps.sh @@ -7,7 +7,6 @@ cyclone_perf=false source_ros=false # hardware-specific deps -leap=false zed=false unitree=false @@ -52,7 +51,7 @@ Options: *) # Unknown option echo "Unknown option: $arg" - echo "Usage: $0 [--basic] [--cyclone-perf] [--source-ros] [--leap] [--zed]" + echo "Usage: $0 [--basic] [--cyclone-perf] [--source-ros] [--zed] [--unitree]" exit 1 ;; esac diff --git a/scripts/user_setup.sh b/scripts/user_setup.sh index 3f91090d..e2d7aebe 100644 --- a/scripts/user_setup.sh +++ b/scripts/user_setup.sh @@ -1,7 +1,6 @@ #!/bin/bash # script flags -pixi=false obk_aliases=false zed=false @@ -10,10 +9,6 @@ unitree=false for arg in "$@"; do case $arg in # general user setup - --pixi) - pixi=true - shift # Installs pixi - ;; --obk-aliases) obk_aliases=true shift # Adds obelisk aliases to the ~/.bash_aliases file @@ -31,23 +26,13 @@ for arg in "$@"; do *) # Unknown option echo "Unknown option: $arg" - echo "Usage: $0 [--cyclone-perf] [--obk-aliases]" + echo "Usage: $0 [--obk-aliases] [--zed] [--unitree]" exit 1 ;; esac done -# [1] installs pixi -if [ "$pixi" = true ]; then - if ! command -v pixi &> /dev/null; then - echo -e "\033[1;32mPixi is not installed. Installing Pixi...\033[0m" - curl -fsSL https://pixi.sh/install.sh | bash - else - echo -e "\033[1;33mPixi is already installed. Skipping Pixi installation.\033[0m" - fi -fi - -# [2] adds obelisk aliases to the ~/.bash_aliases file +# adds obelisk aliases to the ~/.bash_aliases file if [ "$obk_aliases" = true ]; then # check if ~/.bash_aliases is sourced in ~/.bashrc; if not, add it block='if [ -f ~/.bash_aliases ]; then @@ -127,9 +112,9 @@ export ROS_DOMAIN_ID=$ROS_DOMAIN_ID export HAS_OBK_ACTIVATED=true fi - # checks if current shell is a conda or pixi shell - if not, source base ros if /opt/ros/humble/setup.bash exists + # checks if current shell is a conda shell - if not, source base ros if /opt/ros/humble/setup.bash exists # in the case of conda, it will source base ros if \$CONDA_DEFAULT_ENV is "base" still - if [[ -z "\$CONDA_DEFAULT_ENV" || "\$CONDA_DEFAULT_ENV" == "base" ]] && [[ -z "\$PIXI_ENVIRONMENT_NAME" ]]; then + if [[ -z "\$CONDA_DEFAULT_ENV" || "\$CONDA_DEFAULT_ENV" == "base" ]]; then if [ -f "/opt/ros/humble/setup.bash" ]; then echo -e "\033[1;32mSourcing base ROS2 Humble installation...\033[0m" source /opt/ros/humble/setup.bash @@ -143,11 +128,6 @@ export ROS_DOMAIN_ID=$ROS_DOMAIN_ID source \$OBELISK_ROOT/scripts/build_obelisk.sh $OBELISK_BUILD_OPTIONS source \$OBELISK_ROOT/obelisk_ws/install/setup.bash fi - - # if PIXI_ZED=true, then run bash $OBELISK_ROOT/scripts/install_pyzed.sh - if [[ -n "\$PIXI_ZED" && "\$PIXI_ZED" == "true" ]]; then - bash \$OBELISK_ROOT/scripts/install_pyzed.sh - fi } # convenience aliases for building/cleaning obelisk source packages @@ -240,9 +220,8 @@ function obk-kill { # convenience function for ros2 launch command function obk-launch { local config="" - local device="" local auto_start="True" - local bag="True" + local bag="False" while [[ \$# -gt 0 ]]; do key="\$1" @@ -251,10 +230,6 @@ function obk-launch { config="\${key#*=}" shift ;; - device=*) - device="\${key#*=}" - shift - ;; auto_start=*) auto_start="\${key#*=}" shift @@ -270,14 +245,13 @@ function obk-launch { esac done - # Check if any of the required arguments are empty - if [[ -z "\$config" || -z "\$device" ]]; then - echo -e "\033[1;34mError: Missing required arguments.\033[0m" - echo -e "\033[1;34mUsage: obk-launch config= device= auto_start=\033[0m" + if [[ -z "\$config" ]]; then + echo -e "\033[1;34mError: Missing required argument.\033[0m" + echo -e "\033[1;34mUsage: obk-launch config= [auto_start=] [bag=]\033[0m" return 1 fi - ros2 launch obelisk_ros obelisk_bringup.launch.py config_file_path:=\${config} device_name:=\${device} auto_start:=\${auto_start} bag:=\${bag} + ros2 launch obelisk_ros obelisk_bringup.launch.py config_file_path:=\${config} auto_start:=\${auto_start} bag:=\${bag} } # help command @@ -291,11 +265,12 @@ Options:\n\ --permanent: Adds the global settings to ~/.bashrc.\n\ --remove: Removes the global settings from ~/.bashrc.\n\n\ obk-build:\n\ -Builds Obelisk nodes after you have activated a pixi environment.\n\n\ +Builds Obelisk ROS2 nodes via colcon (run inside the dev container).\n\n\ obk-launch:\n\ Launches the obelisk_bringup.launch.py with specified arguments.\n\ -Usage: obk-launch config= device= auto_start= bag=\n\ -Example:\n obk-launch config=example.yaml device=onboard auto_start=True bag=True\n\n\ +Usage: obk-launch config= [auto_start=] [bag=]\n\ +Defaults: auto_start=True, bag=False (pass bag=True to record a rosbag of all topics).\n\ +Example:\n obk-launch config=example.yaml\n\n\ State Transitions:\n\ obk-configure:\n Configure all Obelisk nodes.\n Usage: obk-configure \n\ obk-activate:\n Activate all Obelisk nodes.\n Usage: obk-activate \n\ diff --git a/setup.sh b/setup.sh index 17150104..429f0223 100644 --- a/setup.sh +++ b/setup.sh @@ -124,7 +124,6 @@ fi # run user-specific setup source $OBELISK_ROOT/scripts/user_setup.sh \ - $([ "$pixi" = true ] && echo "--pixi") \ $([ "$zed" = true ] && echo "--zed") \ $([ "$unitree" = true ] && echo "--unitree") \ $([ "$obk_aliases" = true ] && echo "--obk-aliases") diff --git a/tests/tests_cpp/CMakeLists.txt b/tests/tests_cpp/CMakeLists.txt index 39b6e497..c21ac879 100644 --- a/tests/tests_cpp/CMakeLists.txt +++ b/tests/tests_cpp/CMakeLists.txt @@ -22,12 +22,8 @@ add_executable(NodeTest test_robot.cpp test_sim_robot.cpp test_mujoco_sim_robot.cpp - claude_obelisk_node_test.cpp - claude_obelisk_controller_test.cpp - claude_obelisk_sim_robot_test.cpp - claude_obelisk_sensor_test.cpp - claude_obelisk_robot_test.cpp - claude_obelisk_estimator_test.cpp) + test_yaml_to_node.cpp + test_lidar_interfaces.cpp) add_executable(VizTest test_viz_robot.cpp diff --git a/tests/tests_cpp/claude_obelisk_controller_test.cpp b/tests/tests_cpp/claude_obelisk_controller_test.cpp deleted file mode 100644 index 9330be3e..00000000 --- a/tests/tests_cpp/claude_obelisk_controller_test.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// test_obelisk_controller.cpp - -#include "obelisk_control_msgs/msg/position_setpoint.hpp" -#include "obelisk_controller.h" -#include "obelisk_estimator_msgs/msg/estimated_state.hpp" -#include - -class TestObeliskController : public obelisk::ObeliskController { - public: - TestObeliskController() : ObeliskController("test_controller") {} - - obelisk_control_msgs::msg::PositionSetpoint ComputeControl() override { - return obelisk_control_msgs::msg::PositionSetpoint(); - } - - void UpdateXHat(const obelisk_estimator_msgs::msg::EstimatedState& msg) override {} -}; - -TEST_CASE("ObeliskController Configuration", "[ObeliskController]") { - rclcpp::init(0, nullptr); - - TestObeliskController controller; - - // Set up parameters - controller.set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:0.1")); - controller.set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:/control,history_depth:10")); - controller.set_parameter(rclcpp::Parameter("sub_est_setting", "topic:/estimate,history_depth:10")); - - // Configure the controller - auto result = controller.on_configure(rclcpp_lifecycle::State()); - - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskController Activation", "[ObeliskController]") { - rclcpp::init(0, nullptr); - TestObeliskController controller; - - // Configure the controller first - controller.set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:0.1")); - controller.set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:/control,history_depth:10")); - controller.set_parameter(rclcpp::Parameter("sub_est_setting", "topic:/estimate,history_depth:10")); - controller.on_configure(rclcpp_lifecycle::State()); - - // Activate the controller - auto result = controller.on_activate(rclcpp_lifecycle::State()); - - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskController Deactivation", "[ObeliskController]") { - rclcpp::init(0, nullptr); - TestObeliskController controller; - - // Configure and activate the controller first - controller.set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:0.1")); - controller.set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:/control,history_depth:10")); - controller.set_parameter(rclcpp::Parameter("sub_est_setting", "topic:/estimate,history_depth:10")); - controller.on_configure(rclcpp_lifecycle::State()); - controller.on_activate(rclcpp_lifecycle::State()); - - // Deactivate the controller - auto result = controller.on_deactivate(rclcpp_lifecycle::State()); - - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskController Cleanup", "[ObeliskController]") { - rclcpp::init(0, nullptr); - TestObeliskController controller; - - // Configure the controller first - controller.set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:0.1")); - controller.set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:/control,history_depth:10")); - controller.set_parameter(rclcpp::Parameter("sub_est_setting", "topic:/estimate,history_depth:10")); - controller.on_configure(rclcpp_lifecycle::State()); - - // Clean up the controller - auto result = controller.on_cleanup(rclcpp_lifecycle::State()); - - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskController Shutdown", "[ObeliskController]") { - rclcpp::init(0, nullptr); - TestObeliskController controller; - - // Configure the controller first - controller.set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:0.1")); - controller.set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:/control,history_depth:10")); - controller.set_parameter(rclcpp::Parameter("sub_est_setting", "topic:/estimate,history_depth:10")); - controller.on_configure(rclcpp_lifecycle::State()); - - // Shut down the controller - auto result = controller.on_shutdown(rclcpp_lifecycle::State()); - - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - rclcpp::shutdown(); -} diff --git a/tests/tests_cpp/claude_obelisk_estimator_test.cpp b/tests/tests_cpp/claude_obelisk_estimator_test.cpp deleted file mode 100644 index e8a3bec4..00000000 --- a/tests/tests_cpp/claude_obelisk_estimator_test.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include "obelisk_estimator.h" -#include "obelisk_estimator_msgs/msg/estimated_state.hpp" -#include - -class TestObeliskEstimator : public obelisk::ObeliskEstimator { - public: - TestObeliskEstimator() : ObeliskEstimator("test_estimator") {} - - obelisk_estimator_msgs::msg::EstimatedState ComputeStateEstimate() override { - return obelisk_estimator_msgs::msg::EstimatedState(); - } -}; - -TEST_CASE("ObeliskEstimator Construction and Configuration", "[ObeliskEstimator]") { - rclcpp::init(0, nullptr); - - TestObeliskEstimator estimator; - - estimator.set_parameter(rclcpp::Parameter("timer_est_setting", "timer_period_sec:0.1")); - estimator.set_parameter(rclcpp::Parameter("pub_est_setting", "topic:/estimated_state,history_depth:10")); - - auto result = estimator.on_configure(rclcpp_lifecycle::State()); - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskEstimator Activation and Deactivation", "[ObeliskEstimator]") { - rclcpp::init(0, nullptr); - - TestObeliskEstimator estimator; - estimator.set_parameter(rclcpp::Parameter("timer_est_setting", "timer_period_sec:0.1")); - estimator.set_parameter(rclcpp::Parameter("pub_est_setting", "topic:/estimated_state,history_depth:10")); - estimator.on_configure(rclcpp_lifecycle::State()); - - auto activate_result = estimator.on_activate(rclcpp_lifecycle::State()); - CHECK(activate_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - auto deactivate_result = estimator.on_deactivate(rclcpp_lifecycle::State()); - CHECK(deactivate_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskEstimator Cleanup and Shutdown", "[ObeliskEstimator]") { - rclcpp::init(0, nullptr); - - TestObeliskEstimator estimator; - estimator.set_parameter(rclcpp::Parameter("timer_est_setting", "timer_period_sec:0.1")); - estimator.set_parameter(rclcpp::Parameter("pub_est_setting", "topic:/estimated_state,history_depth:10")); - - estimator.on_configure(rclcpp_lifecycle::State()); - - auto cleanup_result = estimator.on_cleanup(rclcpp_lifecycle::State()); - CHECK(cleanup_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - auto shutdown_result = estimator.on_shutdown(rclcpp_lifecycle::State()); - CHECK(shutdown_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskEstimator with Empty Sensor Settings", "[ObeliskEstimator]") { - rclcpp::init(0, nullptr); - - TestObeliskEstimator estimator; - estimator.set_parameter(rclcpp::Parameter("timer_est_setting", "timer_period_sec:0.1")); - estimator.set_parameter(rclcpp::Parameter("pub_est_setting", "topic:/estimated_state,history_depth:10")); - - auto result = estimator.on_configure(rclcpp_lifecycle::State()); - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} diff --git a/tests/tests_cpp/claude_obelisk_node_test.cpp b/tests/tests_cpp/claude_obelisk_node_test.cpp deleted file mode 100644 index d29ef590..00000000 --- a/tests/tests_cpp/claude_obelisk_node_test.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// test_obelisk_node.cpp - -#include "obelisk_node.h" -#include - -class TestObeliskNode : public obelisk::ObeliskNode { - public: - TestObeliskNode() : ObeliskNode("test_node") {} - - // Expose protected methods for testing - using ObeliskNode::GetCallbackGroupName; - using ObeliskNode::GetHistoryDepth; - using ObeliskNode::GetMessageName; - using ObeliskNode::GetPeriod; - using ObeliskNode::GetTopic; - using ObeliskNode::ParseCallbackGroupConfig; - using ObeliskNode::ParseConfigStr; -}; - -TEST_CASE("ParseConfigStr", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - - SECTION("Valid config string") { - auto result = node.ParseConfigStr("topic:/test_topic,history_depth:10,non_obelisk:false"); - CHECK(result.size() == 3); - CHECK(result["topic"] == "/test_topic"); - CHECK(result["history_depth"] == "10"); - CHECK(result["non_obelisk"] == "false"); - } - - SECTION("Invalid config string") { REQUIRE_THROWS_AS(node.ParseConfigStr("invalid_string"), std::runtime_error); } - - rclcpp::shutdown(); -} - -TEST_CASE("GetTopic", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - std::map config_map{{"topic", "/test_topic"}}; - - CHECK(node.GetTopic(config_map) == "/test_topic"); - - config_map.clear(); - REQUIRE_THROWS_AS(node.GetTopic(config_map), std::runtime_error); - - rclcpp::shutdown(); -} - -TEST_CASE("GetHistoryDepth", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - std::map config_map{{"history_depth", "20"}}; - - CHECK(node.GetHistoryDepth(config_map) == 20); - - config_map.clear(); - CHECK(node.GetHistoryDepth(config_map) == 10); // Default value - - rclcpp::shutdown(); -} - -TEST_CASE("GetPeriod", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - std::map config_map{{"timer_period_sec", "0.1"}}; - - CHECK(node.GetPeriod(config_map) == 0.1); - - config_map.clear(); - REQUIRE_THROWS_AS(node.GetPeriod(config_map), std::runtime_error); - - rclcpp::shutdown(); -} - -TEST_CASE("GetMessageName", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - std::map config_map{{"message_type", "TestMessage"}}; - - CHECK(node.GetMessageName(config_map) == "TestMessage"); - - config_map.clear(); - REQUIRE_THROWS_AS(node.GetMessageName(config_map), std::runtime_error); - - rclcpp::shutdown(); -} - -TEST_CASE("GetCallbackGroupName", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - std::map config_map{{"callback_group", "TestGroup"}}; - - CHECK(node.GetCallbackGroupName(config_map) == "TestGroup"); - - config_map.clear(); - CHECK(node.GetCallbackGroupName(config_map) == "None"); // Default value - - rclcpp::shutdown(); -} - -TEST_CASE("ParseCallbackGroupConfig", "[ObeliskNode]") { - rclcpp::init(0, nullptr); - - TestObeliskNode node; - - SECTION("Valid config") { - node.ParseCallbackGroupConfig("group1:MutuallyExclusiveCallbackGroup,group2:ReentrantCallbackGroup"); - // We can't directly access callback_groups_, so we'll test indirectly through GetCallbackGroupName - CHECK(node.GetCallbackGroupName({{"callback_group", "group1"}}) == "group1"); - CHECK(node.GetCallbackGroupName({{"callback_group", "group2"}}) == "group2"); - } - - SECTION("Invalid config") { - node.ParseCallbackGroupConfig("group1:InvalidType"); - CHECK(node.GetCallbackGroupName({{"callback_group", "group1"}}) == "group1"); - } - - rclcpp::shutdown(); -} diff --git a/tests/tests_cpp/claude_obelisk_robot_test.cpp b/tests/tests_cpp/claude_obelisk_robot_test.cpp deleted file mode 100644 index eecee7fc..00000000 --- a/tests/tests_cpp/claude_obelisk_robot_test.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "obelisk_control_msgs/msg/position_setpoint.hpp" -#include "obelisk_robot.h" -#include - -class TestObeliskRobot : public obelisk::ObeliskRobot { - public: - TestObeliskRobot() : ObeliskRobot("test_robot") {} - - void ApplyControl(const obelisk_control_msgs::msg::PositionSetpoint& msg) override {} -}; - -TEST_CASE("ObeliskRobot Construction and Configuration", "[ObeliskRobot]") { - rclcpp::init(0, nullptr); - - TestObeliskRobot robot; - - robot.set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:/control,history_depth:10")); - - auto result = robot.on_configure(rclcpp_lifecycle::State()); - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskRobot Configuration with Empty Sensor Settings", "[ObeliskRobot]") { - rclcpp::init(0, nullptr); - - TestObeliskRobot robot; - - robot.set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:/control,history_depth:10")); - - auto result = robot.on_configure(rclcpp_lifecycle::State()); - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskRobot Cleanup and Shutdown", "[ObeliskRobot]") { - rclcpp::init(0, nullptr); - - TestObeliskRobot robot; - robot.set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:/control,history_depth:10")); - robot.on_configure(rclcpp_lifecycle::State()); - - auto cleanup_result = robot.on_cleanup(rclcpp_lifecycle::State()); - CHECK(cleanup_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - auto shutdown_result = robot.on_shutdown(rclcpp_lifecycle::State()); - CHECK(shutdown_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} diff --git a/tests/tests_cpp/claude_obelisk_sensor_test.cpp b/tests/tests_cpp/claude_obelisk_sensor_test.cpp deleted file mode 100644 index db72540f..00000000 --- a/tests/tests_cpp/claude_obelisk_sensor_test.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "obelisk_sensor.h" -#include - -class TestObeliskSensor : public obelisk::ObeliskSensor { - public: - TestObeliskSensor() : ObeliskSensor("test_sensor") {} -}; - -// TEST_CASE("ObeliskSensor Cleanup and Shutdown", "[ObeliskSensor]") { -// rclcpp::init(0, nullptr); - -// TestObeliskSensor sensor; -// // std::vector test_configs = {"topic:/sensor1,history_depth:10"}; -// // sensor.set_parameter(rclcpp::Parameter("pub_sensor_settings", test_configs)); -// sensor.on_configure(rclcpp_lifecycle::State()); - -// auto cleanup_result = sensor.on_cleanup(rclcpp_lifecycle::State()); -// REQUIRE(cleanup_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - -// auto shutdown_result = sensor.on_shutdown(rclcpp_lifecycle::State()); -// REQUIRE(shutdown_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - -// rclcpp::shutdown(); -// } diff --git a/tests/tests_cpp/claude_obelisk_sim_robot_test.cpp b/tests/tests_cpp/claude_obelisk_sim_robot_test.cpp deleted file mode 100644 index 6395c58e..00000000 --- a/tests/tests_cpp/claude_obelisk_sim_robot_test.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "obelisk_control_msgs/msg/position_setpoint.hpp" -#include "obelisk_sim_robot.h" -#include - -class TestObeliskSimRobot : public obelisk::ObeliskSimRobot { - public: - TestObeliskSimRobot() : ObeliskSimRobot("test_sim_robot") {} - - obelisk_sensor_msgs::msg::TrueSimState PublishTrueSimState() override { - return obelisk_sensor_msgs::msg::TrueSimState(); - } - - void RunSimulator() override { - while (!stop_thread_) { - // Simulate some work - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - } - - void ApplyControl(const obelisk_control_msgs::msg::PositionSetpoint& msg) {} - - using ObeliskSimRobot::stop_thread_; -}; - -TEST_CASE("ObeliskSimRobot Construction and Configuration", "[ObeliskSimRobot]") { - rclcpp::init(0, nullptr); - - TestObeliskSimRobot robot; - - robot.set_parameter(rclcpp::Parameter("timer_true_sim_state_setting", "timer_period_sec:0.1")); - robot.set_parameter(rclcpp::Parameter("pub_true_sim_state_setting", "topic:/true_state,history_depth:10")); - robot.set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic5")); - - auto result = robot.on_configure(rclcpp_lifecycle::State()); - CHECK(result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - robot.on_cleanup(rclcpp_lifecycle::State()); - - rclcpp::shutdown(); -} - -TEST_CASE("ObeliskSimRobot Cleanup and Shutdown", "[ObeliskSimRobot]") { - rclcpp::init(0, nullptr); - - TestObeliskSimRobot robot; - robot.set_parameter(rclcpp::Parameter("timer_true_sim_state_setting", "timer_period_sec:0.1")); - robot.set_parameter(rclcpp::Parameter("pub_true_sim_state_setting", "topic:/true_state,history_depth:10")); - robot.set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic5")); - robot.on_configure(rclcpp_lifecycle::State()); - - auto cleanup_result = robot.on_cleanup(rclcpp_lifecycle::State()); - CHECK(cleanup_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - CHECK(robot.stop_thread_ == true); - - auto shutdown_result = robot.on_shutdown(rclcpp_lifecycle::State()); - CHECK(shutdown_result == rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - - rclcpp::shutdown(); -} diff --git a/tests/tests_cpp/test_controller.cpp b/tests/tests_cpp/test_controller.cpp index c9389e14..88374195 100644 --- a/tests/tests_cpp/test_controller.cpp +++ b/tests/tests_cpp/test_controller.cpp @@ -6,10 +6,20 @@ namespace obelisk { obelisk_estimator_msgs::msg::EstimatedState> { public: ObeliskControllerTester() : ObeliskController("obelisk_controller_tester") { - this->set_parameter(rclcpp::Parameter("timer_ctrl_setting", "timer_period_sec:1")); - this->set_parameter(rclcpp::Parameter("pub_ctrl_setting", "topic:topic1")); - this->set_parameter(rclcpp::Parameter("sub_est_setting", "topic:topic2")); - this->set_parameter(rclcpp::Parameter("callback_group_settings", "")); + const std::string settings = R"( +publishers: + - key: pub_ctrl + topic: topic1 + history_depth: 10 +subscribers: + - key: sub_est + topic: topic2 + history_depth: 10 +timers: + - key: timer_ctrl + timer_period_sec: 1.0 +)"; + this->set_parameter(rclcpp::Parameter("obelisk_settings", settings)); } void Configure() { diff --git a/tests/tests_cpp/test_estimator.cpp b/tests/tests_cpp/test_estimator.cpp index 0ee26401..8ace6400 100644 --- a/tests/tests_cpp/test_estimator.cpp +++ b/tests/tests_cpp/test_estimator.cpp @@ -6,9 +6,16 @@ namespace obelisk { class ObeliskEstimatorTester : public ObeliskEstimator { public: ObeliskEstimatorTester() : ObeliskEstimator("obelisk_estimator_tester") { - this->set_parameter(rclcpp::Parameter("timer_est_setting", "timer_period_sec:1")); - this->set_parameter(rclcpp::Parameter("pub_est_setting", "topic:topic1")); - this->set_parameter(rclcpp::Parameter("callback_group_settings", "")); + const std::string settings = R"( +publishers: + - key: pub_est + topic: topic1 + history_depth: 10 +timers: + - key: timer_est + timer_period_sec: 1.0 +)"; + this->set_parameter(rclcpp::Parameter("obelisk_settings", settings)); } void Configure() { diff --git a/tests/tests_cpp/test_lidar_interfaces.cpp b/tests/tests_cpp/test_lidar_interfaces.cpp new file mode 100644 index 00000000..edcb7300 --- /dev/null +++ b/tests/tests_cpp/test_lidar_interfaces.cpp @@ -0,0 +1,259 @@ +// Tests for the ray-caster interfaces under obelisk/cpp/obelisk_cpp/lidar/. +// +// These cover the configuration plumbing of LidarInterface, HeightScanInterface, and DepthInterface +// without exercising MuJoCo (mj_ray etc.). They check that loading a config yields the expected ray +// pattern dimensions, site/frame metadata, and return-value semantics. +// +// Both the legacy file-path constructor and the inline YAML::Node constructor are exercised, so +// these tests guard against regression of the inline-config refactor. + +#include + +#include + +#include +#include +#include +#include + +#include "depth_interface.h" +#include "height_scan_interface.h" +#include "lidar_interface.h" + +namespace { + +// Helper: write a YAML string to a unique temp file and return the path. Caller is responsible for +// removing the file after the test. +std::filesystem::path WriteTempYaml(const std::string& contents, const std::string& tag) { + auto path = std::filesystem::temp_directory_path() / + ("obk_lidar_test_" + tag + "_" + std::to_string(std::rand()) + ".yaml"); + std::ofstream out(path); + REQUIRE(out.is_open()); + out << contents; + out.close(); + return path; +} + +struct TempYaml { + std::filesystem::path path; + ~TempYaml() { std::filesystem::remove(path); } +}; + +} // namespace + +// ---------------------------------------------------------------------------- // +// LidarInterface // +// ---------------------------------------------------------------------------- // + +constexpr const char* kLidarYaml = R"( +site_name: head_lidar_site +geom_groups: [2] +frame: body +max_distance: 0.0 +pattern: + type: lidar_scan + vertical_fov_range: [0.0, 51.0] + horizontal_fov_range: [0.0, 360.0] + channels: 51 + horizontal_res: 1.0 + offset: + pos: [0.0, 0.0, 0.0] + rot: [1.0, 0.0, 0.0, 0.0] +)"; + +TEST_CASE("LidarInterface: load from file path", "[lidar][config_path]") { + TempYaml tmp{WriteTempYaml(kLidarYaml, "lidar")}; + + obelisk::LidarInterface lidar(tmp.path.string()); + + CHECK(lidar.get_site() == "head_lidar_site"); + CHECK(lidar.get_frame() == "body"); + CHECK(lidar.get_max_distance() == 0.0); + // 51 vertical channels x horizontal samples. With horizontal_res=1° over [0,360°], the loop + // emits 360 samples and drops the last one (wraparound at 0/360), leaving 359. + CHECK(lidar.get_image_height() == 51); + CHECK(lidar.get_image_width() == 359); + CHECK(lidar.get_num_rays() == 51 * 359); +} + +TEST_CASE("LidarInterface: load from YAML::Node (inline)", "[lidar][inline]") { + YAML::Node node = YAML::Load(kLidarYaml); + + obelisk::LidarInterface lidar(node); + + CHECK(lidar.get_site() == "head_lidar_site"); + CHECK(lidar.get_frame() == "body"); + CHECK(lidar.get_image_height() == 51); + CHECK(lidar.get_image_width() == 359); + CHECK(lidar.get_num_rays() == 51 * 359); +} + +TEST_CASE("LidarInterface: get_return passes distance through", "[lidar]") { + YAML::Node node = YAML::Load(kLidarYaml); + obelisk::LidarInterface lidar(node); + + std::array hit{1.2, 3.4, 5.6}; + CHECK(lidar.get_return(hit, 7.5f) == 7.5f); + CHECK(lidar.get_return(hit, -1.0f) == -1.0f); +} + +TEST_CASE("LidarInterface: missing required pattern fields throws", "[lidar][errors]") { + YAML::Node node = YAML::Load(R"( +site_name: lidar_site +pattern: + type: lidar_scan + vertical_fov_range: [0.0, 30.0] + channels: 4 + horizontal_res: 5.0 +)"); // missing horizontal_fov_range + + CHECK_THROWS(obelisk::LidarInterface{node}); +} + +// ---------------------------------------------------------------------------- // +// HeightScanInterface // +// ---------------------------------------------------------------------------- // + +constexpr const char* kHeightScanYaml = R"( +site_name: pelvis_mocap_site +geom_groups: [2] +pattern: + type: height_scan + ordering: xy + resolution: 0.1 + size: [1.6, 0.8] + offset: + pos: [0.08, 0.0, 20.0] +)"; + +TEST_CASE("HeightScanInterface: load from file path", "[height_scan][config_path]") { + TempYaml tmp{WriteTempYaml(kHeightScanYaml, "height")}; + + obelisk::HeightScanInterface hs(tmp.path.string()); + + CHECK(hs.get_site() == "pelvis_mocap_site"); + CHECK(hs.get_frame() == "world"); // default when frame not specified + // 1.6 / 0.1 + 1 = 17 columns; 0.8 / 0.1 + 1 = 9 rows + CHECK(hs.get_num_rays() == 17 * 9); +} + +TEST_CASE("HeightScanInterface: load from YAML::Node (inline)", "[height_scan][inline]") { + YAML::Node node = YAML::Load(kHeightScanYaml); + + obelisk::HeightScanInterface hs(node); + + CHECK(hs.get_site() == "pelvis_mocap_site"); + CHECK(hs.get_num_rays() == 17 * 9); +} + +TEST_CASE("HeightScanInterface: get_return returns hit-point z", "[height_scan]") { + YAML::Node node = YAML::Load(kHeightScanYaml); + obelisk::HeightScanInterface hs(node); + + std::array hit{0.1, 0.2, 4.25}; + CHECK(hs.get_return(hit, 99.0f) == 4.25f); // hit_point[2] returned, dist ignored +} + +TEST_CASE("HeightScanInterface: invalid ordering throws", "[height_scan][errors]") { + YAML::Node node = YAML::Load(R"( +site_name: hs +pattern: + ordering: zigzag + resolution: 0.1 + size: [1.0, 1.0] +)"); + + CHECK_THROWS(obelisk::HeightScanInterface{node}); +} + +// ---------------------------------------------------------------------------- // +// DepthInterface // +// ---------------------------------------------------------------------------- // + +constexpr const char* kDepthYaml = R"( +site_name: pelvis_mocap_site +geom_groups: [0, 1, 2] +frame: world +pattern: + type: depth_camera + width: 30 + height: 26 + intrinsic_matrix: [25.9928, 0.0, 14.9936, 0.0, 27.8703, 12.9966, 0.0, 0.0, 1.0] + offset: + pos: [0.153246, 0.0, 0.106799] + rot: [0.843391, 0.0, 0.537300, 0.0] +)"; + +TEST_CASE("DepthInterface: load from file path", "[depth][config_path]") { + TempYaml tmp{WriteTempYaml(kDepthYaml, "depth")}; + + obelisk::DepthInterface depth(tmp.path.string()); + + CHECK(depth.get_site() == "pelvis_mocap_site"); + CHECK(depth.get_frame() == "world"); + CHECK(depth.get_image_width() == 30); + CHECK(depth.get_image_height() == 26); + CHECK(depth.get_num_rays() == 30 * 26); +} + +TEST_CASE("DepthInterface: load from YAML::Node (inline)", "[depth][inline]") { + YAML::Node node = YAML::Load(kDepthYaml); + + obelisk::DepthInterface depth(node); + + CHECK(depth.get_image_width() == 30); + CHECK(depth.get_image_height() == 26); + CHECK(depth.get_num_rays() == 30 * 26); +} + +TEST_CASE("DepthInterface: missing intrinsic_matrix throws", "[depth][errors]") { + YAML::Node node = YAML::Load(R"( +site_name: cam +pattern: + width: 4 + height: 3 +)"); + + CHECK_THROWS(obelisk::DepthInterface{node}); +} + +// ---------------------------------------------------------------------------- // +// Common RayCasterInterface behavior // +// ---------------------------------------------------------------------------- // + +TEST_CASE("RayCasterInterface: max_distance clamps long hits", "[ray_caster][max_distance]") { + YAML::Node node = YAML::Load(R"( +site_name: lidar_site +max_distance: 5.0 +pattern: + type: lidar_scan + vertical_fov_range: [0.0, 0.0] + horizontal_fov_range: [0.0, 360.0] + channels: 1 + horizontal_res: 90.0 +)"); + + obelisk::LidarInterface lidar(node); + + CHECK(lidar.get_max_distance() == 5.0); + CHECK(lidar.apply_max_distance(3.0) == 3.0); // within range + CHECK(lidar.apply_max_distance(10.0) == -1.0); // beyond range -> sentinel + CHECK(lidar.apply_max_distance(-1.0) == -1.0); // existing no-hit unchanged +} + +TEST_CASE("RayCasterInterface: missing site_name throws", "[ray_caster][errors]") { + YAML::Node node = YAML::Load(R"( +pattern: + type: lidar_scan + vertical_fov_range: [0.0, 30.0] + horizontal_fov_range: [0.0, 90.0] + channels: 4 + horizontal_res: 5.0 +)"); + + CHECK_THROWS(obelisk::LidarInterface{node}); +} + +TEST_CASE("RayCasterInterface: file path constructor errors on missing file", "[ray_caster][errors]") { + CHECK_THROWS(obelisk::LidarInterface{std::string("/nonexistent/path/to/lidar.yaml")}); +} diff --git a/tests/tests_cpp/test_mujoco_sim_robot.cpp b/tests/tests_cpp/test_mujoco_sim_robot.cpp index d36a3037..e78ecd04 100644 --- a/tests/tests_cpp/test_mujoco_sim_robot.cpp +++ b/tests/tests_cpp/test_mujoco_sim_robot.cpp @@ -6,15 +6,38 @@ namespace obelisk { class ObeliskMujocoTester : public ObeliskMujocoRobot { public: ObeliskMujocoTester() : ObeliskMujocoRobot("obelisk_mujoco_tester") { - this->set_parameter(rclcpp::Parameter("timer_true_sim_state_setting", "topic:topic1,timer_period_sec:1")); - this->set_parameter(rclcpp::Parameter("pub_true_sim_state_setting", "topic:topic4")); - this->set_parameter(rclcpp::Parameter("callback_group_settings", "topic:topic2")); - this->set_parameter(rclcpp::Parameter( - "mujoco_setting", "model_xml_path:dummy/" - "dummy.xml,n_u:1,sensor_settings:[{group_name=group1|dt=0.01|topic=topic1|sensor_" - "type=jointpos|sensor_names=sensor_joint1&sensor_joint2+group_name=group2|topic=" - "topic2|dt=0.5|sensor_type=jointpos|sensor_names=sensor_joint3}]")); - this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic5")); + const std::string settings = R"( +publishers: + - key: pub_true_sim_state + topic: topic4 + history_depth: 10 +subscribers: + - key: sub_ctrl + topic: topic5 + history_depth: 10 +timers: + - key: timer_true_sim_state + timer_period_sec: 1.0 +)"; + this->set_parameter(rclcpp::Parameter("obelisk_settings", settings)); + + const std::string mujoco = R"( +model_xml_path: dummy/dummy.xml +n_u: 1 +sensor_settings: + - topic: topic1 + dt: 0.01 + msg_type: ObkJointEncoders + sensor_names: + sensor_joint1: jointpos + sensor_joint2: jointpos + - topic: topic2 + dt: 0.5 + msg_type: ObkJointEncoders + sensor_names: + sensor_joint3: jointpos +)"; + this->set_parameter(rclcpp::Parameter("mujoco_setting", mujoco)); } void Configure() { diff --git a/tests/tests_cpp/test_node.cpp b/tests/tests_cpp/test_node.cpp index 937d77cf..d550f6d8 100644 --- a/tests/tests_cpp/test_node.cpp +++ b/tests/tests_cpp/test_node.cpp @@ -1,41 +1,23 @@ #include -#include #include "obelisk_node.h" -#include "rcl_interfaces/msg/parameter_event.hpp" #include "std_msgs/msg/string.hpp" using std::placeholders::_1; namespace obelisk { - // Helper class for testing + /** + * Minimal subclass that lets us register components and exercise the new YAML-driven flow. + */ class ObeliskNodeTester : public ObeliskNode { public: - ObeliskNodeTester() : ObeliskNode("obelisk_tester") { - this->set_parameter(rclcpp::Parameter("callback_group_settings", "my_cbg:None")); - - on_configure(this->get_current_state()); - } - - template void SubscriptionConfigStrTester(const std::string& config) { - CreateSubscriptionFromConfigStr( - config, std::bind(&ObeliskNodeTester::GenericCallback, this, _1)); - } - - template void PublishConfigStrTester(const std::string& config) { - CreatePublisherFromConfigStr(config); - } - - void TimerConfigFromStrTester(const std::string& config) { - CreateWallTimerFromConfigStr(config, std::bind(&ObeliskNodeTester::SimpleCallback, this)); - } - void SimpleCallback() {} - - // Templated generic callback to facilitate easy testing - template void GenericCallback(const MessageT& msg) {} + ObeliskNodeTester() : ObeliskNode("obelisk_tester") {} + // expose for tests using ObeliskNode::create_publisher; using ObeliskNode::create_subscription; + + template void GenericCallback(const MessageT& msg) {} }; } // namespace obelisk @@ -47,39 +29,19 @@ TEST_CASE("Obelisk Node Pub and Sub", "[obelisk_node]") { obelisk::ObeliskNodeTester node; - SECTION("Publishers") { + SECTION("Direct publishers") { REQUIRE_NOTHROW(node.create_publisher("topic", 10)); - - // Verify that allowed messages are fine REQUIRE_NOTHROW(node.create_publisher("topic", 10)); REQUIRE_NOTHROW(node.create_publisher("topic", 10)); REQUIRE_NOTHROW(node.create_publisher("topic", 10)); REQUIRE_NOTHROW(node.create_publisher("topic", 10)); } - SECTION("Subscribers") { - // Verify that allowed messages are fine + SECTION("Direct subscribers") { REQUIRE_NOTHROW(node.create_subscription( "topic", 10, std::bind(&obelisk::ObeliskNodeTester::GenericCallback, &node, _1))); - REQUIRE_NOTHROW(node.create_subscription( - "topic", 10, - std::bind(&obelisk::ObeliskNodeTester::GenericCallback, &node, - _1))); - REQUIRE_NOTHROW(node.create_subscription( - "topic", 10, - std::bind(&obelisk::ObeliskNodeTester::GenericCallback, &node, - _1))); - REQUIRE_NOTHROW(node.create_subscription( - "topic", 10, - std::bind(&obelisk::ObeliskNodeTester::GenericCallback, &node, - _1))); - REQUIRE_NOTHROW(node.create_subscription( - "topic", 10, - std::bind(&obelisk::ObeliskNodeTester::GenericCallback, &node, _1))); - - // Try with the override flag REQUIRE_NOTHROW(node.create_subscription( "topic", 10, std::bind(&obelisk::ObeliskNodeTester::GenericCallback, &node, _1))); } @@ -87,55 +49,67 @@ TEST_CASE("Obelisk Node Pub and Sub", "[obelisk_node]") { rclcpp::shutdown(); } -TEST_CASE("Components from string", "[obelisk_node]") { +TEST_CASE("Obelisk Node configures pub/sub/timer from obelisk_settings YAML", "[obelisk_node]") { rclcpp::init(0, nullptr); obelisk::ObeliskNodeTester node; - SECTION("Subscribers") { - // Check with good config string - REQUIRE_NOTHROW( - node.SubscriptionConfigStrTester("topic:test1,depth:10")); + // Register a publisher, subscription, and timer by key. + node.RegisterObkPublisher("pub_x"); + node.RegisterObkSubscription( + "sub_x", std::bind(&obelisk::ObeliskNodeTester::GenericCallback, + &node, _1)); + bool fired = false; + node.RegisterObkTimer("timer_x", [&fired]() { fired = true; }); + + const std::string settings = R"( +publishers: + - key: pub_x + topic: /test/pub + history_depth: 7 +subscribers: + - key: sub_x + topic: /test/sub + history_depth: 3 +timers: + - key: timer_x + timer_period_sec: 0.01 +)"; + node.set_parameter(rclcpp::Parameter("obelisk_settings", settings)); + + REQUIRE(node.on_configure(node.get_current_state()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + auto pub = node.GetPublisher("pub_x"); + REQUIRE(pub != nullptr); + CHECK(pub->get_actual_qos().depth() == 7); + + auto sub = node.GetSubscription("sub_x"); + REQUIRE(sub != nullptr); + CHECK(sub->get_actual_qos().depth() == 3); + + REQUIRE(node.GetTimer("timer_x") != nullptr); - REQUIRE_NOTHROW(node.SubscriptionConfigStrTester( - "topic:test1,depth:10,non_obelisk:true")); - - // Check without topic - REQUIRE_THROWS(node.SubscriptionConfigStrTester("depth:10")); - - // Check when missing a colon - REQUIRE_THROWS( - node.SubscriptionConfigStrTester("topic:test1,depth 10")); - } - - SECTION("Publishers") { - // Check with good config string - REQUIRE_NOTHROW( - node.PublishConfigStrTester("topic:test1,depth:10")); - - REQUIRE_NOTHROW(node.PublishConfigStrTester( - "topic:test1,depth:10,non_obelisk:true")); - - // Check without topic - REQUIRE_THROWS(node.PublishConfigStrTester("depth:10")); + rclcpp::shutdown(); +} - // Check when missing a colon - REQUIRE_THROWS( - node.PublishConfigStrTester("topic:test1,depth 10")); - } +TEST_CASE("Empty obelisk_settings is a no-op (no components registered)", "[obelisk_node]") { + rclcpp::init(0, nullptr); - SECTION("Timers") { - // Check with good config string - REQUIRE_NOTHROW(node.TimerConfigFromStrTester("timer_period_sec:2")); + obelisk::ObeliskNodeTester node; + // Default value of obelisk_settings is "" (empty string). + REQUIRE(node.on_configure(node.get_current_state()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - REQUIRE_NOTHROW(node.TimerConfigFromStrTester("timer_period_sec:.5")); + rclcpp::shutdown(); +} - // // Check without period - REQUIRE_THROWS(node.TimerConfigFromStrTester("depth:10")); +TEST_CASE("Malformed YAML in obelisk_settings raises", "[obelisk_node]") { + rclcpp::init(0, nullptr); - // // Check when missing a colon - REQUIRE_THROWS(node.TimerConfigFromStrTester("timer_period_sec:2, test")); - } + obelisk::ObeliskNodeTester node; + node.set_parameter(rclcpp::Parameter("obelisk_settings", std::string("publishers: [unterminated"))); + REQUIRE_THROWS(node.on_configure(node.get_current_state())); rclcpp::shutdown(); } diff --git a/tests/tests_cpp/test_robot.cpp b/tests/tests_cpp/test_robot.cpp index 0b0f8faa..35dd4624 100644 --- a/tests/tests_cpp/test_robot.cpp +++ b/tests/tests_cpp/test_robot.cpp @@ -6,8 +6,13 @@ namespace obelisk { class ObeliskRobotTester : public ObeliskRobot { public: ObeliskRobotTester() : ObeliskRobot("obelisk_sensor_tester") { - this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic4")); - this->set_parameter(rclcpp::Parameter("callback_group_settings", "")); + const std::string settings = R"( +subscribers: + - key: sub_ctrl + topic: topic4 + history_depth: 10 +)"; + this->set_parameter(rclcpp::Parameter("obelisk_settings", settings)); } void Configure() { diff --git a/tests/tests_cpp/test_sensor.cpp b/tests/tests_cpp/test_sensor.cpp index e8cc891d..5bdf9c57 100644 --- a/tests/tests_cpp/test_sensor.cpp +++ b/tests/tests_cpp/test_sensor.cpp @@ -6,7 +6,7 @@ namespace obelisk { class ObeliskSensorTester : public ObeliskSensor { public: ObeliskSensorTester() : ObeliskSensor("obelisk_sensor_tester") { - this->set_parameter(rclcpp::Parameter("callback_group_settings", "")); + this->set_parameter(rclcpp::Parameter("obelisk_settings", "")); } void Configure() { diff --git a/tests/tests_cpp/test_sim_robot.cpp b/tests/tests_cpp/test_sim_robot.cpp index 3d3eee62..87c9a249 100644 --- a/tests/tests_cpp/test_sim_robot.cpp +++ b/tests/tests_cpp/test_sim_robot.cpp @@ -6,10 +6,20 @@ namespace obelisk { class ObeliskSimRobotTester : public ObeliskSimRobot { public: ObeliskSimRobotTester() : ObeliskSimRobot("obelisk_sim_robot_tester") { - this->set_parameter(rclcpp::Parameter("timer_true_sim_state_setting", "topic:topic3,timer_period_sec:1")); - this->set_parameter(rclcpp::Parameter("pub_true_sim_state_setting", "topic:topic4")); - this->set_parameter(rclcpp::Parameter("callback_group_settings", "topic:topic2")); - this->set_parameter(rclcpp::Parameter("sub_ctrl_setting", "topic:topic5")); + const std::string settings = R"( +publishers: + - key: pub_true_sim_state + topic: topic4 + history_depth: 10 +subscribers: + - key: sub_ctrl + topic: topic5 + history_depth: 10 +timers: + - key: timer_true_sim_state + timer_period_sec: 1.0 +)"; + this->set_parameter(rclcpp::Parameter("obelisk_settings", settings)); } void Configure() { diff --git a/tests/tests_cpp/test_viz_robot.cpp b/tests/tests_cpp/test_viz_robot.cpp index fc182f67..4aab059f 100644 --- a/tests/tests_cpp/test_viz_robot.cpp +++ b/tests/tests_cpp/test_viz_robot.cpp @@ -14,10 +14,20 @@ namespace obelisk::viz { data_path += "/tests/tests_cpp/test_data/r2d2.urdf"; std::cout << "path: " << data_path << std::endl; this->set_parameter(rclcpp::Parameter("urdf_path_param", data_path)); - this->set_parameter(rclcpp::Parameter("pub_viz_joint_setting", "topic:topic1")); - this->set_parameter(rclcpp::Parameter("sub_viz_est_setting", "topic:topic2")); - this->set_parameter(rclcpp::Parameter("timer_viz_joint_setting", "timer_period_sec:1")); - this->set_parameter(rclcpp::Parameter("callback_group_settings", "")); + const std::string settings = R"( +publishers: + - key: pub_viz_joint + topic: topic1 + history_depth: 10 +subscribers: + - key: sub_est + topic: topic2 + history_depth: 10 +timers: + - key: time_joints + timer_period_sec: 1.0 +)"; + this->set_parameter(rclcpp::Parameter("obelisk_settings", settings)); } void Configure() { diff --git a/tests/tests_cpp/test_yaml_to_node.cpp b/tests/tests_cpp/test_yaml_to_node.cpp new file mode 100644 index 00000000..e9bfe200 --- /dev/null +++ b/tests/tests_cpp/test_yaml_to_node.cpp @@ -0,0 +1,121 @@ +// Behavioral safety-net tests for the C++ side of the YAML-config -> ROS-parameter -> running-node pipeline. +// +// These tests pin down the *observable* properties of an Obelisk node configured from the single +// ``obelisk_settings`` ROS parameter (a YAML string): topic names, history depth (QoS), and msg-type +// wiring. They mirror the safety-net Python tests in tests/tests_python/tests_core/test_yaml_to_node.py. + +#include + +#include "obelisk_controller.h" +#include "obelisk_estimator.h" + +namespace obelisk { + + class SafetyNetController + : public ObeliskController { + public: + SafetyNetController() : ObeliskController("safety_net_controller") {} + + void ApplySettings(const std::string& yaml_settings) { + this->set_parameter(rclcpp::Parameter("obelisk_settings", yaml_settings)); + } + + // expose protected key fields for assertions + using ObeliskController::ctrl_key_; + using ObeliskController::est_key_; + using ObeliskController::timer_key_; + + protected: + void UpdateXHat(const obelisk_estimator_msgs::msg::EstimatedState&) override {} + obelisk_control_msgs::msg::PositionSetpoint ComputeControl() override { + return obelisk_control_msgs::msg::PositionSetpoint{}; + } + }; + + class SafetyNetEstimator : public ObeliskEstimator { + public: + SafetyNetEstimator() : ObeliskEstimator("safety_net_estimator") {} + + void ApplySettings(const std::string& yaml_settings) { + this->set_parameter(rclcpp::Parameter("obelisk_settings", yaml_settings)); + } + + using ObeliskEstimator::est_pub_key_; + using ObeliskEstimator::est_timer_key_; + + protected: + obelisk_estimator_msgs::msg::EstimatedState ComputeStateEstimate() override { + return obelisk_estimator_msgs::msg::EstimatedState{}; + } + }; + +} // namespace obelisk + +// ------------------------------------------------------------------ // +// Tests // +// ------------------------------------------------------------------ // + +TEST_CASE("Controller YAML-derived params produce the configured pub/sub/timer", "[safety_net][controller]") { + rclcpp::init(0, nullptr); + + obelisk::SafetyNetController node; + node.ApplySettings(R"( +publishers: + - key: pub_ctrl + topic: /safety_net/ctrl + history_depth: 7 +subscribers: + - key: sub_est + topic: /safety_net/est + history_depth: 3 +timers: + - key: timer_ctrl + timer_period_sec: 0.005 +)"); + + REQUIRE(node.on_configure(node.get_current_state()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + auto pub = node.GetPublisher(node.ctrl_key_); + REQUIRE(pub != nullptr); + CHECK(std::string(pub->get_topic_name()).find("/safety_net/ctrl") != std::string::npos); + CHECK(pub->get_actual_qos().depth() == 7); + + auto sub = node.GetSubscription(node.est_key_); + REQUIRE(sub != nullptr); + CHECK(std::string(sub->get_topic_name()).find("/safety_net/est") != std::string::npos); + CHECK(sub->get_actual_qos().depth() == 3); + + auto timer = node.GetTimer(node.timer_key_); + REQUIRE(timer != nullptr); + + rclcpp::shutdown(); +} + +TEST_CASE("Estimator YAML-derived params expose configured publisher topic + depth", "[safety_net][estimator]") { + rclcpp::init(0, nullptr); + + obelisk::SafetyNetEstimator node; + node.ApplySettings(R"( +publishers: + - key: pub_est + topic: /safety_net/est_out + history_depth: 5 +timers: + - key: timer_est + timer_period_sec: 0.02 +)"); + + REQUIRE(node.on_configure(node.get_current_state()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + auto pub = node.GetPublisher(node.est_pub_key_); + REQUIRE(pub != nullptr); + CHECK(std::string(pub->get_topic_name()).find("/safety_net/est_out") != std::string::npos); + CHECK(pub->get_actual_qos().depth() == 5); + + auto timer = node.GetTimer(node.est_timer_key_); + REQUIRE(timer != nullptr); + + rclcpp::shutdown(); +} diff --git a/tests/tests_python/test_assets/compose_a.yaml b/tests/tests_python/test_assets/compose_a.yaml new file mode 100644 index 00000000..e70d50e3 --- /dev/null +++ b/tests/tests_python/test_assets/compose_a.yaml @@ -0,0 +1,9 @@ +# Composition unit-test fixture: top-level entry that includes B and adds a control entry. +config: composed_a +include: + - compose_b.yaml +control: + - pkg: a_pkg + executable: a_ctrl +joystick: + pub_topic: /from_a diff --git a/tests/tests_python/test_assets/compose_b.yaml b/tests/tests_python/test_assets/compose_b.yaml new file mode 100644 index 00000000..e09fc51e --- /dev/null +++ b/tests/tests_python/test_assets/compose_b.yaml @@ -0,0 +1,11 @@ +# Composition unit-test fixture: middle layer that includes C and adds another control entry. +config: composed_b +include: + - compose_c.yaml +control: + - pkg: b_pkg + executable: b_ctrl +joystick: + on: True + pub_topic: /from_b + sub_topic: /from_b_sub diff --git a/tests/tests_python/test_assets/compose_c.yaml b/tests/tests_python/test_assets/compose_c.yaml new file mode 100644 index 00000000..74e1f1a2 --- /dev/null +++ b/tests/tests_python/test_assets/compose_c.yaml @@ -0,0 +1,8 @@ +# Composition unit-test fixture: leaf in the include chain. +config: composed_c +control: + - pkg: c_pkg + executable: c_ctrl +estimation: + - pkg: c_est_pkg + executable: c_est diff --git a/tests/tests_python/test_assets/compose_cycle_a.yaml b/tests/tests_python/test_assets/compose_cycle_a.yaml new file mode 100644 index 00000000..6ba705a5 --- /dev/null +++ b/tests/tests_python/test_assets/compose_cycle_a.yaml @@ -0,0 +1,3 @@ +# Cycle-detection fixture: includes compose_cycle_b which includes us back. +include: + - compose_cycle_b.yaml diff --git a/tests/tests_python/test_assets/compose_cycle_b.yaml b/tests/tests_python/test_assets/compose_cycle_b.yaml new file mode 100644 index 00000000..f6be60aa --- /dev/null +++ b/tests/tests_python/test_assets/compose_cycle_b.yaml @@ -0,0 +1,3 @@ +# Cycle-detection fixture: closes the loop back to compose_cycle_a. +include: + - compose_cycle_a.yaml diff --git a/tests/tests_python/test_assets/test_config.yaml b/tests/tests_python/test_assets/test_config.yaml index 1442ac2e..c51d9778 100644 --- a/tests/tests_python/test_assets/test_config.yaml +++ b/tests/tests_python/test_assets/test_config.yaml @@ -1,63 +1,62 @@ -onboard: - control: - - pkg: test_pkg1 - executable: test_controller - callback_groups: - cbg1: MutuallyExclusiveCallbackGroup - cbg2: ReentrantCallbackGroup - publishers: - - ros_parameter: pub_ctrl_settings1 - key: pub1 - topic: /test/controller/pub1 - msg_type: TestMsg - history_depth: 10 - callback_group: cbg1 - non_obelisk: False - subscribers: - - ros_parameter: sub_ctrl_settings1 - key: sub1 - topic: /test/controller/sub1 - msg_type: TestMsg - history_depth: 5 - callback_key: sub_callback1 - callback_group: cbg2 - non_obelisk: False - timers: - - ros_parameter: timer_ctrl_settings1 - key: timer1 - timer_period_sec: 0.1 - callback_group: cbg1 - callback_key: timer_callback1 - estimation: - - pkg: test_pkg2 - executable: test_estimator - callback_groups: - cbg1: ReentrantCallbackGroup - robot: - - pkg: test_pkg3 - executable: test_robot - callback_groups: - cbg1: MutuallyExclusiveCallbackGroup - sensing: - - pkg: test_pkg4 - executable: test_sensor1 - callback_groups: - cbg1: MutuallyExclusiveCallbackGroup - publishers: - - ros_parameter: pub_sensor_settings1 - key: pub1 - topic: /test/sensor1/pub1 - msg_type: SensorMsg - history_depth: 10 - callback_group: cbg1 - non_obelisk: False - - pkg: test_pkg5 - executable: test_sensor2 - callback_groups: - cbg1: ReentrantCallbackGroup - timers: - - ros_parameter: timer_sensor_settings1 - key: timer1 - timer_period_sec: 0.01 - callback_group: cbg1 - callback_key: timer_callback1 +control: + - pkg: test_pkg1 + executable: test_controller + callback_groups: + cbg1: MutuallyExclusiveCallbackGroup + cbg2: ReentrantCallbackGroup + publishers: + - ros_parameter: pub_ctrl_settings1 + key: pub1 + topic: /test/controller/pub1 + msg_type: TestMsg + history_depth: 10 + callback_group: cbg1 + non_obelisk: False + subscribers: + - ros_parameter: sub_ctrl_settings1 + key: sub1 + topic: /test/controller/sub1 + msg_type: TestMsg + history_depth: 5 + callback_key: sub_callback1 + callback_group: cbg2 + non_obelisk: False + timers: + - ros_parameter: timer_ctrl_settings1 + key: timer1 + timer_period_sec: 0.1 + callback_group: cbg1 + callback_key: timer_callback1 +estimation: + - pkg: test_pkg2 + executable: test_estimator + callback_groups: + cbg1: ReentrantCallbackGroup +robot: + - pkg: test_pkg3 + executable: test_robot + callback_groups: + cbg1: MutuallyExclusiveCallbackGroup +sensing: + - pkg: test_pkg4 + executable: test_sensor1 + callback_groups: + cbg1: MutuallyExclusiveCallbackGroup + publishers: + - ros_parameter: pub_sensor_settings1 + key: pub1 + topic: /test/sensor1/pub1 + msg_type: SensorMsg + history_depth: 10 + callback_group: cbg1 + non_obelisk: False + - pkg: test_pkg5 + executable: test_sensor2 + callback_groups: + cbg1: ReentrantCallbackGroup + timers: + - ros_parameter: timer_sensor_settings1 + key: timer1 + timer_period_sec: 0.01 + callback_group: cbg1 + callback_key: timer_callback1 diff --git a/tests/tests_python/tests_core/test_control.py b/tests/tests_python/tests_core/test_control.py index 0d0103d1..a0d5857a 100644 --- a/tests/tests_python/tests_core/test_control.py +++ b/tests/tests_python/tests_core/test_control.py @@ -1,6 +1,7 @@ from typing import Any, Callable, Generator import pytest +import yaml from rclpy.publisher import Publisher from rclpy.subscription import Subscription from rclpy.timer import Timer @@ -51,9 +52,11 @@ def test_controller_initialization(test_controller: TestController) -> None: test_controller: An instance of TestController. """ assert test_controller.get_name() == "test_controller" - assert test_controller.has_parameter("timer_ctrl_setting") - assert test_controller.has_parameter("pub_ctrl_setting") - assert test_controller.has_parameter("sub_est_setting") + assert test_controller.has_parameter("obelisk_settings") + # Internal registration table is populated for all three components. + assert any(s["key"] == "timer_ctrl" for s in test_controller._obk_timer_settings) + assert any(s["key"] == "pub_ctrl" for s in test_controller._obk_pub_settings) + assert any(s["key"] == "sub_est" for s in test_controller._obk_sub_settings) def test_timer_registration(test_controller: TestController) -> None: @@ -91,14 +94,12 @@ def test_controller_configuration(test_controller: TestController, set_node_para test_controller: An instance of TestController. set_node_parameters: A fixture to set node parameters. """ - set_node_parameters( - test_controller, - { - "timer_ctrl_setting": "timer_period_sec:0.1", - "pub_ctrl_setting": "topic:/test_control,msg_type:PositionSetpoint", - "sub_est_setting": "topic:/test_estimate,msg_type:EstimatedState", - }, - ) + settings = { + "timers": [{"key": "timer_ctrl", "timer_period_sec": 0.1}], + "publishers": [{"key": "pub_ctrl", "topic": "/test_control", "history_depth": 10}], + "subscribers": [{"key": "sub_est", "topic": "/test_estimate", "history_depth": 10}], + } + set_node_parameters(test_controller, {"obelisk_settings": yaml.safe_dump(settings)}) test_controller.on_configure(None) assert "timer_ctrl" in test_controller.obk_timers diff --git a/tests/tests_python/tests_core/test_estimation.py b/tests/tests_python/tests_core/test_estimation.py index 12328b7f..fea638fa 100644 --- a/tests/tests_python/tests_core/test_estimation.py +++ b/tests/tests_python/tests_core/test_estimation.py @@ -1,7 +1,9 @@ +"""Tests for ObeliskEstimator (post-config-string-refactor).""" + from typing import Any, Callable, Generator, Type import pytest -import rclpy +import yaml from obelisk_sensor_msgs.msg import ObkJointEncoders from rclpy.lifecycle.node import TransitionCallbackReturn from rclpy.publisher import Publisher @@ -11,10 +13,6 @@ from obelisk_py.core.estimation import ObeliskEstimator from obelisk_py.core.obelisk_typing import ObeliskEstimatorMsg, ObeliskSensorMsg -# ##### # -# SETUP # -# ##### # - class TestEstimator(ObeliskEstimator): """A concrete implementation of ObeliskEstimator for testing purposes.""" @@ -23,10 +21,9 @@ def __init__(self, node_name: str, est_msg_type: Type) -> None: """Initialize the TestEstimator.""" super().__init__(node_name, est_msg_type) self.register_obk_subscription( - "sub_sensor_setting", - self.update_sensor, - ObkJointEncoders, key="sub_sensor", + callback=self.update_sensor, + msg_type=ObkJointEncoders, ) def update_sensor(self, sensor_msg: ObeliskSensorMsg) -> None: @@ -46,69 +43,32 @@ def test_estimator(ros_context: Any) -> Generator[TestEstimator, None, None]: estimator.destroy_node() -# ##### # -# TESTS # -# ##### # - - def test_estimator_initialization(test_estimator: TestEstimator) -> None: - """Test the initialization of the ObeliskEstimator. - - This test checks if the estimator is properly initialized with the correct name and if the required timers, - publishers, and subscriptions are registered. - - Parameters: - test_estimator: An instance of TestEstimator. - """ + """The estimator is named correctly and declares the obelisk_settings parameter.""" assert test_estimator.get_name() == "test_estimator" - assert test_estimator.has_parameter("timer_est_setting") - assert test_estimator.has_parameter("pub_est_setting") - assert test_estimator.has_parameter("sub_sensor_setting") + assert test_estimator.has_parameter("obelisk_settings") def test_timer_registration(test_estimator: TestEstimator) -> None: - """Test the registration of the estimation timer. - - This test verifies that the estimation timer is properly registered with the correct key and callback. - - Parameters: - test_estimator: An instance of TestEstimator. - """ + """The compute_state_estimate timer is registered under key 'timer_est'.""" timer_setting = next(s for s in test_estimator._obk_timer_settings if s["key"] == "timer_est") assert timer_setting["callback"] == test_estimator.compute_state_estimate def test_subscription_registration(test_estimator: TestEstimator) -> None: - """Test the registration of the sensor subscription. - - This test verifies that the sensor subscription is properly registered with the correct key, callback, and - message type. - - Parameters: - test_estimator: An instance of TestEstimator. - """ + """The sensor subscription is registered under key 'sub_sensor' with the right callback.""" sub_setting = next(s for s in test_estimator._obk_sub_settings if s["key"] == "sub_sensor") assert sub_setting["callback"] == test_estimator.update_sensor def test_estimator_configuration(test_estimator: TestEstimator, set_node_parameters: Callable) -> None: - """Test the configuration of the ObeliskEstimator. - - This test verifies that the estimator can be properly configured with the necessary parameters and that the - timers, publishers, and subscriptions are created correctly. - - Parameters: - test_estimator: An instance of TestEstimator. - set_node_parameters: A fixture to set node parameters. - """ - set_node_parameters( - test_estimator, - { - "timer_est_setting": "timer_period_sec:0.1", - "pub_est_setting": "topic:/test_estimate,msg_type:EstimatedState", - "sub_sensor_setting": "topic:/test_sensor", - }, - ) + """The estimator wires up timer/publisher/subscriber from obelisk_settings.""" + settings = { + "timers": [{"key": "timer_est", "timer_period_sec": 0.1}], + "publishers": [{"key": "pub_est", "topic": "/test_estimate", "history_depth": 10}], + "subscribers": [{"key": "sub_sensor", "topic": "/test_sensor", "history_depth": 10}], + } + set_node_parameters(test_estimator, {"obelisk_settings": yaml.safe_dump(settings)}) result = test_estimator.on_configure(None) assert result == TransitionCallbackReturn.SUCCESS @@ -120,11 +80,7 @@ def test_estimator_configuration(test_estimator: TestEstimator, set_node_paramet def test_abstract_methods() -> None: - """Test the abstract methods of ObeliskEstimator. - - This test verifies that the abstract method compute_state_estimate is properly defined and must be implemented - by subclasses. - """ + """ObeliskEstimator's abstract methods must be implemented by subclasses.""" with pytest.raises(TypeError): class IncompleteEstimator(ObeliskEstimator): @@ -138,18 +94,3 @@ def compute_state_estimate(self) -> ObeliskEstimatorMsg: complete_estimator = CompleteEstimator("complete_estimator", String) assert hasattr(complete_estimator, "compute_state_estimate") - - -def test_missing_sensor_subscriber() -> None: - """Test the requirement for at least one sensor subscriber. - - This test verifies that an assertion error is raised when no sensor subscriber is registered. - """ - - class NoSensorEstimator(ObeliskEstimator): - def compute_state_estimate(self) -> ObeliskEstimatorMsg: - return ObeliskEstimatorMsg() - - estimator = NoSensorEstimator("no_sensor_estimator", String) - with pytest.raises(rclpy.exceptions.ParameterUninitializedException): - estimator.on_configure(None) diff --git a/tests/tests_python/tests_core/test_node.py b/tests/tests_python/tests_core/test_node.py index 841ee4c2..2f6d9d7b 100644 --- a/tests/tests_python/tests_core/test_node.py +++ b/tests/tests_python/tests_core/test_node.py @@ -1,7 +1,14 @@ +"""Tests for ObeliskNode (post-config-string-refactor). + +These exercise the new ``obelisk_settings`` YAML-string parameter and the simplified +``register_obk_*(key, ...)`` API. +""" + from typing import Any, Callable, Generator import obelisk_control_msgs.msg as ocm import pytest +import yaml from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.lifecycle.node import TransitionCallbackReturn from rclpy.publisher import Publisher @@ -11,10 +18,6 @@ from obelisk_py.core.node import ObeliskNode -# ##### # -# SETUP # -# ##### # - @pytest.fixture def test_node(ros_context: Any) -> Generator[ObeliskNode, None, None]: @@ -24,92 +27,54 @@ def test_node(ros_context: Any) -> Generator[ObeliskNode, None, None]: node.destroy_node() -# ##### # -# TESTS # -# ##### # +def _set_obelisk_settings(node: ObeliskNode, settings: dict, set_node_parameters: Callable) -> None: + """Serialize ``settings`` to YAML and assign it to the node's ``obelisk_settings`` parameter.""" + set_node_parameters(node, {"obelisk_settings": yaml.safe_dump(settings)}) def test_node_initialization(test_node: ObeliskNode) -> None: - """Test the initialization of the ObeliskNode. - - This test checks if the node is properly initialized with the correct name and if the callback_group_settings - parameter is declared. - - Parameters: - test_node: An instance of ObeliskNode. - """ + """The node declares the single ``obelisk_settings`` parameter and the user ``params_path`` slot.""" assert test_node.get_name() == "test_node_exclusivity" - assert test_node.has_parameter("callback_group_settings") + assert test_node.has_parameter("obelisk_settings") + assert test_node.has_parameter("params_path") def test_register_obk_publisher(test_node: ObeliskNode) -> None: - """Test the registration of an Obelisk publisher. - - This test verifies that a publisher can be registered with the node and that the appropriate ROS parameter is - declared. - - Parameters: - test_node: An instance of ObeliskNode. - """ - test_node.register_obk_publisher("test_pub_param", String, key="test_pub") - assert test_node.has_parameter("test_pub_param") + """register_obk_publisher records the registration without declaring a per-component parameter.""" + test_node.register_obk_publisher(key="test_pub", msg_type=String) + assert any(s["key"] == "test_pub" for s in test_node._obk_pub_settings) + # No legacy *_setting parameter should be declared anymore. + assert not test_node.has_parameter("test_pub_setting") def test_register_obk_subscription(test_node: ObeliskNode) -> None: - """Test the registration of an Obelisk subscription. - - This test verifies that a subscription can be registered with the node and that the appropriate ROS parameter is - declared. - - Parameters: - test_node: An instance of ObeliskNode. - """ + """register_obk_subscription stores the callback and msg type, no extra ROS param declared.""" def callback(msg: String) -> None: pass - test_node.register_obk_subscription("test_sub_param", callback, String, key="test_sub") - assert test_node.has_parameter("test_sub_param") + test_node.register_obk_subscription(key="test_sub", callback=callback, msg_type=String) + assert any(s["key"] == "test_sub" and s["callback"] is callback for s in test_node._obk_sub_settings) def test_register_obk_timer(test_node: ObeliskNode) -> None: - """Test the registration of an Obelisk timer. - - This test verifies that a timer can be registered with the node and that the appropriate ROS parameter is declared. - - Parameters: - test_node: An instance of ObeliskNode. - """ + """register_obk_timer stores the callback, no extra ROS param declared.""" def callback() -> None: pass - test_node.register_obk_timer("test_timer_param", callback, key="test_timer") - assert test_node.has_parameter("test_timer_param") + test_node.register_obk_timer(key="test_timer", callback=callback) + assert any(s["key"] == "test_timer" and s["callback"] is callback for s in test_node._obk_timer_settings) def test_create_publisher(test_node: ObeliskNode) -> None: - """Test the creation of a publisher. - - This test verifies that a publisher can be created with an Obelisk message type and that an ObeliskMsgError is - raised for non-Obelisk message types. - - Parameters: - test_node: An instance of ObeliskNode. - """ + """The underlying create_publisher still works for direct use cases.""" pub = test_node.create_publisher(ocm.PositionSetpoint, "test_topic", 10) assert isinstance(pub, Publisher) def test_create_subscription(test_node: ObeliskNode) -> None: - """Test the creation of a subscription. - - This test verifies that a subscription can be created with an Obelisk message type and that an ObeliskMsgError is - raised for non-Obelisk message types. - - Parameters: - test_node: An instance of ObeliskNode. - """ + """The underlying create_subscription still works for direct use cases.""" def callback(msg: ocm.PositionSetpoint) -> None: pass @@ -119,14 +84,7 @@ def callback(msg: ocm.PositionSetpoint) -> None: def test_lifecycle_callbacks(test_node: ObeliskNode) -> None: - """Test the lifecycle callbacks of the ObeliskNode. - - This test verifies that the lifecycle callbacks (on_configure, on_activate, on_deactivate, on_cleanup, on_shutdown) - return the expected TransitionCallbackReturn. - - Parameters: - test_node: An instance of ObeliskNode. - """ + """All lifecycle callbacks return SUCCESS for an empty ObeliskNode.""" assert test_node.on_configure(None) == TransitionCallbackReturn.SUCCESS assert test_node.on_activate(None) == TransitionCallbackReturn.SUCCESS assert test_node.on_deactivate(None) == TransitionCallbackReturn.SUCCESS @@ -135,78 +93,82 @@ def test_lifecycle_callbacks(test_node: ObeliskNode) -> None: def test_callback_group_creation(test_node: ObeliskNode, set_node_parameters: Callable) -> None: - """Test the creation of callback groups. - - This test verifies that callback groups can be created from a configuration string. - - Parameters: - test_node: An instance of ObeliskNode. - set_node_parameters: A fixture to set node parameters. - """ - set_node_parameters( - test_node, {"callback_group_settings": "group1:MutuallyExclusiveCallbackGroup,group2:ReentrantCallbackGroup"} + """Callback groups specified in obelisk_settings are instantiated and bound as attributes.""" + _set_obelisk_settings( + test_node, + { + "callback_groups": { + "group1": "MutuallyExclusiveCallbackGroup", + "group2": "ReentrantCallbackGroup", + } + }, + set_node_parameters, ) test_node.on_configure(None) - assert hasattr(test_node, "group1") assert isinstance(test_node.group1, MutuallyExclusiveCallbackGroup) - assert hasattr(test_node, "group2") assert isinstance(test_node.group2, ReentrantCallbackGroup) -def test_publisher_creation_from_config(test_node: ObeliskNode, set_node_parameters: Callable) -> None: - """Test the creation of a publisher from a configuration string. - - This test verifies that a publisher can be created from a configuration string. - - Parameters: - test_node: An instance of ObeliskNode. - set_node_parameters: A fixture to set node parameters. - """ - test_node.register_obk_publisher("test_pub_param", ocm.PositionSetpoint, key="test_pub") - set_node_parameters(test_node, {"test_pub_param": "topic:/test_topic,msg_type:PositionSetpoint,history_depth:10"}) +def test_publisher_creation_from_settings(test_node: ObeliskNode, set_node_parameters: Callable) -> None: + """A publisher entry in obelisk_settings.publishers becomes a real rclpy Publisher.""" + test_node.register_obk_publisher(key="test_pub", msg_type=ocm.PositionSetpoint) + _set_obelisk_settings( + test_node, + { + "publishers": [ + {"key": "test_pub", "topic": "/test_topic", "history_depth": 10}, + ] + }, + set_node_parameters, + ) test_node.on_configure(None) assert "test_pub" in test_node.obk_publishers assert isinstance(test_node.obk_publishers["test_pub"], Publisher) -def test_subscription_creation_from_config(test_node: ObeliskNode, set_node_parameters: Callable) -> None: - """Test the creation of a subscription from a configuration string. - - This test verifies that a subscription can be created from a configuration string. - - Parameters: - test_node: An instance of ObeliskNode. - set_node_parameters: A fixture to set node parameters. - """ +def test_subscription_creation_from_settings(test_node: ObeliskNode, set_node_parameters: Callable) -> None: + """A subscriber entry in obelisk_settings.subscribers becomes a real rclpy Subscription.""" def callback(msg: ocm.PositionSetpoint) -> None: pass - test_node.register_obk_subscription("test_sub_param", callback, ocm.PositionSetpoint, key="test_sub") - set_node_parameters(test_node, {"test_sub_param": "topic:/test_topic,msg_type:PositionSetpoint,history_depth:10"}) + test_node.register_obk_subscription(key="test_sub", callback=callback, msg_type=ocm.PositionSetpoint) + _set_obelisk_settings( + test_node, + { + "subscribers": [ + {"key": "test_sub", "topic": "/test_topic", "history_depth": 10}, + ] + }, + set_node_parameters, + ) test_node.on_configure(None) assert "test_sub" in test_node.obk_subscriptions assert isinstance(test_node.obk_subscriptions["test_sub"], Subscription) -def test_timer_creation_from_config(test_node: ObeliskNode, set_node_parameters: Callable) -> None: - """Test the creation of a timer from a configuration string. - - This test verifies that a timer can be created from a configuration string. - - Parameters: - test_node: An instance of ObeliskNode. - set_node_parameters: A fixture to set node parameters. - """ +def test_timer_creation_from_settings(test_node: ObeliskNode, set_node_parameters: Callable) -> None: + """A timer entry in obelisk_settings.timers becomes a real rclpy Timer.""" def callback() -> None: pass - test_node.register_obk_timer("test_timer_param", callback, key="test_timer") - set_node_parameters(test_node, {"test_timer_param": "timer_period_sec:1.0"}) + test_node.register_obk_timer(key="test_timer", callback=callback) + _set_obelisk_settings( + test_node, + {"timers": [{"key": "test_timer", "timer_period_sec": 1.0}]}, + set_node_parameters, + ) test_node.on_configure(None) assert "test_timer" in test_node.obk_timers assert isinstance(test_node.obk_timers["test_timer"], Timer) + + +def test_invalid_callback_group_type(test_node: ObeliskNode, set_node_parameters: Callable) -> None: + """An unknown callback-group type raises ValueError at on_configure time.""" + _set_obelisk_settings(test_node, {"callback_groups": {"bad": "NotARealGroupType"}}, set_node_parameters) + with pytest.raises(ValueError): + test_node.on_configure(None) diff --git a/tests/tests_python/tests_core/test_robot.py b/tests/tests_python/tests_core/test_robot.py index b535c693..1f8b63c5 100644 --- a/tests/tests_python/tests_core/test_robot.py +++ b/tests/tests_python/tests_core/test_robot.py @@ -2,6 +2,7 @@ import obelisk_sensor_msgs.msg as osm import pytest +import yaml from rclpy.lifecycle.node import TransitionCallbackReturn from rclpy.publisher import Publisher from rclpy.subscription import Subscription @@ -67,7 +68,8 @@ def test_robot_initialization(test_robot: TestRobot) -> None: test_robot: An instance of TestRobot. """ assert test_robot.get_name() == "test_robot" - assert test_robot.has_parameter("sub_ctrl_setting") + assert test_robot.has_parameter("obelisk_settings") + assert any(s["key"] == "sub_ctrl" for s in test_robot._obk_sub_settings) def test_robot_subscription_registration(test_robot: TestRobot) -> None: @@ -93,7 +95,8 @@ def test_robot_configuration(test_robot: TestRobot, set_node_parameters: Callabl test_robot: An instance of TestRobot. set_node_parameters: A fixture to set node parameters. """ - set_node_parameters(test_robot, {"sub_ctrl_setting": "topic:/test_control,msg_type:PositionSetpoint"}) + settings = {"subscribers": [{"key": "sub_ctrl", "topic": "/test_control", "history_depth": 10}]} + set_node_parameters(test_robot, {"obelisk_settings": yaml.safe_dump(settings)}) result = test_robot.on_configure(None) assert result == TransitionCallbackReturn.SUCCESS @@ -111,8 +114,9 @@ def test_sim_robot_initialization(test_sim_robot: TestSimRobot) -> None: test_sim_robot: An instance of TestSimRobot. """ assert test_sim_robot.get_name() == "test_sim_robot" - assert test_sim_robot.has_parameter("timer_true_sim_state_setting") - assert test_sim_robot.has_parameter("pub_true_sim_state_setting") + assert test_sim_robot.has_parameter("obelisk_settings") + assert any(s["key"] == "timer_true_sim_state" for s in test_sim_robot._obk_timer_settings) + assert any(s["key"] == "pub_true_sim_state" for s in test_sim_robot._obk_pub_settings) def test_sim_robot_timer_registration(test_sim_robot: TestSimRobot) -> None: @@ -149,14 +153,12 @@ def test_sim_robot_configuration(test_sim_robot: TestSimRobot, set_node_paramete test_sim_robot: An instance of TestSimRobot. set_node_parameters: A fixture to set node parameters. """ - set_node_parameters( - test_sim_robot, - { - "sub_ctrl_setting": "topic:/test_control,msg_type:PositionSetpoint", - "timer_true_sim_state_setting": "timer_period_sec:0.1", - "pub_true_sim_state_setting": "topic:/test_true_sim_state", - }, - ) + settings = { + "subscribers": [{"key": "sub_ctrl", "topic": "/test_control", "history_depth": 10}], + "timers": [{"key": "timer_true_sim_state", "timer_period_sec": 0.1}], + "publishers": [{"key": "pub_true_sim_state", "topic": "/test_true_sim_state", "history_depth": 10}], + } + set_node_parameters(test_sim_robot, {"obelisk_settings": yaml.safe_dump(settings)}) result = test_sim_robot.on_configure(None) assert result == TransitionCallbackReturn.SUCCESS diff --git a/tests/tests_python/tests_core/test_sensing.py b/tests/tests_python/tests_core/test_sensing.py index f92ff0b5..f90d2031 100644 --- a/tests/tests_python/tests_core/test_sensing.py +++ b/tests/tests_python/tests_core/test_sensing.py @@ -1,7 +1,10 @@ +"""Tests for ObeliskSensor (post-config-string-refactor).""" + from typing import Any, Callable, Generator import obelisk_sensor_msgs.msg as osm import pytest +import yaml from rclpy.lifecycle import TransitionCallbackReturn from obelisk_py.core.sensing import ObeliskSensor @@ -16,40 +19,22 @@ def test_sensor(ros_context: Any) -> Generator[ObeliskSensor, None, None]: def test_sensor_initialization(test_sensor: ObeliskSensor) -> None: - """Test the initialization of the ObeliskSensor. - - This test checks if the sensor is properly initialized with the correct name. - - Parameters: - test_sensor: An instance of ObeliskSensor. - """ + """The sensor is named correctly and starts without a sensor publisher.""" assert test_sensor.get_name() == "test_sensor" assert not test_sensor._has_sensor_publisher def test_sensor_configuration_without_publisher(test_sensor: ObeliskSensor) -> None: - """Test the configuration of ObeliskSensor without a sensor publisher. - - This test verifies that configuring a sensor without a sensor publisher raises an assertion error. - - Parameters: - test_sensor: An instance of ObeliskSensor. - """ + """ObeliskSensor without any sensor publisher fails the assert in on_configure.""" with pytest.raises(AssertionError): test_sensor.on_configure(None) def test_sensor_configuration_with_publisher(test_sensor: ObeliskSensor, set_node_parameters: Callable) -> None: - """Test the configuration of ObeliskSensor with a sensor publisher. - - This test verifies that configuring a sensor with a valid sensor publisher succeeds. - - Parameters: - test_sensor: An instance of ObeliskSensor. - set_node_parameters: A fixture to set node parameters. - """ - test_sensor.register_obk_publisher("test_pub_param", osm.ObkJointEncoders, key="test_pub") - set_node_parameters(test_sensor, {"test_pub_param": "topic:/test_topic,msg_type:ObkJointEncoders,history_depth:10"}) + """ObeliskSensor with a registered sensor publisher and a matching settings entry configures cleanly.""" + test_sensor.register_obk_publisher(key="test_pub", msg_type=osm.ObkJointEncoders) + settings = {"publishers": [{"key": "test_pub", "topic": "/test_topic", "history_depth": 10}]} + set_node_parameters(test_sensor, {"obelisk_settings": yaml.safe_dump(settings)}) result = test_sensor.on_configure(None) assert result == TransitionCallbackReturn.SUCCESS @@ -57,13 +42,7 @@ def test_sensor_configuration_with_publisher(test_sensor: ObeliskSensor, set_nod def test_sensor_lifecycle_callbacks(test_sensor: ObeliskSensor) -> None: - """Test the lifecycle callbacks of the ObeliskSensor. - - This test verifies that the lifecycle callbacks return the expected TransitionCallbackReturn. - - Parameters: - test_sensor: An instance of ObeliskSensor. - """ + """Lifecycle callbacks return SUCCESS for a bare sensor (no on_configure called).""" assert test_sensor.on_activate(None) == TransitionCallbackReturn.SUCCESS assert test_sensor.on_deactivate(None) == TransitionCallbackReturn.SUCCESS assert test_sensor.on_cleanup(None) == TransitionCallbackReturn.SUCCESS diff --git a/tests/tests_python/tests_core/test_utils/test_launch_utils.py b/tests/tests_python/tests_core/test_utils/test_launch_utils.py index 9b8ade29..69c4eaeb 100644 --- a/tests/tests_python/tests_core/test_utils/test_launch_utils.py +++ b/tests/tests_python/tests_core/test_utils/test_launch_utils.py @@ -1,11 +1,18 @@ +"""Tests for launch_utils (post-config-string-refactor). + +The producer now bundles all Obelisk-internal sections (publishers/subscribers/timers/sim/ +callback_groups) into a single ``obelisk_settings`` ROS parameter whose value is a YAML string. +User ``params`` and ``params_path`` flow through as separate ROS parameters. +""" + from pathlib import Path from typing import Any, Dict import pytest +import yaml from launch_ros.actions import LifecycleNode from obelisk_py.core.utils.launch_utils import ( - get_component_settings_subdict, get_launch_actions_from_node_settings, get_parameters_dict, load_config_file, @@ -14,95 +21,93 @@ @pytest.fixture def test_config() -> Dict[str, Any]: - """Fixture to provide test configuration data.""" + """Fixture to provide test configuration data (mirrors test_assets/test_config.yaml).""" return { - "onboard": { - "control": [ - { - "pkg": "test_pkg1", - "executable": "test_controller", - "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup", "cbg2": "ReentrantCallbackGroup"}, - "publishers": [ - { - "ros_parameter": "pub_ctrl_settings1", - "key": "pub1", - "topic": "/test/controller/pub1", - "msg_type": "TestMsg", - "history_depth": 10, - "callback_group": "cbg1", - "non_obelisk": False, - } - ], - "subscribers": [ - { - "ros_parameter": "sub_ctrl_settings1", - "key": "sub1", - "topic": "/test/controller/sub1", - "msg_type": "TestMsg", - "history_depth": 5, - "callback_key": "sub_callback1", - "callback_group": "cbg2", - "non_obelisk": False, - } - ], - "timers": [ - { - "ros_parameter": "timer_ctrl_settings1", - "key": "timer1", - "timer_period_sec": 0.1, - "callback_group": "cbg1", - "callback_key": "timer_callback1", - } - ], - } - ], - "estimation": [ - { - "pkg": "test_pkg2", - "executable": "test_estimator", - "callback_groups": {"cbg1": "ReentrantCallbackGroup"}, - } - ], - "robot": [ - { - "pkg": "test_pkg3", - "executable": "test_robot", - "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, - } - ], - "sensing": [ - { - "pkg": "test_pkg4", - "executable": "test_sensor1", - "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, - "publishers": [ - { - "ros_parameter": "pub_sensor_settings1", - "key": "pub1", - "topic": "/test/sensor1/pub1", - "msg_type": "SensorMsg", - "history_depth": 10, - "callback_group": "cbg1", - "non_obelisk": False, - } - ], - }, - { - "pkg": "test_pkg5", - "executable": "test_sensor2", - "callback_groups": {"cbg1": "ReentrantCallbackGroup"}, - "timers": [ - { - "ros_parameter": "timer_sensor_settings1", - "key": "timer1", - "timer_period_sec": 0.01, - "callback_group": "cbg1", - "callback_key": "timer_callback1", - } - ], - }, - ], - } + "control": [ + { + "pkg": "test_pkg1", + "executable": "test_controller", + "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup", "cbg2": "ReentrantCallbackGroup"}, + "publishers": [ + { + "ros_parameter": "pub_ctrl_settings1", + "key": "pub1", + "topic": "/test/controller/pub1", + "msg_type": "TestMsg", + "history_depth": 10, + "callback_group": "cbg1", + "non_obelisk": False, + } + ], + "subscribers": [ + { + "ros_parameter": "sub_ctrl_settings1", + "key": "sub1", + "topic": "/test/controller/sub1", + "msg_type": "TestMsg", + "history_depth": 5, + "callback_key": "sub_callback1", + "callback_group": "cbg2", + "non_obelisk": False, + } + ], + "timers": [ + { + "ros_parameter": "timer_ctrl_settings1", + "key": "timer1", + "timer_period_sec": 0.1, + "callback_group": "cbg1", + "callback_key": "timer_callback1", + } + ], + } + ], + "estimation": [ + { + "pkg": "test_pkg2", + "executable": "test_estimator", + "callback_groups": {"cbg1": "ReentrantCallbackGroup"}, + } + ], + "robot": [ + { + "pkg": "test_pkg3", + "executable": "test_robot", + "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, + } + ], + "sensing": [ + { + "pkg": "test_pkg4", + "executable": "test_sensor1", + "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, + "publishers": [ + { + "ros_parameter": "pub_sensor_settings1", + "key": "pub1", + "topic": "/test/sensor1/pub1", + "msg_type": "SensorMsg", + "history_depth": 10, + "callback_group": "cbg1", + "non_obelisk": False, + } + ], + }, + { + "pkg": "test_pkg5", + "executable": "test_sensor2", + "callback_groups": {"cbg1": "ReentrantCallbackGroup"}, + "timers": [ + { + "ros_parameter": "timer_sensor_settings1", + "key": "timer1", + "timer_period_sec": 0.01, + "callback_group": "cbg1", + "callback_key": "timer_callback1", + } + ], + }, + ], } @@ -118,216 +123,192 @@ def test_global_state_node() -> LifecycleNode: ) -def test_load_config_file(test_config: Dict[str, Any]) -> None: - """Test the load_config_file function. +_ASSETS = Path(__file__).parent.parent.parent / "test_assets" - Args: - test_config: Test configuration fixture. - """ - # Test with absolute path - abs_path = Path(__file__).parent.parent.parent / "test_assets" / "test_config.yaml" - result = load_config_file(abs_path) + +def test_load_config_file(test_config: Dict[str, Any]) -> None: + """load_config_file reads YAML files (used by the launch entry point).""" + result = load_config_file(_ASSETS / "test_config.yaml") assert result == test_config - # Test file not found with pytest.raises(FileNotFoundError): load_config_file("non_existent.yaml") -def test_get_component_settings_subdict(test_config: Dict[str, Any]) -> None: - """Test the get_component_settings_subdict function. +# ---------------------------------------------------------------------- # +# include: directive — composition tests # +# ---------------------------------------------------------------------- # - Args: - test_config: Test configuration fixture. - """ - node_settings = test_config["onboard"]["control"][0] - - # Test publishers - pub_settings = get_component_settings_subdict(node_settings, "publishers") - expected_pub_settings = { - "pub_ctrl_settings1": ( - "key:pub1,topic:/test/controller/pub1,msg_type:TestMsg,history_depth:10," - "callback_group:cbg1,non_obelisk:False" - ) - } - assert pub_settings == expected_pub_settings - - # Test subscribers - sub_settings = get_component_settings_subdict(node_settings, "subscribers") - expected_sub_settings = { - "sub_ctrl_settings1": ( - "key:sub1,topic:/test/controller/sub1,msg_type:TestMsg,history_depth:5,callback_key:sub_callback1," - "callback_group:cbg2,non_obelisk:False" - ) - } - assert sub_settings == expected_sub_settings - - # Test timers - timer_settings = get_component_settings_subdict(node_settings, "timers") - expected_timer_settings = { - "timer_ctrl_settings1": "key:timer1,timer_period_sec:0.1,callback_group:cbg1,callback_key:timer_callback1" - } - assert timer_settings == expected_timer_settings +def test_load_config_no_include_returns_same_dict(test_config: Dict[str, Any]) -> None: + """A YAML without an `include:` key behaves exactly as before (regression check).""" + result = load_config_file(_ASSETS / "test_config.yaml") + assert "include" not in result + assert result == test_config -def test_get_parameters_dict(test_config: Dict[str, Any]) -> None: - """Test the get_parameters_dict function. - Args: - test_config: Test configuration fixture. +def test_load_config_with_include_merges_and_strips_directive() -> None: + """compose_a includes compose_b which includes compose_c. The include key is dropped from + the result, scalars take the outermost value, and list-shaped sections concat in include order + (innermost first). """ - node_settings = test_config["onboard"]["control"][0] - parameters_dict = get_parameters_dict(node_settings) + result = load_config_file(_ASSETS / "compose_a.yaml") + + assert "include" not in result + # config: outermost (a) wins + assert result["config"] == "composed_a" + # control: c, b, a appended in that order (deepest include first, outermost last) + ctrl_pkgs = [entry["pkg"] for entry in result["control"]] + assert ctrl_pkgs == ["c_pkg", "b_pkg", "a_pkg"] + # estimation only defined in c, propagates up unchanged + assert result["estimation"][0]["pkg"] == "c_est_pkg" + - expected_dict = { - "callback_group_settings": "cbg1:MutuallyExclusiveCallbackGroup,cbg2:ReentrantCallbackGroup", - "pub_ctrl_settings1": ( - "key:pub1,topic:/test/controller/pub1,msg_type:TestMsg,history_depth:10,callback_group:cbg1," - "non_obelisk:False" - ), - "sub_ctrl_settings1": ( - "key:sub1,topic:/test/controller/sub1,msg_type:TestMsg,history_depth:5,callback_key:sub_callback1," - "callback_group:cbg2,non_obelisk:False" - ), - "timer_ctrl_settings1": "key:timer1,timer_period_sec:0.1,callback_group:cbg1,callback_key:timer_callback1", +def test_load_config_dict_section_deep_merge() -> None: + """joystick is dict-shaped: deep-merge with parent winning on per-key conflicts. b sets `on` + and `sub_topic`; a overrides `pub_topic`. + """ + result = load_config_file(_ASSETS / "compose_a.yaml") + assert result["joystick"] == { + "on": True, + "pub_topic": "/from_a", # a overrode b's value + "sub_topic": "/from_b_sub", # only b set this; survives } - assert parameters_dict == expected_dict +def test_load_config_recursive_includes_three_deep() -> None: + """A includes B, B includes C; C's contributions appear in the final result.""" + result = load_config_file(_ASSETS / "compose_a.yaml") + # only c defines an estimation list, and it has one entry + assert len(result["estimation"]) == 1 + assert result["estimation"][0]["executable"] == "c_est" + + +def test_load_config_cycle_detection_raises() -> None: + """An include cycle raises RuntimeError mentioning the chain — never hangs.""" + with pytest.raises(RuntimeError, match="Cycle"): + load_config_file(_ASSETS / "compose_cycle_a.yaml") -def test_get_launch_actions_from_node_settings( - test_config: Dict[str, Any], test_global_state_node: LifecycleNode -) -> None: - """Test the get_launch_actions_from_node_settings function. - Args: - test_config: Test configuration fixture. - test_global_state_node: Dummy global state node fixture. +def test_load_config_relative_includes_resolve_to_parent_dir(tmp_path: Path) -> None: + """Relative paths in `include:` resolve against the parent file's directory, not the + obelisk_ros package share dir. """ - node_settings = test_config["onboard"]["control"] - launch_actions = get_launch_actions_from_node_settings(node_settings, "control", test_global_state_node) + # Build a small two-file tree in a fresh tmpdir so we know the dir is unrelated to share/. + sub = tmp_path / "configs" / "subdir" + sub.mkdir(parents=True) + (sub / "leaf.yaml").write_text("config: leaf\ncontrol:\n - pkg: leaf_pkg\n executable: e\n") + (sub / "root.yaml").write_text( + "config: root\ninclude:\n - leaf.yaml\ncontrol:\n - pkg: root_pkg\n executable: e\n" + ) - assert len(launch_actions) == 8 # noqa: PLR2004 - assert isinstance(launch_actions[0], LifecycleNode) + result = load_config_file(sub / "root.yaml") + assert [c["pkg"] for c in result["control"]] == ["leaf_pkg", "root_pkg"] + assert result["config"] == "root" - # Test for sensors (multiple nodes) - sensors_settings = [ - { - "pkg": "test_pkg1", - "executable": "test_sensor1", - "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, - "publishers": [ - { - "ros_parameter": "pub_sensor_settings1", - "key": "pub1", - "topic": "/test/sensor1/pub1", - "msg_type": "SensorMsg", - "history_depth": 10, - "callback_group": "cbg1", - "non_obelisk": False, - } - ], - }, - { - "pkg": "test_pkg2", - "executable": "test_sensor2", - "callback_groups": {"cbg1": "ReentrantCallbackGroup"}, - "timers": [ - { - "ros_parameter": "timer_sensor_settings1", - "key": "timer1", - "timer_period_sec": 0.01, - "callback_group": "cbg1", - "callback_key": "timer_callback1", - } - ], - }, - ] - - launch_actions = get_launch_actions_from_node_settings(sensors_settings, "sensing", test_global_state_node) - - assert len(launch_actions) == 16 # noqa: PLR2004 - assert isinstance(launch_actions[0], LifecycleNode) - assert isinstance(launch_actions[8], LifecycleNode) +def test_load_config_absolute_include_path_works(tmp_path: Path) -> None: + """An absolute path inside `include:` is used as-is.""" + leaf = tmp_path / "leaf.yaml" + leaf.write_text("control:\n - pkg: abs_pkg\n executable: e\n") + root = tmp_path / "root.yaml" + root.write_text(f"include:\n - {leaf}\n") -def test_get_component_settings_subdict_sensors(test_config: Dict[str, Any]) -> None: - """Test the get_component_settings_subdict function for sensors. + result = load_config_file(root) + assert result["control"][0]["pkg"] == "abs_pkg" - Args: - test_config: Test configuration fixture. - """ - sensors_settings = test_config["onboard"]["sensing"] - - # Test publishers for the first sensor - pub_settings = get_component_settings_subdict(sensors_settings[0], "publishers") - expected_pub_settings = { - "pub_sensor_settings1": ( - "key:pub1,topic:/test/sensor1/pub1,msg_type:SensorMsg,history_depth:10,callback_group:cbg1," - "non_obelisk:False" - ) - } - assert pub_settings == expected_pub_settings - # Test timers for the second sensor - timer_settings = get_component_settings_subdict(sensors_settings[1], "timers") - expected_timer_settings = { - "timer_sensor_settings1": "key:timer1,timer_period_sec:0.01,callback_group:cbg1,callback_key:timer_callback1" - } - assert timer_settings == expected_timer_settings +def test_load_config_dummy_composed_resolves_to_full_stack() -> None: + """End-to-end smoke test: dummy_composed.yaml (which uses `include:`) loads into a dict that + looks like a single-file config — control + estimation + robot + joystick all present, no + leftover `include:` key.""" + composed = load_config_file("dummy_composed.yaml") + assert "include" not in composed + assert composed["config"] == "dummy_composed" + assert len(composed["control"]) == 1 + assert composed["control"][0]["pkg"] == "obelisk_control_cpp" + assert len(composed["estimation"]) == 1 + assert len(composed["robot"]) == 1 + assert composed["robot"][0]["is_simulated"] is True + assert composed["joystick"]["on"] is True + +def test_get_parameters_dict_bundles_obelisk_sections(test_config: Dict[str, Any]) -> None: + """A controller node's settings collapse into a single obelisk_settings YAML string.""" + node_settings = test_config["control"][0] + parameters_dict = get_parameters_dict(node_settings) -def test_get_parameters_dict_sensors(test_config: Dict[str, Any]) -> None: - """Test the get_parameters_dict function for sensors. + assert "obelisk_settings" in parameters_dict + bundled = yaml.safe_load(parameters_dict["obelisk_settings"]) - Args: - test_config: Test configuration fixture. - """ - sensors_settings = test_config["onboard"]["sensing"] - - # Test parameters for the first sensor - parameters_dict1 = get_parameters_dict(sensors_settings[0]) - expected_dict1 = { - "callback_group_settings": "cbg1:MutuallyExclusiveCallbackGroup", - "pub_sensor_settings1": ( - "key:pub1,topic:/test/sensor1/pub1,msg_type:SensorMsg,history_depth:10,callback_group:cbg1," - "non_obelisk:False" - ), + # callback_groups round-trip + assert bundled["callback_groups"] == { + "cbg1": "MutuallyExclusiveCallbackGroup", + "cbg2": "ReentrantCallbackGroup", + } + # publisher entry has key + topic + ros_parameter dropped + assert len(bundled["publishers"]) == 1 + pub = bundled["publishers"][0] + assert pub["key"] == "pub1" + assert pub["topic"] == "/test/controller/pub1" + assert "ros_parameter" not in pub + # timer + subscriber too + assert bundled["timers"][0]["timer_period_sec"] == 0.1 + assert bundled["subscribers"][0]["callback_group"] == "cbg2" + + +def test_get_parameters_dict_derives_key_from_ros_parameter() -> None: + """An entry that has only ``ros_parameter`` (no ``key``) gets ``key`` derived by stripping ``_setting``.""" + node_settings = { + "publishers": [ + {"ros_parameter": "pub_ctrl_setting", "topic": "/foo"}, + ] } - assert parameters_dict1 == expected_dict1 + bundled = yaml.safe_load(get_parameters_dict(node_settings)["obelisk_settings"]) + assert bundled["publishers"][0]["key"] == "pub_ctrl" - # Test parameters for the second sensor - parameters_dict2 = get_parameters_dict(sensors_settings[1]) - expected_dict2 = { - "callback_group_settings": "cbg1:ReentrantCallbackGroup", - "timer_sensor_settings1": "key:timer1,timer_period_sec:0.01,callback_group:cbg1,callback_key:timer_callback1", + +def test_get_parameters_dict_user_params_passthrough() -> None: + """User-defined ``params`` and ``params_path`` are forwarded as separate ROS parameters.""" + node_settings = { + "params_path": "/some/path.txt", + "params": {"user_int": 7, "user_str": "x"}, } - assert parameters_dict2 == expected_dict2 + parameters_dict = get_parameters_dict(node_settings) + assert parameters_dict["params_path"] == "/some/path.txt" + assert parameters_dict["user_int"] == 7 + assert parameters_dict["user_str"] == "x" -def test_get_parameters_dict_empty_node(test_config: Dict[str, Any]) -> None: - """Test the get_parameters_dict function for a node with minimal settings. +def test_get_parameters_dict_empty_node() -> None: + """A node with only callback_groups still produces obelisk_settings.""" + minimal = {"pkg": "p", "executable": "e", "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}} + parameters_dict = get_parameters_dict(minimal) + bundled = yaml.safe_load(parameters_dict["obelisk_settings"]) + assert bundled == {"callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}} - Args: - test_config: Test configuration fixture. - """ - minimal_node_settings = { - "pkg": "test_pkg1", - "executable": "test_minimal", - "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, - } - parameters_dict = get_parameters_dict(minimal_node_settings) - expected_dict = {"callback_group_settings": "cbg1:MutuallyExclusiveCallbackGroup"} - assert parameters_dict == expected_dict + +def test_get_parameters_dict_handles_none() -> None: + """get_parameters_dict on None returns an empty dict.""" + assert get_parameters_dict(None) == {} + + +def test_get_launch_actions_from_node_settings( + test_config: Dict[str, Any], test_global_state_node: LifecycleNode +) -> None: + """Each node setting becomes one LifecycleNode plus its lifecycle handlers (8 actions per node).""" + node_settings = test_config["control"] + launch_actions = get_launch_actions_from_node_settings(node_settings, "control", test_global_state_node) + assert len(launch_actions) == 8 # noqa: PLR2004 + assert isinstance(launch_actions[0], LifecycleNode) def test_get_launch_actions_from_node_settings_invalid_type(test_global_state_node: LifecycleNode) -> None: - """Test the get_launch_actions_from_node_settings function with an invalid node type.""" - invalid_node_settings = { - "pkg": "test_pkg1", - "executable": "test_invalid", + """An unknown node type asserts.""" + invalid = { + "pkg": "p", + "executable": "e", "callback_groups": {"cbg1": "MutuallyExclusiveCallbackGroup"}, } - with pytest.raises(AssertionError): - get_launch_actions_from_node_settings(invalid_node_settings, "invalid_type", test_global_state_node) + get_launch_actions_from_node_settings(invalid, "invalid_type", test_global_state_node) diff --git a/tests/tests_python/tests_core/test_yaml_to_node.py b/tests/tests_python/tests_core/test_yaml_to_node.py new file mode 100644 index 00000000..8620592a --- /dev/null +++ b/tests/tests_python/tests_core/test_yaml_to_node.py @@ -0,0 +1,308 @@ +"""Behavioral safety-net tests for the YAML-config -> ROS-parameter -> running-node pipeline. + +These tests pin down the *observable* properties of an Obelisk node configured from a YAML-style +settings dict: topic names, history depth, callback-group bindings, timer periods, and msg types. +They are intentionally written to survive the config-string-removal refactor: the producer side +(`get_parameters_dict`) may change the shape of its output, but the assertions about the +configured node should hold either way. +""" + +from typing import Any, Callable, Generator, Type + +import pytest +from obelisk_estimator_msgs.msg import EstimatedState +from obelisk_sensor_msgs.msg import ObkJointEncoders, TrueSimState +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.lifecycle.node import TransitionCallbackReturn +from rclpy.parameter import Parameter + +from obelisk_py.core.control import ObeliskController +from obelisk_py.core.estimation import ObeliskEstimator +from obelisk_py.core.utils.launch_utils import get_parameters_dict + + +# ------------------------------------------------------------------ # +# Concrete test subclasses # +# ------------------------------------------------------------------ # + + +class _DummyController(ObeliskController): + """Minimal concrete ObeliskController for use as a test subject.""" + + def update_x_hat(self, x_hat_msg: Any) -> None: # pragma: no cover - not exercised here + pass + + def compute_control(self) -> Any: # pragma: no cover - not exercised here + return None + + +class _DummyEstimator(ObeliskEstimator): + """Minimal concrete ObeliskEstimator that adds a sensor subscription.""" + + def __init__(self, node_name: str, est_msg_type: Type) -> None: + super().__init__(node_name, est_msg_type) + self.register_obk_subscription( + key="sub_sensor", + callback=self.update_sensor, + msg_type=ObkJointEncoders, + ) + + def update_sensor(self, msg: Any) -> None: # pragma: no cover + pass + + def compute_state_estimate(self) -> Any: # pragma: no cover + return None + + +# ------------------------------------------------------------------ # +# Helpers # +# ------------------------------------------------------------------ # + + +def _apply_settings(node: Any, node_settings: dict) -> None: + """Run a YAML-style settings dict through the producer and apply the result to the node. + + This mirrors the launch-time flow: `get_parameters_dict` builds the ROS-parameter dict, + we assign each entry as a parameter on the node, then `on_configure` is what actually + instantiates pubs/subs/timers from those parameters. + """ + parameters_dict = get_parameters_dict(node_settings) + + # Producer may emit lists when there are multiple entries with the same ros_parameter name; + # the existing parser handles single strings (declare_parameter happens once per ros_parameter). + # If we hit a list, we use the first element so that the node's single parameter declaration + # is satisfied. The multi-publisher-per-ros_parameter case is exercised by separate parameters + # in the test bodies below rather than via list-valued params. + parameters = [] + for name, value in parameters_dict.items(): + if isinstance(value, list): + value = value[0] + if isinstance(value, str): + parameters.append(Parameter(name, Parameter.Type.STRING, value)) + elif isinstance(value, int): + parameters.append(Parameter(name, Parameter.Type.INTEGER, value)) + elif isinstance(value, float): + parameters.append(Parameter(name, Parameter.Type.DOUBLE, value)) + elif isinstance(value, bool): + parameters.append(Parameter(name, Parameter.Type.BOOL, value)) + node.set_parameters(parameters) + + +# ------------------------------------------------------------------ # +# Fixtures # +# ------------------------------------------------------------------ # + + +@pytest.fixture +def controller(ros_context: Any) -> Generator[_DummyController, None, None]: + node = _DummyController( + "yaml_to_node_controller", + ctrl_msg_type=EstimatedState, + est_msg_type=EstimatedState, + ) + yield node + node.destroy_node() + + +@pytest.fixture +def estimator(ros_context: Any) -> Generator[_DummyEstimator, None, None]: + node = _DummyEstimator("yaml_to_node_estimator", est_msg_type=EstimatedState) + yield node + node.destroy_node() + + +# ------------------------------------------------------------------ # +# Tests # +# ------------------------------------------------------------------ # + + +def test_simple_controller_yaml_to_node(controller: _DummyController) -> None: + """A trivial controller YAML configures pub, sub, and timer with the values from YAML.""" + node_settings = { + "publishers": [ + { + "ros_parameter": "pub_ctrl_setting", + "topic": "/safety_net/ctrl", + "history_depth": 7, + "callback_group": "None", + } + ], + "subscribers": [ + { + "ros_parameter": "sub_est_setting", + "topic": "/safety_net/est", + "history_depth": 3, + "callback_group": "None", + } + ], + "timers": [ + { + "ros_parameter": "timer_ctrl_setting", + "timer_period_sec": 0.005, + "callback_group": "None", + } + ], + } + _apply_settings(controller, node_settings) + assert controller.on_configure(None) == TransitionCallbackReturn.SUCCESS + + pub = controller.obk_publishers["pub_ctrl"] + assert pub.topic_name == "/safety_net/ctrl" + assert pub.qos_profile.depth == 7 + + sub = controller.obk_subscriptions["sub_est"] + assert sub.topic_name == "/safety_net/est" + assert sub.qos_profile.depth == 3 + + timer = controller.obk_timers["timer_ctrl"] + # rclpy Timer exposes its period in nanoseconds as `timer_period_ns`. + assert timer.timer_period_ns == int(0.005 * 1e9) + + +def test_callback_group_binding(controller: _DummyController) -> None: + """A pub bound to a named callback group ends up on the right group instance.""" + node_settings = { + "callback_groups": { + "ctrl_cbg": "MutuallyExclusiveCallbackGroup", + "io_cbg": "ReentrantCallbackGroup", + }, + "publishers": [ + { + "ros_parameter": "pub_ctrl_setting", + "topic": "/safety_net/ctrl", + "history_depth": 10, + "callback_group": "ctrl_cbg", + } + ], + "subscribers": [ + { + "ros_parameter": "sub_est_setting", + "topic": "/safety_net/est", + "history_depth": 10, + "callback_group": "io_cbg", + } + ], + "timers": [ + { + "ros_parameter": "timer_ctrl_setting", + "timer_period_sec": 0.01, + "callback_group": "ctrl_cbg", + } + ], + } + _apply_settings(controller, node_settings) + assert controller.on_configure(None) == TransitionCallbackReturn.SUCCESS + + cbgs = controller.obk_callback_groups + assert isinstance(cbgs["ctrl_cbg"], MutuallyExclusiveCallbackGroup) + assert isinstance(cbgs["io_cbg"], ReentrantCallbackGroup) + + +def test_estimator_with_sensor_sub(estimator: _DummyEstimator) -> None: + """The estimator subclass exposes a sensor subscription configured by YAML.""" + node_settings = { + "publishers": [ + { + "ros_parameter": "pub_est_setting", + "topic": "/safety_net/est_out", + "history_depth": 5, + "callback_group": "None", + } + ], + "subscribers": [ + { + "ros_parameter": "sub_sensor_setting", + "topic": "/safety_net/sensors", + "history_depth": 10, + "callback_group": "None", + } + ], + "timers": [ + { + "ros_parameter": "timer_est_setting", + "timer_period_sec": 0.02, + "callback_group": "None", + } + ], + } + _apply_settings(estimator, node_settings) + assert estimator.on_configure(None) == TransitionCallbackReturn.SUCCESS + + assert estimator.obk_publishers["pub_est"].topic_name == "/safety_net/est_out" + assert estimator.obk_publishers["pub_est"].qos_profile.depth == 5 + assert estimator.obk_subscriptions["sub_sensor"].topic_name == "/safety_net/sensors" + assert estimator.obk_timers["timer_est"].timer_period_ns == int(0.02 * 1e9) + + +def test_get_parameters_dict_handles_no_settings() -> None: + """get_parameters_dict on None returns an empty dict (launch-side robustness).""" + assert get_parameters_dict(None) == {} + + +def test_user_params_pass_through() -> None: + """User-defined `params` and `params_path` entries are forwarded as ROS parameters as-is.""" + node_settings = { + "params_path": "/some/path/params.txt", + "params": { + "user_int": 42, + "user_str": "hello", + "user_list": [1, 2, 3], + }, + } + parameters_dict = get_parameters_dict(node_settings) + assert parameters_dict["params_path"] == "/some/path/params.txt" + assert parameters_dict["user_int"] == 42 + assert parameters_dict["user_str"] == "hello" + assert parameters_dict["user_list"] == [1, 2, 3] + + +def test_sim_settings_with_nested_sensors() -> None: + """The mujoco-style sim entry with nested `sensor_settings` and `sensor_names` survives the + producer pipeline. + + This is the worst-case for today's implementation (the regex-escaping path). The behavior we + pin: the producer must not lose any of the leaf data, the result must be reachable as a single + ROS parameter named after `ros_parameter`, and the leaf values (topic, dt, sensor names) must + still be discoverable from the encoded output. + """ + node_settings = { + "sim": [ + { + "ros_parameter": "mujoco_setting", + "num_steps_per_viz": 5, + "robot_pkg": "dummy_bot", + "model_xml_path": "dummy.xml", + "sensor_settings": [ + { + "topic": "/safety_net/joint_encoders", + "dt": 0.001, + "msg_type": "ObkJointEncoders", + "sensor_names": { + "joint_pos": "jointpos", + "joint_vel": "jointvel", + }, + }, + ], + } + ] + } + parameters_dict = get_parameters_dict(node_settings) + assert "mujoco_setting" in parameters_dict + encoded = parameters_dict["mujoco_setting"] + # The new producer emits sim entries as YAML strings; round-trip and check leaf values. + import yaml as _yaml + + parsed = _yaml.safe_load(encoded) + assert parsed["model_xml_path"] == "dummy.xml" + assert parsed["sensor_settings"][0]["topic"] == "/safety_net/joint_encoders" + assert parsed["sensor_settings"][0]["dt"] == 0.001 + assert parsed["sensor_settings"][0]["msg_type"] == "ObkJointEncoders" + assert parsed["sensor_settings"][0]["sensor_names"] == {"joint_pos": "jointpos", "joint_vel": "jointvel"} + + +def test_true_sim_state_msg_imports() -> None: + """Sanity check that the sensor message types referenced by sim configs are importable — + the mujoco_sim_robot ties to TrueSimState and ObkJointEncoders specifically. + """ + assert TrueSimState is not None + assert ObkJointEncoders is not None