Skip to content

Commit 09fceff

Browse files
HansRoboHansRobo
authored
Merge pull request #1569 from tier4/feature/agnocast
2 parents a6f0f41 + e26e14e commit 09fceff

File tree

18 files changed

+301
-17
lines changed

18 files changed

+301
-17
lines changed

‎.github/workflows/custom_spell.json

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,8 @@
4848
"xerces",
4949
"xercesc",
5050
"Szymon",
51-
"Parapura"
51+
"Parapura",
52+
"libagnocast",
53+
"agnocast"
5254
]
5355
}

‎common/agnocast_wrapper/CHANGELOG.rst

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package agnocast_wrapper
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
16.1.3 (2025-04-21)
6+
-------------------

‎common/agnocast_wrapper/CMakeLists.txt

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(agnocast_wrapper)
4+
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 17)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
find_package(ament_cmake_auto REQUIRED)
14+
15+
if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
16+
message(WARNING "Building with agnocastlib dependency")
17+
find_package(agnocastlib REQUIRED)
18+
else()
19+
message(WARNING "Building without agnocastlib dependency")
20+
endif()
21+
22+
ament_auto_find_build_dependencies()
23+
24+
if(BUILD_TESTING)
25+
find_package(ament_lint_auto REQUIRED)
26+
ament_lint_auto_find_test_dependencies()
27+
endif()
28+
29+
ament_auto_package(CONFIG_EXTRAS "cmake/agnocast_wrapper_setup_target.cmake")

‎common/agnocast_wrapper/cmake/agnocast_wrapper_setup_target.cmake

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
# Copyright 2015 TIER IV, Inc. All rights reserved.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
function(agnocast_wrapper_setup target)
16+
if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
17+
message(WARNING "agnocast_wrapper_setup: Defining USE_AGNOCAST_ENABLED for target ${target}")
18+
target_compile_definitions(${target} PRIVATE USE_AGNOCAST_ENABLED)
19+
else()
20+
message(WARNING "agnocast_wrapper_setup: Not defining USE_AGNOCAST_ENABLED for target ${target}")
21+
endif()
22+
endfunction()

