Repository: tier4/scenario_simulator_v2
Pull request #1569: RJD-1681 - add agnocast_wrapper, use agnocast for /perception/obstacle_segmentation/pointcloud
Author: login: TauTheLepton
Labels: name: feature
description: New feature or request
name: bump minor
description: If this pull request merged, bump minor version of the scenario_simulator_v2
Created: 2025-04-10T15:01:24Z | Merged: 2025-04-25T01:37:59Z

# Description

## Abstract

This PR introduces the possibility of using `agnocast` in `scenario_simulator_v2`. 
At this time, the possibility to use `agnocast` has been introduced only for topic `/perception/obstacle_segmentation/pointcloud`.

Regression tests were run - 0 negative scenarios resulted, detailed report [here](https://github.com/tier4/sim_evaluation_tools/issues/499#issuecomment-2823554185).

## Background

There is a need for optional `agnocast` communication on selected topics. This is because not all autoware support `agnocast`.

## Details

- New package `agnocast_wrapper` has been developed which provides a wrapper to easily switch communication between “agnocast” and “ros2”.
  - It includes subscribing, publishing and creating messages.
  - The basic code of the wrapper can be found [here](https://github.com/tier4/scenario_simulator_v2/blob/ccaa46ffad4be4eaf94108d33440e1a368de01d6/common/agnocast_wrapper/include/agnocast_wrapper/agnocast_wrapper.hpp).
  - This works in a similar way to the integration with Autoware described [here](https://github.com/orgs/autowarefoundation/discussions/5835).

- To be more precise, to add optional communication for e.g. a publisher, it is enough to change its type and constructor call to `agnocast_wrapper`.

- I would also add that part of the communication in `ss2` is already wrapped up in a different way, so some additional integration will certainly be necessary, but I do not imagine significant difficulties here.

- The `agnocast_wrapper` has been used for the publication of topic `/perception/obstacle_segmentation/pointcloud` in module `simple_sensor_simulator` - it can be seen [here](https://github.com/tier4/scenario_simulator_v2/pull/1569/files#diff-c8a5e1412aceb3017de2fe1bef0d16a6686d9f1b495731cb2a98127efc964570).

- The switching is handled in the build time by setting the `ENABLE_AGNOCAST_SIMULATOR` environment variable - see _Build with agnocast_ section. 

- **Note**: During tests I noticed a issue with the `agnocast` communication running in the [main](https://github.com/tier4/agnocast/tree/main) version, version [v2.0.1](https://github.com/tier4/agnocast/tree/2.0.1) works fine - this one is set in the dependencies.


#### Build with agnocast
Since `agnocast` is to be optional for `ss2`, dependence on it must also be optional. 
Therefore a dependency cannot be added to the `package.xml` of the `agnocast_wrapper` package. 
Because of this, during the build process the dependencies are not resolved correctly and the build order may be incorrect. 

Therefore, if you want to use Autoware where the [agnocast repository](https://github.com/tier4/agnocast/tree/2.0.1) is included, you must first build the `agnocast package` (step 4, 5) and then the whole repository (step 6). If agnocast is not in autoware, but you want to build ss2 with agnocast, you first need to clone the repository (step 3).

##### Steps
Open the terminal and go to workspace (e.g. a cloned autoware repository).
In this terminal, call the following commands:

1. `export ENABLE_AGNOCAST_SIMULATOR=1` 
    - (this command ensures that agnocast is used on the ss2 side)
2. `export ENABLE_AGNOCAST=1`  
   - (this command ensures that agnocast is used on the Autoware side)
3. `git clone --branch 2.0.1 git@github.com:tier4/agnocast.git ./src/agnocast` 
    - (this command clones the agnocast repository, **skip if workspace already contains agnocast**)
4. `colcon build --packages-up-to agnocastlib --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release` 
    - (this command builds agnocast repository first)
5.  `source install/setup.bash`
    - (this command source agnocast built repository to be seen for the next build step
6. `colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-w"`
    - (this command builds the entire repository)

##### Example screen
![Screenshot from 2025-04-18 09-44-35](https://github.com/user-attachments/assets/374bc26a-1fe8-4092-b005-7ae05fd85fb1)

##### Example log
[Logfile](https://github.com/user-attachments/files/19845519/build_with_agnocast_works_well.log)

##### Troubleshooting
1. If `agnocast` is not in workspace or if it is but was not built first, you will encounter the following problem. To avoid this remember to do steps 3, 4, 5.
![Screenshot from 2025-04-17 15-06-11](https://github.com/user-attachments/assets/51213e52-aa83-46c8-bd6f-75b837133649)

2. If you want to use `agnocast` but do not set the environment variable `ENABLE_AGNOCAST_SIMULATOR` to 1, you will encounter the following problem. Remember to do steps 1,2.
![Screenshot from 2025-04-17 14-25-51](https://github.com/user-attachments/assets/8c25a4da-fe31-416f-b4be-6c57bfb50e3d)

#### Build without agnocast
To build a workspace without using agnocast simply skip steps 1-6 mentioned above.
In other words, build everything the usual way without including agnocast in the build process.

**Note**: If you have previously defined `ENABLE_AGNOCAST_SIMULATOR` and `ENABLE_AGNOCAST`, set them to 0 before the build process:
1. `export ENABLE_AGNOCAST_SIMULATOR=0` 
    - (this command ensures that agnocast is not used on the ss2 side)
2. `export ENABLE_AGNOCAST=0`  
   - (this command ensures that agnocast is not used on the Autoware side)

##### Example log
[Logfile](https://github.com/user-attachments/files/19845891/build_without_angnocast.log)

## References

[RJD-1681](https://tier4.atlassian.net/browse/RJD-1681)

# Destructive Changes

None

# Known Limitations
At this time, the possibility to use `agnocast` has been introduced only for topic `/perception/obstacle_segmentation/pointcloud`.

This PR introduces an implicit dependency on `agnocastlib`. The `package.xml` for the `agnocast_wrapper` package does not add `agnocastlib` in order for ss2 to be built without it (when `agnocast` is not to be used). 
In other words, if we want `agnocastlib` to be added to `package.xml` `rosdep` would always require the `agnocastlib` package to exist in the repository.



[RJD-1681]: https://tier4.atlassian.net/browse/RJD-1681?atlOrigin=eyJpIjoiNWRkNTljNzYxNjVmNDY3MDlhMDU5Y2ZhYzA5YTRkZjUiLCJwIjoiZ2l0aHViLWNvbS1KU1cifQ


Diff:
diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json
index 50ca6f5fe94..c763b23f0f9 100644
--- a/.github/workflows/custom_spell.json
+++ b/.github/workflows/custom_spell.json
@@ -48,6 +48,8 @@
     "xerces",
     "xercesc",
     "Szymon",
-    "Parapura"
+    "Parapura",
+    "libagnocast",
+    "agnocast"
   ]
 }
diff --git a/common/agnocast_wrapper/CHANGELOG.rst b/common/agnocast_wrapper/CHANGELOG.rst
new file mode 100644
index 00000000000..195bf7aa113
--- /dev/null
+++ b/common/agnocast_wrapper/CHANGELOG.rst
@@ -0,0 +1,6 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package agnocast_wrapper
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+16.1.3 (2025-04-21)
+-------------------
diff --git a/common/agnocast_wrapper/CMakeLists.txt b/common/agnocast_wrapper/CMakeLists.txt
new file mode 100644
index 00000000000..69dda00948a
--- /dev/null
+++ b/common/agnocast_wrapper/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(agnocast_wrapper)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+
+if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
+  message(WARNING "Building with agnocastlib dependency")
+  find_package(agnocastlib REQUIRED)
+else()
+  message(WARNING "Building without agnocastlib dependency")
+endif()
+
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(CONFIG_EXTRAS "cmake/agnocast_wrapper_setup_target.cmake")
diff --git a/common/agnocast_wrapper/cmake/agnocast_wrapper_setup_target.cmake b/common/agnocast_wrapper/cmake/agnocast_wrapper_setup_target.cmake
new file mode 100644
index 00000000000..689901939bf
--- /dev/null
+++ b/common/agnocast_wrapper/cmake/agnocast_wrapper_setup_target.cmake
@@ -0,0 +1,22 @@
+# Copyright 2015 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+function(agnocast_wrapper_setup target)
+  if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
+    message(WARNING "agnocast_wrapper_setup: Defining USE_AGNOCAST_ENABLED for target ${target}")
+    target_compile_definitions(${target} PRIVATE USE_AGNOCAST_ENABLED)
+  else()
+    message(WARNING "agnocast_wrapper_setup: Not defining USE_AGNOCAST_ENABLED for target ${target}")
+  endif()
+endfunction()
diff --git a/common/agnocast_wrapper/include/agnocast_wrapper/agnocast_wrapper.hpp b/common/agnocast_wrapper/include/agnocast_wrapper/agnocast_wrapper.hpp
new file mode 100644
index 00000000000..9325dab4f0e
--- /dev/null
+++ b/common/agnocast_wrapper/include/agnocast_wrapper/agnocast_wrapper.hpp
@@ -0,0 +1,136 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AGNOCAST_WRAPPER__AGNOCAST_WRAPPER_HPP_
+#define AGNOCAST_WRAPPER__AGNOCAST_WRAPPER_HPP_
+
+#include <string>
+#include <utility>
+
+/// @sa https://github.com/veqcc/autoware.universe/blob/252ae60788a8388585780a6b1935a5682c688464/common/autoware_agnocast_wrapper/include/autoware_agnocast_wrapper/autoware_agnocast_wrapper.hpp
+
+#ifdef USE_AGNOCAST_ENABLED
+#pragma message("Building with USE_AGNOCAST_ENABLED defined")
+#else
+#pragma message("Building without USE_AGNOCAST_ENABLED defined")
+#endif
+
+#ifdef USE_AGNOCAST_ENABLED
+#include <agnocast/agnocast.hpp>
+#else
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#endif  // USE_AGNOCAST_ENABLED
+
+namespace agnocast_wrapper
+{
+/**
+ * @brief Templated types
+ */
+#ifdef USE_AGNOCAST_ENABLED
+template<typename MessageT>
+using MessagePtr = agnocast::ipc_shared_ptr<MessageT>;
+
+template<typename MessageT>
+using SubscriptionPtr = typename agnocast::Subscription<MessageT>::SharedPtr;
+
+template<typename MessageT>
+using PublisherPtr = typename agnocast::Publisher<MessageT>::SharedPtr;
+
+using SubscriptionOptions = agnocast::SubscriptionOptions;
+using PublisherOptions = agnocast::PublisherOptions;
+#else
+template<typename MessageT>
+using MessagePtr = std::shared_ptr<MessageT>;
+
+template<typename MessageT>
+using SubscriptionPtr = typename rclcpp::Subscription<MessageT>::SharedPtr;
+
+template<typename MessageT>
+using PublisherPtr = typename rclcpp::Publisher<MessageT>::SharedPtr;
+
+using SubscriptionOptions = rclcpp::SubscriptionOptions;
+using PublisherOptions = rclcpp::PublisherOptions;
+#endif  // USE_AGNOCAST_ENABLED
+
+/**
+ * @brief Create subscription
+ */
+template<typename MessageT, typename CallbackT, typename NodeT>
+auto create_subscription(
+  NodeT & node, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback,
+  const SubscriptionOptions & options = SubscriptionOptions{}) -> SubscriptionPtr<MessageT>
+{
+#ifdef USE_AGNOCAST_ENABLED
+  if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
+    return agnocast::create_subscription<MessageT>(
+      node.get(), topic_name, qos, std::forward<CallbackT>(callback), options);
+  } else {
+    return agnocast::create_subscription<MessageT>(
+      &node, topic_name, qos, std::forward<CallbackT>(callback), options);
+  }
+#else
+  if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
+    return node->template create_subscription<MessageT>(
+      topic_name, qos, std::forward<CallbackT>(callback), options);
+  } else {
+    return node.template create_subscription<MessageT>(
+      topic_name, qos, std::forward<CallbackT>(callback), options);
+  }
+#endif  // USE_AGNOCAST_ENABLED
+}
+
+/**
+ * @brief Create publisher
+ */
+template<typename MessageT, typename NodeT>
+auto create_publisher(
+  NodeT & node, const std::string & topic_name, const rclcpp::QoS & qos,
+  const PublisherOptions & options = PublisherOptions{}) -> PublisherPtr<MessageT>
+{
+#ifdef USE_AGNOCAST_ENABLED
+  if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
+    return agnocast::create_publisher<MessageT>(node.get(), topic_name, qos, options);
+  } else {
+    return agnocast::create_publisher<MessageT>(&node, topic_name, qos, options);
+  }
+#else
+  if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
+    return node->template create_publisher<MessageT>(topic_name, qos, options);
+  } else {
+    return node.template create_publisher<MessageT>(topic_name, qos, options);
+  }
+#endif  // USE_AGNOCAST_ENABLED
+}
+
+/**
+ * @brief Create message object pointer
+ * @param publisher_ptr pointer to the publisher for which the message will be
+ * created. The message type will be deduced from the passed publisher.
+ * @return pointer to the message - dependent on the publisher type
+ */
+template<typename PublisherPtrT>
+auto create_message(PublisherPtrT publisher_ptr)
+{
+#ifdef USE_AGNOCAST_ENABLED
+  return publisher_ptr->borrow_loaned_message();
+#else
+  using ROSMessageT =
+    typename std::remove_reference<decltype(*publisher_ptr)>::type::ROSMessageType;
+  return std::make_unique<ROSMessageT>();
+#endif  // USE_AGNOCAST_ENABLED
+}
+}  // namespace agnocast_wrapper
+
+#endif  // AGNOCAST_WRAPPER__AGNOCAST_WRAPPER_HPP_
diff --git a/common/agnocast_wrapper/package.xml b/common/agnocast_wrapper/package.xml
new file mode 100644
index 00000000000..dce5344c219
--- /dev/null
+++ b/common/agnocast_wrapper/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agnocast_wrapper</name>
+  <version>16.2.0</version>
+  <description>A wrapper package for agnocast</description>
+  <maintainer email="mateusz.palczuk@robotec.ai">Mateusz Palczuk</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <!-- <depend>agnocastlib</depend> agnocastlib is an optional dependency only required when USE_AGNOCAST_ENABLED is defined, therefore must be ignored here -->
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/dependency_humble.repos b/dependency_humble.repos
index ddb733fd99a..ab4e828b9c4 100644
--- a/dependency_humble.repos
+++ b/dependency_humble.repos
@@ -36,3 +36,7 @@ repositories:
     type: git
     url: https://github.com/tier4/tier4_autoware_msgs.git
     version: tier4/universe # for Autoware.Universe (TIER IV)
+  agnocast:
+    type: git
+    url: https://github.com/tier4/agnocast.git
+    version: 2.0.1
diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt
index 1330ff5affc..a7ea26851d9 100644
--- a/simulation/simple_sensor_simulator/CMakeLists.txt
+++ b/simulation/simple_sensor_simulator/CMakeLists.txt
@@ -22,6 +22,14 @@ add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS")
 find_package(ament_cmake_auto REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
+find_package(agnocast_wrapper REQUIRED)
+
+if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
+  message(WARNING "Building with agnocastlib dependency")
+  find_package(agnocastlib REQUIRED)
+else()
+  message(WARNING "Building without agnocastlib dependency")
+endif()
 
 ament_auto_find_build_dependencies()
 
@@ -63,8 +71,22 @@ target_link_libraries(simple_sensor_simulator_component
   zmq
 )
 ament_target_dependencies(simple_sensor_simulator_component
+  agnocast_wrapper
   simulation_interface
 )
+
+target_include_directories(${PROJECT_NAME}_component PRIVATE
+  ${agnocast_wrapper_INCLUDE_DIRS}
+)
+
+agnocast_wrapper_setup(${PROJECT_NAME}_component)
+if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
+ament_target_dependencies(simple_sensor_simulator_component agnocastlib)
+target_include_directories(${PROJECT_NAME}_component PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+endif()
+
 rclcpp_components_register_nodes(simple_sensor_simulator_component
   "simple_sensor_simulator::ScenarioSimulator")
 
@@ -75,6 +97,8 @@ target_link_libraries(simple_sensor_simulator_node
   simple_sensor_simulator_component
 )
 
+agnocast_wrapper_setup(${PROJECT_NAME}_node)
+
 install(TARGETS
   simple_sensor_simulator_node
   DESTINATION lib/simple_sensor_simulator
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp
index 1b78e99770d..dcd79ec15e1 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp
@@ -17,6 +17,7 @@
 
 #include <simulation_api_schema.pb.h>
 
+#include <agnocast_wrapper/agnocast_wrapper.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <memory>
 #include <queue>
@@ -58,7 +59,7 @@ class LidarSensorBase
 template <typename T>
 class LidarSensor : public LidarSensorBase
 {
-  const typename rclcpp::Publisher<T>::SharedPtr publisher_ptr_;
+  const agnocast_wrapper::PublisherPtr<T> publisher_ptr_;
 
   std::queue<std::pair<sensor_msgs::msg::PointCloud2, double>> queue_pointcloud_;
 
@@ -69,7 +70,7 @@ class LidarSensor : public LidarSensorBase
   explicit LidarSensor(
     const double current_simulation_time,
     const simulation_api_schema::LidarConfiguration & configuration,
-    const typename rclcpp::Publisher<T>::SharedPtr & publisher_ptr)
+    const agnocast_wrapper::PublisherPtr<T> & publisher_ptr)
   : LidarSensorBase(current_simulation_time, configuration), publisher_ptr_(publisher_ptr)
   {
     raycaster_.setDirection(configuration);
@@ -94,9 +95,10 @@ class LidarSensor : public LidarSensorBase
       not queue_pointcloud_.empty() and
       current_simulation_time - queue_pointcloud_.front().second >=
         configuration_.lidar_sensor_delay()) {
-      const auto pointcloud = queue_pointcloud_.front().first;
+      auto pointcloud = agnocast_wrapper::create_message(publisher_ptr_);
+      *pointcloud = queue_pointcloud_.front().first;
       queue_pointcloud_.pop();
-      publisher_ptr_->publish(pointcloud);
+      publisher_ptr_->publish(std::move(pointcloud));
     }
   }
 
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp
index 2e830f26883..94ffaad6851 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp
@@ -17,6 +17,7 @@
 
 #include <simulation_api_schema.pb.h>
 
+#include <agnocast_wrapper/agnocast_wrapper.hpp>
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <iomanip>
@@ -50,8 +51,8 @@ class SensorSimulation
     if (configuration.architecture_type().find("awf/universe") != std::string::npos) {
       lidar_sensors_.push_back(std::make_unique<LidarSensor<sensor_msgs::msg::PointCloud2>>(
         current_simulation_time, configuration,
-        node.create_publisher<sensor_msgs::msg::PointCloud2>(
-          "/perception/obstacle_segmentation/pointcloud", 1)));
+        agnocast_wrapper::create_publisher<sensor_msgs::msg::PointCloud2>(
+          node, "/perception/obstacle_segmentation/pointcloud", 1)));
     } else {
       std::stringstream ss;
       ss << "Unexpected architecture_type " << std::quoted(configuration.architecture_type())
diff --git a/simulation/simple_sensor_simulator/launch/scenario_simulator.launch b/simulation/simple_sensor_simulator/launch/scenario_simulator.launch
index 25eb8269249..117595a2acf 100644
--- a/simulation/simple_sensor_simulator/launch/scenario_simulator.launch
+++ b/simulation/simple_sensor_simulator/launch/scenario_simulator.launch
@@ -1,5 +1,14 @@
 <?xml version="1.0"?>
 <launch>
+    <!-- Agnocast -->
+    <arg name="use_system_agnocast_heaphook" default="true" description="true: apt installed library, false: locally built"/>
+    <let name="agnocast_heaphook_path" value="/opt/ros/humble/lib/libagnocast_heaphook.so" if="$(var use_system_agnocast_heaphook)"/>
+    <let name="agnocast_heaphook_path" value="$(env AGNOCAST_HEAPHOOK_PATH)" unless="$(var use_system_agnocast_heaphook)"/>
+    
+    <let name="enable_agnocast" value="$(env ENABLE_AGNOCAST_SIMULATOR 0)" />
+    <let name="current_ld_preload" value="$(env LD_PRELOAD '')" />
+    <let name="new_ld_preload" value="$(eval &quot;\'$(var agnocast_heaphook_path):$(env LD_PRELOAD '')\' if \'$(var enable_agnocast)\' == \'1\' else \'$(var current_ld_preload)\'&quot;)"/>
+    <!--  -->
     <arg name="load_map" default="true"/>
     <arg name="map_path"/>
     <arg name="lanelet2_map_file" default="lanelet2_map.osm"/>
@@ -7,10 +16,14 @@
     <arg name="port" default="9000"/>
 
     <node pkg="simple_sensor_simulator" type="simple_sensor_simulator_node" name="simple_sensor_simulator_node" output="screen" respawn="false" respawn_delay="0">
+        <env name="LD_PRELOAD" value="$(var new_ld_preload)" />
+        <env name="AGNOCAST_MEMPOOL_SIZE" value="134217728"/>
         <param name="port" value="$(arg port)"/>
     </node>
 
     <node pkg="simple_sensor_simulator" type="scenario_runner_moc_node" name="scenario_runner_moc_node" output="screen" respawn="false" respawn_delay="0">
+        <env name="LD_PRELOAD" value="$(var new_ld_preload)" />
+        <env name="AGNOCAST_MEMPOOL_SIZE" value="134217728"/>
         <param name="port" value="$(arg port)"/>
     </node>
 
diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml
index 343375d5827..1e03abadad8 100644
--- a/simulation/simple_sensor_simulator/package.xml
+++ b/simulation/simple_sensor_simulator/package.xml
@@ -50,7 +50,7 @@
   <depend>visualization_msgs</depend>
   <depend>geographic_msgs</depend>
   <depend>traffic_simulator</depend>
-
+  <depend>agnocast_wrapper</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_clang_format</test_depend>
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt
index 7d16e7f14cf..ffa9f066cc9 100644
--- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt
@@ -1,5 +1,17 @@
+# AGNOCAST is explicitly disabled for this module because the tests create both a publisher and a subscriber
+# in the same process. This setup causes issues with AGNOCAST memory handling.
+# As a temporary workaround, unit tests for lidar_sensor are only run for the ROS 2-based use case.
+set(ENV{ENABLE_AGNOCAST_SIMULATOR} "0" CACHE STRING "Set ENABLE_AGNOCAST_SIMULATOR to 0 for testing" FORCE)
+
 ament_add_gtest(test_raycaster test_raycaster.cpp)
+agnocast_wrapper_setup(test_raycaster)
 target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES})
 
 ament_add_gtest(test_lidar_sensor test_lidar_sensor.cpp)
+agnocast_wrapper_setup(test_lidar_sensor)
 target_link_libraries(test_lidar_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES})
+
+set_tests_properties(test_lidar_sensor
+  PROPERTIES
+  ENVIRONMENT "LD_PRELOAD=/opt/ros/humble/lib/libagnocast_heaphook.so:;AGNOCAST_MEMPOOL_SIZE=134217728"
+)
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp
index 2a225940738..49c39f899a2 100644
--- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp
@@ -36,7 +36,7 @@ TEST_F(LidarSensorTest, update_correct)
   // Spin the node to process callbacks
   rclcpp::spin_some(node_);
 
-  ASSERT_NE(received_msg_, nullptr);
+  ASSERT_TRUE(received_msg_);
   const auto total_num_of_points = received_msg_->width * received_msg_->height;
   EXPECT_GT(total_num_of_points, 0);
   EXPECT_EQ(received_msg_->header.frame_id, "base_link");
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp
index 9a70ef80bf1..def86acfb7c 100644
--- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp
@@ -17,6 +17,7 @@
 
 #include <gtest/gtest.h>
 
+#include <agnocast_wrapper/agnocast_wrapper.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <set>
@@ -45,14 +46,14 @@ class LidarSensorTest : public ::testing::Test
   ~LidarSensorTest() { rclcpp::shutdown(); }
 
   rclcpp::Node::SharedPtr node_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
+  agnocast_wrapper::PublisherPtr<sensor_msgs::msg::PointCloud2> publisher_;
+  agnocast_wrapper::SubscriptionPtr<sensor_msgs::msg::PointCloud2> subscription_;
 
   std::vector<EntityStatus> status_;
 
   std::unique_ptr<LidarSensorBase> lidar_;
   simulation_api_schema::LidarConfiguration config_;
-  sensor_msgs::msg::PointCloud2::SharedPtr received_msg_;
+  agnocast_wrapper::MessagePtr<sensor_msgs::msg::PointCloud2> received_msg_;
 
   double current_simulation_time_{1.0};
   rclcpp::Time current_ros_time_{1};
@@ -76,10 +77,13 @@ class LidarSensorTest : public ::testing::Test
 
   auto makeRosInterface() -> void
   {
-    publisher_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("lidar_output", 10);
-    subscription_ = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
-      "lidar_output", 10,
-      [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { received_msg_ = msg; });
+    publisher_ =
+      agnocast_wrapper::create_publisher<sensor_msgs::msg::PointCloud2>(node_, "lidar_output", 10);
+    subscription_ = agnocast_wrapper::create_subscription<sensor_msgs::msg::PointCloud2>(
+      node_, "lidar_output", 10,
+      [this](const agnocast_wrapper::MessagePtr<sensor_msgs::msg::PointCloud2> msg) {
+        received_msg_ = msg;
+      });
   }
 };
 #endif  // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_
diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp
index 0bd1f09e53f..65532e148b4 100644
--- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp
+++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp
@@ -168,6 +168,7 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time)
 void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto);
 void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header);
 
+// clang-format off
 #define DEFINE_CONVERSION(PACKAGE, TYPENAME)                               \
   auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \
   auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void
@@ -254,6 +255,7 @@ auto toMsg(
   traffic_light_bulb_state.color = convert_color(proto.color());
   traffic_light_bulb_state.confidence = proto.confidence();
 }
+// clang-format off
 
 auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex &)
   -> traffic_simulator_msgs::Vertex;
diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
index f4162f3e9b0..0afd80a8a75 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
@@ -44,6 +44,7 @@ class BehaviorPluginBase
   virtual auto update(const double current_time, const double step_time) -> void = 0;
   virtual const std::string & getCurrentAction() const = 0;
 
+  // clang-format off
 #define DEFINE_GETTER_SETTER(NAME, KEY, TYPE)      \
   virtual TYPE get##NAME() = 0;                    \
   virtual void set##NAME(const TYPE & value) = 0;  \
@@ -53,7 +54,6 @@ class BehaviorPluginBase
     return key;                                    \
   }
 
-  // clang-format off
   DEFINE_GETTER_SETTER(BehaviorParameter,                                "behavior_parameter",                             traffic_simulator_msgs::msg::BehaviorParameter)
   DEFINE_GETTER_SETTER(CanonicalizedEntityStatus,                        "canonicalized_entity_status",                    std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
   DEFINE_GETTER_SETTER(CurrentTime,                                      "current_time",                                   double)
diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
index fc36ff9104d..1aa527df507 100755
--- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
+++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
@@ -15,6 +15,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
@@ -240,6 +242,10 @@ def collect_prefixed_parameters():
             on_exit=ShutdownOnce(),
             parameters=make_parameters(),
             condition=IfCondition(launch_simple_sensor_simulator),
+            additional_env={
+                'LD_PRELOAD': f"/opt/ros/humble/lib/libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}",
+                'AGNOCAST_MEMPOOL_SIZE': '134217728',
+            }
         ),
         # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear.
         # For LifecycleNode the `name` parameter is required


