diff --git a/extensions/ros2/CMakeLists.txt b/extensions/ros2/CMakeLists.txt index 0d14d067..dbd21228 100644 --- a/extensions/ros2/CMakeLists.txt +++ b/extensions/ros2/CMakeLists.txt @@ -12,6 +12,7 @@ target_sources(RobotecGPULidar PRIVATE src/api/apiRos2.cpp src/graph/Ros2PublishPointsNode.cpp src/graph/Ros2PublishPointVelocityMarkersNode.cpp + src/graph/Ros2PublishRadarScanNode.cpp ) target_include_directories(RobotecGPULidar PUBLIC diff --git a/extensions/ros2/include/rgl/api/extensions/ros2.h b/extensions/ros2/include/rgl/api/extensions/ros2.h index befbe9ce..d1cc316d 100644 --- a/extensions/ros2/include/rgl/api/extensions/ros2.h +++ b/extensions/ros2/include/rgl/api/extensions/ros2.h @@ -87,3 +87,22 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, con rgl_qos_policy_reliability_t qos_reliability, rgl_qos_policy_durability_t qos_durability, rgl_qos_policy_history_t qos_history, int32_t qos_history_depth); + +/** + * Creates or modifies Ros2PublishRadarScanNode. + * The node publishes a RadarScan message to the ROS2 topic using specified Quality of Service settings. + * The message header stamp gets time from the raytraced scene. If the scene has no time, header will get the actual time. + * Graph input: point cloud + * Graph output: point cloud + * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. + * @param topic_name Topic name to publish on. + * @param frame_id Frame this data is associated with. + * @param qos_reliability QoS reliability policy. + * @param qos_durability QoS durability policy. + * @param qos_history QoS history policy. + * @param qos_history_depth QoS history depth. If history policy is KEEP_ALL, depth is ignored but must always be non-negative. + */ +RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const char* topic_name, const char* frame_id, + rgl_qos_policy_reliability_t qos_reliability, + rgl_qos_policy_durability_t qos_durability, + rgl_qos_policy_history_t qos_history, int32_t qos_history_depth); diff --git a/extensions/ros2/src/api/apiRos2.cpp b/extensions/ros2/src/api/apiRos2.cpp index 8aebb1c0..e4c1e6cc 100644 --- a/extensions/ros2/src/api/apiRos2.cpp +++ b/extensions/ros2/src/api/apiRos2.cpp @@ -53,7 +53,7 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish(rgl_node_t* node, const char* void TapeRos2::tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state) { - size_t nodeId = yamlNode[0].as(); + auto nodeId = yamlNode[0].as(); rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; rgl_node_points_ros2_publish(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str()); state.nodes.insert(std::make_pair(nodeId, node)); @@ -83,7 +83,7 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, con void TapeRos2::tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state) { - size_t nodeId = yamlNode[0].as(); + auto nodeId = yamlNode[0].as(); rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; rgl_node_points_ros2_publish_with_qos(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str(), (rgl_qos_policy_reliability_t) yamlNode[3].as(), @@ -91,4 +91,38 @@ void TapeRos2::tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode (rgl_qos_policy_history_t) yamlNode[5].as(), yamlNode[6].as()); state.nodes.insert(std::make_pair(nodeId, node)); } + +RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const char* topic_name, const char* frame_id, + rgl_qos_policy_reliability_t qos_reliability, + rgl_qos_policy_durability_t qos_durability, + rgl_qos_policy_history_t qos_history, int32_t qos_history_depth) +{ + auto status = rglSafeCall([&]() { + RGL_DEBUG( + "tape_node_publish_ros2_radarscan(node={}, topic_name={}, frame_id={}, qos_reliability={}, qos_durability={}, " + "qos_history={}, qos_history_depth={})", + repr(node), topic_name, frame_id, qos_reliability, qos_durability, qos_history, qos_history_depth); + CHECK_ARG(topic_name != nullptr); + CHECK_ARG(topic_name[0] != '\0'); + CHECK_ARG(frame_id != nullptr); + CHECK_ARG(frame_id[0] != '\0'); + CHECK_ARG(qos_history_depth >= 0); + + createOrUpdateNode(node, topic_name, frame_id, qos_reliability, qos_durability, qos_history, + qos_history_depth); + }); + TAPE_HOOK(node, topic_name, frame_id, qos_reliability, qos_durability, qos_history, qos_history_depth); + return status; +} + +void TapeRos2::tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; + rgl_node_publish_ros2_radarscan(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str(), + (rgl_qos_policy_reliability_t) yamlNode[3].as(), + (rgl_qos_policy_durability_t) yamlNode[4].as(), + (rgl_qos_policy_history_t) yamlNode[5].as(), yamlNode[6].as()); + state.nodes.insert(std::make_pair(nodeId, node)); +} } diff --git a/extensions/ros2/src/graph/NodesRos2.hpp b/extensions/ros2/src/graph/NodesRos2.hpp index 0d624e52..af370513 100644 --- a/extensions/ros2/src/graph/NodesRos2.hpp +++ b/extensions/ros2/src/graph/NodesRos2.hpp @@ -80,20 +80,19 @@ struct Ros2PublishPointVelocityMarkersNode : IPointsNodeSingleInput struct Ros2PublishRadarScanNode : IPointsNodeSingleInput { - void setParameters(const char* topicName, const char* frameId, - rgl_qos_policy_reliability_t qosReliability = QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, - rgl_qos_policy_durability_t qosDurability = QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, - rgl_qos_policy_history_t qosHistory = QOS_POLICY_HISTORY_SYSTEM_DEFAULT, int32_t qosHistoryDepth = 10) - {} + void setParameters(const char* topicName, const char* frameId, rgl_qos_policy_reliability_t qosReliability, + rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory, int32_t qosHistoryDepth); std::vector getRequiredFieldList() const override { - return {DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32}; + return {DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32, /* placeholder for amplitude */ PADDING_32}; } - void validateImpl() override {} - void enqueueExecImpl() override {} + void validateImpl() override; + void enqueueExecImpl() override; private: radar_msgs::msg::RadarScan ros2Message; rclcpp::Publisher::SharedPtr ros2Publisher; + DeviceAsyncArray::Ptr formattedData = DeviceAsyncArray::create(arrayMgr); std::shared_ptr ros2InitGuard; + GPUFieldDescBuilder fieldDescBuilder; }; diff --git a/extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp b/extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp new file mode 100644 index 00000000..7b70cf38 --- /dev/null +++ b/extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp @@ -0,0 +1,55 @@ +// Copyright 2024 Robotec.AI +// +// 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. + +#include +#include + +void Ros2PublishRadarScanNode::setParameters(const char* topicName, const char* frameId, + rgl_qos_policy_reliability_t qosReliability, + rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory, + int32_t qosHistoryDepth) +{ + ros2InitGuard = Ros2InitGuard::acquire(); + + ros2Message.header.frame_id = frameId; + + auto qos = rclcpp::QoS(qosHistoryDepth); + qos.reliability(static_cast(qosReliability)); + qos.durability(static_cast(qosDurability)); + qos.history(static_cast(qosHistory)); + ros2Publisher = ros2InitGuard->createUniquePublisher(topicName, qos); +} + +void Ros2PublishRadarScanNode::validateImpl() +{ + IPointsNodeSingleInput::validateImpl(); + if (input->getHeight() != 1) { + throw InvalidPipeline("ROS2 radar publish supports unorganized pointclouds only"); + } +} + +void Ros2PublishRadarScanNode::enqueueExecImpl() +{ + ros2Message.header.stamp = Scene::instance().getTime().has_value() ? + Scene::instance().getTime().value().asRos2Msg() : + static_cast(ros2InitGuard->getNode().get_clock()->now()); + std::vector fields = this->getRequiredFieldList(); + FormatPointsNode::formatAsync(formattedData, input, fields, fieldDescBuilder); + ros2Message.returns.resize(input->getPointCount()); + CHECK_CUDA(cudaMemcpyAsync(ros2Message.returns.data(), formattedData->getReadPtr(), + formattedData->getSizeOf() * formattedData->getCount(), cudaMemcpyDeviceToHost, + formattedData->getStream()->getHandle())); + CHECK_CUDA(cudaStreamSynchronize(formattedData->getStream()->getHandle())); + ros2Publisher->publish(ros2Message); +} diff --git a/extensions/ros2/src/tape/TapeRos2.hpp b/extensions/ros2/src/tape/TapeRos2.hpp index 5aabd910..1bc4f932 100644 --- a/extensions/ros2/src/tape/TapeRos2.hpp +++ b/extensions/ros2/src/tape/TapeRos2.hpp @@ -21,12 +21,14 @@ class TapeRos2 { static void tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state); static void tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state); // Called once in the translation unit static inline bool autoExtendTapeFunctions = std::invoke([]() { std::map tapeFunctions = { TAPE_CALL_MAPPING("rgl_node_points_ros2_publish", TapeRos2::tape_node_points_ros2_publish), TAPE_CALL_MAPPING("rgl_node_points_ros2_publish_with_qos", TapeRos2::tape_node_points_ros2_publish_with_qos), + TAPE_CALL_MAPPING("rgl_node_publish_ros2_radarscan", TapeRos2::tape_node_publish_ros2_radarscan), }; TapePlayer::extendTapeFunctions(tapeFunctions); return true; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 73dd12b9..4ea7ebb8 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -48,6 +48,12 @@ set(RGL_TEST_FILES src/scene/incidentAngleTest.cpp ) +if (RGL_BUILD_ROS2_EXTENSION) + list(APPEND RGL_TEST_FILES + src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp + ) +endif() + # Only Linux if ((NOT WIN32)) # list(APPEND RGL_TEST_FILES @@ -90,12 +96,15 @@ if (RGL_BUILD_ROS2_EXTENSION) # This allows accessing Nodes directly (as opposed to through RGL-API) # It is needed by EntityVelocity test, which makes a use of non-public Ros2 node. find_package(rclcpp REQUIRED) + find_package(radar_msgs REQUIRED) + list(REMOVE_DUPLICATES radar_msgs_LIBRARIES) target_include_directories(RobotecGPULidar_test PRIVATE ${CMAKE_SOURCE_DIR}/extensions/ros2/src ${rclcpp_INCLUDE_DIRS} ) target_link_libraries(RobotecGPULidar_test PRIVATE ${rclcpp_LIBRARIES} + ${radar_msgs_LIBRARIES} ) endif() diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp index bc2461e8..8802a41e 100644 --- a/test/src/TapeTest.cpp +++ b/test/src/TapeTest.cpp @@ -262,14 +262,15 @@ TEST_F(TapeTest, RecordPlayAllCalls) EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, 1.0f, 0.5f)); #if RGL_BUILD_ROS2_EXTENSION - rgl_node_t ros2pub = nullptr; - EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&ros2pub, "pointcloud", "rgl")); + rgl_node_t pointcloud2Pub = nullptr, radarscanPub = nullptr; + EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&pointcloud2Pub, "pointcloud", "rgl")); rgl_node_t ros2pubqos = nullptr; rgl_qos_policy_reliability_t qos_r = QOS_POLICY_RELIABILITY_BEST_EFFORT; rgl_qos_policy_durability_t qos_d = QOS_POLICY_DURABILITY_VOLATILE; rgl_qos_policy_history_t qos_h = QOS_POLICY_HISTORY_KEEP_LAST; EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish_with_qos(&ros2pubqos, "pointcloud_ex", "rgl", qos_r, qos_d, qos_h, 10)); + EXPECT_RGL_SUCCESS(rgl_node_publish_ros2_radarscan(&radarscanPub, "radarscan", "rgl", qos_r, qos_d, qos_h, 10)); #endif rgl_node_t noiseAngularRay = nullptr; diff --git a/test/src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp b/test/src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp new file mode 100644 index 00000000..2037538a --- /dev/null +++ b/test/src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp @@ -0,0 +1,71 @@ +#include +#include +#include +#include + +#include + +#include +#include +class Ros2PublishRadarScanNodeTest : public RGLTest +{}; + +TEST_F(Ros2PublishRadarScanNodeTest, should_receive_sent_data) +{ + const auto POINT_COUNT = 5; + const auto TOPIC_NAME = "rgl_test_radar_scan"; + const auto FRAME_ID = "rgl_test_frame_id"; + const auto NODE_NAME = "rgl_test_node"; + const auto WAIT_TIME_SECS = 1; + const auto MESSAGE_REPEATS = 3; + TestPointCloud input({DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32}, POINT_COUNT); + + // Create nodes + rgl_node_t inputNode = input.createUsePointsNode(), radarScanNode = nullptr; + ASSERT_RGL_SUCCESS( + rgl_node_publish_ros2_radarscan(&radarScanNode, TOPIC_NAME, FRAME_ID, QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, QOS_POLICY_HISTORY_SYSTEM_DEFAULT, 0)); + + // Connect nodes + ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(inputNode, radarScanNode)); + + // Synchronization primitives + std::atomic messageCount = 0; + + // Create ROS2 subscriber to receive RadarScan messages on topic "radar_scan" + auto node = std::make_shared(NODE_NAME, rclcpp::NodeOptions{}); + auto qos = rclcpp::QoS(10); + qos.reliability(static_cast(QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)); + qos.durability(static_cast(QOS_POLICY_DURABILITY_SYSTEM_DEFAULT)); + qos.history(static_cast(QOS_POLICY_HISTORY_SYSTEM_DEFAULT)); + auto subscriber = node->create_subscription( + TOPIC_NAME, qos, [&](const radar_msgs::msg::RadarScan::ConstSharedPtr msg) { + EXPECT_EQ(msg->returns.size(), POINT_COUNT); + EXPECT_EQ(msg->header.frame_id, FRAME_ID); + EXPECT_NE(msg->header.stamp.sec + msg->header.stamp.nanosec, 0); + + for (int i = 0; i < POINT_COUNT; ++i) { + EXPECT_EQ(msg->returns[i].range, input.getFieldValue(i)); + EXPECT_EQ(msg->returns[i].azimuth, input.getFieldValue(i)); + EXPECT_EQ(msg->returns[i].elevation, input.getFieldValue(i)); + EXPECT_EQ(msg->returns[i].doppler_velocity, input.getFieldValue(i)); + } + ++messageCount; + }); + + // Run + for (int i = 0; i < MESSAGE_REPEATS; ++i) { + ASSERT_RGL_SUCCESS(rgl_graph_run(inputNode)); + } + + // Wait for messages + { + auto start = std::chrono::steady_clock::now(); + do { + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } while (messageCount != MESSAGE_REPEATS && + std::chrono::steady_clock::now() - start < std::chrono::seconds(WAIT_TIME_SECS)); + ASSERT_EQ(messageCount, MESSAGE_REPEATS); + } +} \ No newline at end of file