‎common/agnocast_wrapper/include/agnocast_wrapper/agnocast_wrapper.hpp

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
// Copyright 2015 TIER IV, Inc. All rights reserved.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AGNOCAST_WRAPPER__AGNOCAST_WRAPPER_HPP_
16+
#define AGNOCAST_WRAPPER__AGNOCAST_WRAPPER_HPP_
17+
18+
#include <string>
19+
#include <utility>
20+
21+
/// @sa https://github.com/veqcc/autoware.universe/blob/252ae60788a8388585780a6b1935a5682c688464/common/autoware_agnocast_wrapper/include/autoware_agnocast_wrapper/autoware_agnocast_wrapper.hpp
22+
23+
#ifdef USE_AGNOCAST_ENABLED
24+
#pragma message("Building with USE_AGNOCAST_ENABLED defined")
25+
#else
26+
#pragma message("Building without USE_AGNOCAST_ENABLED defined")
27+
#endif
28+
29+
#ifdef USE_AGNOCAST_ENABLED
30+
#include <agnocast/agnocast.hpp>
31+
#else
32+
#include <memory>
33+
#include <rclcpp/rclcpp.hpp>
34+
#endif // USE_AGNOCAST_ENABLED
35+
36+
namespace agnocast_wrapper
37+
{
38+
/**
39+
* @brief Templated types
40+
*/
41+
#ifdef USE_AGNOCAST_ENABLED
42+
template<typename MessageT>
43+
using MessagePtr = agnocast::ipc_shared_ptr<MessageT>;
44+
45+
template<typename MessageT>
46+
using SubscriptionPtr = typename agnocast::Subscription<MessageT>::SharedPtr;
47+
48+
template<typename MessageT>
49+
using PublisherPtr = typename agnocast::Publisher<MessageT>::SharedPtr;
50+
51+
using SubscriptionOptions = agnocast::SubscriptionOptions;
52+
using PublisherOptions = agnocast::PublisherOptions;
53+
#else
54+
template<typename MessageT>
55+
using MessagePtr = std::shared_ptr<MessageT>;
56+
57+
template<typename MessageT>
58+
using SubscriptionPtr = typename rclcpp::Subscription<MessageT>::SharedPtr;
59+
60+
template<typename MessageT>
61+
using PublisherPtr = typename rclcpp::Publisher<MessageT>::SharedPtr;
62+
63+
using SubscriptionOptions = rclcpp::SubscriptionOptions;
64+
using PublisherOptions = rclcpp::PublisherOptions;
65+
#endif // USE_AGNOCAST_ENABLED
66+
67+
/**
68+
* @brief Create subscription
69+
*/
70+
template<typename MessageT, typename CallbackT, typename NodeT>
71+
auto create_subscription(
72+
NodeT & node, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback,
73+
const SubscriptionOptions & options = SubscriptionOptions{}) -> SubscriptionPtr<MessageT>
74+
{
75+
#ifdef USE_AGNOCAST_ENABLED
76+
if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
77+
return agnocast::create_subscription<MessageT>(
78+
node.get(), topic_name, qos, std::forward<CallbackT>(callback), options);
79+
} else {
80+
return agnocast::create_subscription<MessageT>(
81+
&node, topic_name, qos, std::forward<CallbackT>(callback), options);
82+
}
83+
#else
84+
if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
85+
return node->template create_subscription<MessageT>(
86+
topic_name, qos, std::forward<CallbackT>(callback), options);
87+
} else {
88+
return node.template create_subscription<MessageT>(
89+
topic_name, qos, std::forward<CallbackT>(callback), options);
90+
}
91+
#endif // USE_AGNOCAST_ENABLED
92+
}
93+
94+
/**
95+
* @brief Create publisher
96+
*/
97+
template<typename MessageT, typename NodeT>
98+
auto create_publisher(
99+
NodeT & node, const std::string & topic_name, const rclcpp::QoS & qos,
100+
const PublisherOptions & options = PublisherOptions{}) -> PublisherPtr<MessageT>
101+
{
102+
#ifdef USE_AGNOCAST_ENABLED
103+
if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
104+
return agnocast::create_publisher<MessageT>(node.get(), topic_name, qos, options);
105+
} else {
106+
return agnocast::create_publisher<MessageT>(&node, topic_name, qos, options);
107+
}
108+
#else
109+
if constexpr (std::is_same_v<std::decay_t<NodeT>, rclcpp::Node::SharedPtr>) {
110+
return node->template create_publisher<MessageT>(topic_name, qos, options);
111+
} else {
112+
return node.template create_publisher<MessageT>(topic_name, qos, options);
113+
}
114+
#endif // USE_AGNOCAST_ENABLED
115+
}
116+
117+
/**
118+
* @brief Create message object pointer
119+
* @param publisher_ptr pointer to the publisher for which the message will be
120+
* created. The message type will be deduced from the passed publisher.
121+
* @return pointer to the message - dependent on the publisher type
122+
*/
123+
template<typename PublisherPtrT>
124+
auto create_message(PublisherPtrT publisher_ptr)
125+
{
126+
#ifdef USE_AGNOCAST_ENABLED
127+
return publisher_ptr->borrow_loaned_message();
128+
#else
129+
using ROSMessageT =
130+
typename std::remove_reference<decltype(*publisher_ptr)>::type::ROSMessageType;
131+
return std::make_unique<ROSMessageT>();
132+
#endif // USE_AGNOCAST_ENABLED
133+
}
134+
} // namespace agnocast_wrapper
135+
136+
#endif // AGNOCAST_WRAPPER__AGNOCAST_WRAPPER_HPP_

‎common/agnocast_wrapper/package.xml

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>agnocast_wrapper</name>
5+
<version>16.2.0</version>
6+
<description>A wrapper package for agnocast</description>
7+
<maintainer email="mateusz.palczuk@robotec.ai">Mateusz Palczuk</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
12+
13+
<depend>rclcpp</depend>
14+
<!-- <depend>agnocastlib</depend> agnocastlib is an optional dependency only required when USE_AGNOCAST_ENABLED is defined, therefore must be ignored here -->
15+
<test_depend>ament_lint_auto</test_depend>
16+
<test_depend>ament_lint_common</test_depend>
17+
18+
<export>
19+
<build_type>ament_cmake</build_type>
20+
</export>
21+
</package>

‎dependency_humble.repos

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,3 +36,7 @@ repositories:
3636
type: git
3737
url: https://github.com/tier4/tier4_autoware_msgs.git
3838
version: tier4/universe # for Autoware.Universe (TIER IV)
39+
agnocast:
40+
type: git
41+
url: https://github.com/tier4/agnocast.git
42+
version: 2.0.1