Reviews:
user: login: HansRobo
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-24T10:35:09Z
user: login: HansRobo
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-24T11:38:23Z
user: login: HansRobo
body: LGTM, waiting for green CIs.
and I'll do some checks and merge this.
state: APPROVED
author_association: MEMBER
submitted_at: 2025-04-24T11:53:00Z

Review comments:
diff_hunk: @@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agnocast_wrapper</name>
+  <version>16.1.3</version>
+  <description>TODO: Package description</description>
path: common/agnocast_wrapper/package.xml
user: login: HansRobo
body: ```suggestion
  <description>A wrapper package for agnocast</description>
```
created_at: 2025-04-24T10:35:08Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agnocast_wrapper</name>
+  <version>16.1.3</version>
path: common/agnocast_wrapper/package.xml
user: login: HansRobo
body: ```suggestion
  <version>16.2.0</version>
```
created_at: 2025-04-24T11:38:23Z
author_association: MEMBER

PR comments:
user: login: github-actions[bot]
created_at: 2025-04-10T15:01:37Z
body: # Checklist for reviewers ☑️
All references to "You" in the following text refer to the code reviewer.
- [x] Is this pull request written in a way that is easy to read from a third-party perspective?
- [x] Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
- [x] If this pull request contains a destructive change, does this pull request contain the migration guide?
- [x] Labels of this pull request are valid?
- [x] All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
- [x] The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.
author_association: NONE
user: login: HansRobo
created_at: 2025-04-22T05:46:08Z
body: Sorry for updating this branch.
I was trying to resolve Release CI errors and update the branch, but I noticed that this work was being done on 15.1.1 and quickly reverted.
author_association: MEMBER
user: login: sonarqubecloud[bot]
created_at: 2025-04-24T13:34:13Z
body: ## [![Quality Gate Passed](https://sonarsource.github.io/sonarcloud-github-static-resources/v2/checks/QualityGateBadge/qg-passed-20px.png 'Quality Gate Passed')](https://sonarcloud.io/dashboard?id=tier4_scenario_simulator_v2&pullRequest=1569) **Quality Gate passed**  
Issues  
![](https://sonarsource.github.io/sonarcloud-github-static-resources/v2/common/passed-16px.png '') [0 New issues](https://sonarcloud.io/project/issues?id=tier4_scenario_simulator_v2&pullRequest=1569&issueStatuses=OPEN,CONFIRMED&sinceLeakPeriod=true)  
![](https://sonarsource.github.io/sonarcloud-github-static-resources/v2/common/accepted-16px.png '') [0 Accepted issues](https://sonarcloud.io/project/issues?id=tier4_scenario_simulator_v2&pullRequest=1569&issueStatuses=ACCEPTED)

Measures  
![](https://sonarsource.github.io/sonarcloud-github-static-resources/v2/common/passed-16px.png '') [0 Security Hotspots](https://sonarcloud.io/project/security_hotspots?id=tier4_scenario_simulator_v2&pullRequest=1569&issueStatuses=OPEN,CONFIRMED&sinceLeakPeriod=true)  
![](https://sonarsource.github.io/sonarcloud-github-static-resources/v2/common/passed-16px.png '') [28.6% Coverage on New Code](https://sonarcloud.io/component_measures?id=tier4_scenario_simulator_v2&pullRequest=1569&metric=new_coverage&view=list)  
![](https://sonarsource.github.io/sonarcloud-github-static-resources/v2/common/passed-16px.png '') [0.0% Duplication on New Code](https://sonarcloud.io/component_measures?id=tier4_scenario_simulator_v2&pullRequest=1569&metric=new_duplicated_lines_density&view=list)  
  
[See analysis details on SonarQube Cloud](https://sonarcloud.io/dashboard?id=tier4_scenario_simulator_v2&pullRequest=1569)


author_association: NONE