‎simulation/simple_sensor_simulator/CMakeLists.txt

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,14 @@ add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS")
2222
find_package(ament_cmake_auto REQUIRED)
2323
find_package(Eigen3 REQUIRED)
2424
find_package(PCL REQUIRED)
25+
find_package(agnocast_wrapper REQUIRED)
26+
27+
if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
28+
message(WARNING "Building with agnocastlib dependency")
29+
find_package(agnocastlib REQUIRED)
30+
else()
31+
message(WARNING "Building without agnocastlib dependency")
32+
endif()
2533

2634
ament_auto_find_build_dependencies()
2735

@@ -63,8 +71,22 @@ target_link_libraries(simple_sensor_simulator_component
6371
zmq
6472
)
6573
ament_target_dependencies(simple_sensor_simulator_component
74+
agnocast_wrapper
6675
simulation_interface
6776
)
77+
78+
target_include_directories(${PROJECT_NAME}_component PRIVATE
79+
${agnocast_wrapper_INCLUDE_DIRS}
80+
)
81+
82+
agnocast_wrapper_setup(${PROJECT_NAME}_component)
83+
if(DEFINED ENV{ENABLE_AGNOCAST_SIMULATOR} AND "$ENV{ENABLE_AGNOCAST_SIMULATOR}" STREQUAL "1")
84+
ament_target_dependencies(simple_sensor_simulator_component agnocastlib)
85+
target_include_directories(${PROJECT_NAME}_component PRIVATE
86+
${agnocastlib_INCLUDE_DIRS}
87+
)
88+
endif()
89+
6890
rclcpp_components_register_nodes(simple_sensor_simulator_component
6991
"simple_sensor_simulator::ScenarioSimulator")
7092

@@ -75,6 +97,8 @@ target_link_libraries(simple_sensor_simulator_node
7597
simple_sensor_simulator_component
7698
)
7799

100+
agnocast_wrapper_setup(${PROJECT_NAME}_node)
101+
78102
install(TARGETS
79103
simple_sensor_simulator_node
80104
DESTINATION lib/simple_sensor_simulator

‎simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <simulation_api_schema.pb.h>
1919

20+
#include <agnocast_wrapper/agnocast_wrapper.hpp>
2021
#include <geometry/quaternion/get_rotation_matrix.hpp>
2122
#include <memory>
2223
#include <queue>
@@ -58,7 +59,7 @@ class LidarSensorBase
5859
template <typename T>
5960
class LidarSensor : public LidarSensorBase
6061
{
61-
const typename rclcpp::Publisher<T>::SharedPtr publisher_ptr_;
62+
const agnocast_wrapper::PublisherPtr<T> publisher_ptr_;
6263

6364
std::queue<std::pair<sensor_msgs::msg::PointCloud2, double>> queue_pointcloud_;
6465

@@ -69,7 +70,7 @@ class LidarSensor : public LidarSensorBase
6970
explicit LidarSensor(
7071
const double current_simulation_time,
7172
const simulation_api_schema::LidarConfiguration & configuration,
72-
const typename rclcpp::Publisher<T>::SharedPtr & publisher_ptr)
73+
const agnocast_wrapper::PublisherPtr<T> & publisher_ptr)
7374
: LidarSensorBase(current_simulation_time, configuration), publisher_ptr_(publisher_ptr)
7475
{
7576
raycaster_.setDirection(configuration);
@@ -94,9 +95,10 @@ class LidarSensor : public LidarSensorBase
9495
not queue_pointcloud_.empty() and
9596
current_simulation_time - queue_pointcloud_.front().second >=
9697
configuration_.lidar_sensor_delay()) {
97-
const auto pointcloud = queue_pointcloud_.front().first;
98+
auto pointcloud = agnocast_wrapper::create_message(publisher_ptr_);
99+
*pointcloud = queue_pointcloud_.front().first;
98100
queue_pointcloud_.pop();
99-
publisher_ptr_->publish(pointcloud);
101+
publisher_ptr_->publish(std::move(pointcloud));
100102
}
101103
}
102104

‎simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <simulation_api_schema.pb.h>
1919

20+
#include <agnocast_wrapper/agnocast_wrapper.hpp>
2021
#include <autoware_perception_msgs/msg/detected_objects.hpp>
2122
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
2223
#include <iomanip>
@@ -50,8 +51,8 @@ class SensorSimulation
5051
if (configuration.architecture_type().find("awf/universe") != std::string::npos) {
5152
lidar_sensors_.push_back(std::make_unique<LidarSensor<sensor_msgs::msg::PointCloud2>>(
5253
current_simulation_time, configuration,
53-
node.create_publisher<sensor_msgs::msg::PointCloud2>(
54-
"/perception/obstacle_segmentation/pointcloud", 1)));
54+
agnocast_wrapper::create_publisher<sensor_msgs::msg::PointCloud2>(
55+
node, "/perception/obstacle_segmentation/pointcloud", 1)));
5556
} else {
5657
std::stringstream ss;
5758
ss << "Unexpected architecture_type " << std::quoted(configuration.architecture_type())

‎simulation/simple_sensor_simulator/launch/scenario_simulator.launch

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,29 @@
11
<?xml version="1.0"?>
22
<launch>
3+
<!-- Agnocast -->
4+
<arg name="use_system_agnocast_heaphook" default="true" description="true: apt installed library, false: locally built"/>
5+
<let name="agnocast_heaphook_path" value="/opt/ros/humble/lib/libagnocast_heaphook.so" if="$(var use_system_agnocast_heaphook)"/>
6+
<let name="agnocast_heaphook_path" value="$(env AGNOCAST_HEAPHOOK_PATH)" unless="$(var use_system_agnocast_heaphook)"/>
7+
8+
<let name="enable_agnocast" value="$(env ENABLE_AGNOCAST_SIMULATOR 0)" />
9+
<let name="current_ld_preload" value="$(env LD_PRELOAD '')" />
10+
<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;)"/>
11+
<!-- -->
312
<arg name="load_map" default="true"/>
413
<arg name="map_path"/>
514
<arg name="lanelet2_map_file" default="lanelet2_map.osm"/>
615
<arg name="pointcloud_map_file" default="pointcloud_map.pcd"/>
716
<arg name="port" default="9000"/>
817

918
<node pkg="simple_sensor_simulator" type="simple_sensor_simulator_node" name="simple_sensor_simulator_node" output="screen" respawn="false" respawn_delay="0">
19+
<env name="LD_PRELOAD" value="$(var new_ld_preload)" />
20+
<env name="AGNOCAST_MEMPOOL_SIZE" value="134217728"/>
1021
<param name="port" value="$(arg port)"/>
1122
</node>
1223

1324
<node pkg="simple_sensor_simulator" type="scenario_runner_moc_node" name="scenario_runner_moc_node" output="screen" respawn="false" respawn_delay="0">
25+
<env name="LD_PRELOAD" value="$(var new_ld_preload)" />
26+
<env name="AGNOCAST_MEMPOOL_SIZE" value="134217728"/>
1427
<param name="port" value="$(arg port)"/>
1528
</node>
1629

‎simulation/simple_sensor_simulator/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@
5050
<depend>visualization_msgs</depend>
5151
<depend>geographic_msgs</depend>
5252
<depend>traffic_simulator</depend>
53-
53+
<depend>agnocast_wrapper</depend>
5454

5555
<test_depend>ament_lint_auto</test_depend>
5656
<test_depend>ament_cmake_clang_format</test_depend>

‎simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,17 @@
1+
# AGNOCAST is explicitly disabled for this module because the tests create both a publisher and a subscriber
2+
# in the same process. This setup causes issues with AGNOCAST memory handling.
3+
# As a temporary workaround, unit tests for lidar_sensor are only run for the ROS 2-based use case.
4+
set(ENV{ENABLE_AGNOCAST_SIMULATOR} "0" CACHE STRING "Set ENABLE_AGNOCAST_SIMULATOR to 0 for testing" FORCE)
5+
16
ament_add_gtest(test_raycaster test_raycaster.cpp)
7+
agnocast_wrapper_setup(test_raycaster)
28
target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES})
39

410
ament_add_gtest(test_lidar_sensor test_lidar_sensor.cpp)
11+
agnocast_wrapper_setup(test_lidar_sensor)
512
target_link_libraries(test_lidar_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES})
13+
14+
set_tests_properties(test_lidar_sensor
15+
PROPERTIES
16+
ENVIRONMENT "LD_PRELOAD=/opt/ros/humble/lib/libagnocast_heaphook.so:;AGNOCAST_MEMPOOL_SIZE=134217728"
17+
)

0 commit comments

Comments
 (0)