From 71dfc94f4a23ee9500a4b4346e5fba0b33e2e504 Mon Sep 17 00:00:00 2001 From: nebraszka Date: Tue, 26 Mar 2024 12:36:46 +0100 Subject: [PATCH 1/3] Implement rgl_node_publish_ros2_laserscan --- extensions/ros2/CMakeLists.txt | 1 + .../ros2/include/rgl/api/extensions/ros2.h | 24 +++++ extensions/ros2/src/api/apiRos2.cpp | 23 +++++ extensions/ros2/src/graph/NodesRos2.hpp | 29 ++++++ .../src/graph/Ros2PublishLaserScanNode.cpp | 92 +++++++++++++++++++ .../ros2/src/graph/Ros2PublishPointsNode.cpp | 1 - extensions/ros2/src/tape/TapeRos2.hpp | 2 + 7 files changed, 171 insertions(+), 1 deletion(-) create mode 100644 extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp diff --git a/extensions/ros2/CMakeLists.txt b/extensions/ros2/CMakeLists.txt index dbd212286..b7dda4dc8 100644 --- a/extensions/ros2/CMakeLists.txt +++ b/extensions/ros2/CMakeLists.txt @@ -13,6 +13,7 @@ target_sources(RobotecGPULidar PRIVATE src/graph/Ros2PublishPointsNode.cpp src/graph/Ros2PublishPointVelocityMarkersNode.cpp src/graph/Ros2PublishRadarScanNode.cpp + src/graph/Ros2PublishLaserScanNode.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 d1cc316db..da63fe2b7 100644 --- a/extensions/ros2/include/rgl/api/extensions/ros2.h +++ b/extensions/ros2/include/rgl/api/extensions/ros2.h @@ -106,3 +106,27 @@ RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const cha 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 Ros2PublishLaserScanNode. + * The node publishes a LaserScan message to the ROS2 topic using default Quality of Service settings. + * TODO + * Graph input: FormatNode + * 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. + */ +RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id); + +/** + * Creates or modifies Ros2PublishLaserScanNode. + * The node publishes a LaserScan message to the ROS2 topic using default Quality of Service settings. + * TODO + * Graph input: FormatNode + * 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. + */ +RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id); \ No newline at end of file diff --git a/extensions/ros2/src/api/apiRos2.cpp b/extensions/ros2/src/api/apiRos2.cpp index e4c1e6cc8..b3a331f50 100644 --- a/extensions/ros2/src/api/apiRos2.cpp +++ b/extensions/ros2/src/api/apiRos2.cpp @@ -125,4 +125,27 @@ void TapeRos2::tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, Play (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_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id) +{ + auto status = rglSafeCall([&]() { + RGL_DEBUG("rgl_node_publish_ros2_laserscan(node={}, topic_name={}, frame_id={})", repr(node), topic_name, frame_id); + CHECK_ARG(topic_name != nullptr); + CHECK_ARG(topic_name[0] != '\0'); + CHECK_ARG(frame_id != nullptr); + CHECK_ARG(frame_id[0] != '\0'); + + createOrUpdateNode(node, topic_name, frame_id); + }); + TAPE_HOOK(node, topic_name, frame_id); + return status; +} + +void TapeRos2::tape_node_publish_ros2_laserscan(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_laserscan(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str()); + 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 80db5e769..9ab2b93f9 100644 --- a/extensions/ros2/src/graph/NodesRos2.hpp +++ b/extensions/ros2/src/graph/NodesRos2.hpp @@ -26,6 +26,7 @@ #include #include #include +#include struct Ros2Node : IPointsNodeSingleInput { @@ -122,3 +123,31 @@ struct Ros2PublishRadarScanNode : Ros2Node DeviceAsyncArray::Ptr formattedData = DeviceAsyncArray::create(arrayMgr); GPUFieldDescBuilder fieldDescBuilder; }; + +struct Ros2PublishLaserScanNode : Ros2Node +{ + 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); + + std::vector getRequiredFieldList() const override + { + return {AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32}; + } + + // Ros2Node + void ros2ValidateImpl() override; + void ros2EnqueueExecImpl() override; + +private: + DeviceAsyncArray::Ptr inputFmtData = DeviceAsyncArray::create(arrayMgr); + + sensor_msgs::msg::LaserScan ros2Message; + rclcpp::Publisher::SharedPtr ros2Publisher; + + HostPinnedArray::type>::Ptr angles = HostPinnedArray::type>::create(); + HostPinnedArray::type>::Ptr times = HostPinnedArray::type>::create(); + HostPinnedArray::type>::Ptr ranges = HostPinnedArray::type>::create(); + HostPinnedArray::type>::Ptr intensities = HostPinnedArray::type>::create(); +}; diff --git a/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp b/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp new file mode 100644 index 000000000..3b0da868d --- /dev/null +++ b/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp @@ -0,0 +1,92 @@ +// 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 +#include + +void Ros2PublishLaserScanNode::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) +{ + 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 Ros2PublishLaserScanNode::ros2ValidateImpl() +{ + if (input->getHeight() != 1) { + throw InvalidPipeline("ROS2 publish supports unorganized pointclouds only"); + } +} + +void Ros2PublishLaserScanNode::ros2EnqueueExecImpl() +{ + const auto currentTime = Scene::instance().getTime(); + ros2Message.header.stamp = currentTime.has_value() ? + currentTime.value().asRos2Msg() : + static_cast(ros2InitGuard->getNode().get_clock()->now()); + + const size_t pointCount = input->getPointCount(); + + angles->copyFrom(input->getFieldDataTyped()); + auto minAng = angles->at(0); + auto maxAng = minAng; + for (size_t i = 1; i < pointCount; ++i) { + if (angles->at(i) < minAng) { + minAng = angles->at(i); + } + if (angles->at(i) > maxAng) { + maxAng = angles->at(i); + } + } + ros2Message.angle_min = minAng; + ros2Message.angle_max = maxAng; + ros2Message.angle_increment = angles->at(1) - angles->at(0); + + times->copyFrom(input->getFieldDataTyped()); + ros2Message.time_increment = static_cast(times->at(1) - times->at(0)); + ros2Message.scan_time = static_cast(times->at(pointCount - 1) - times->at(0)); + + ranges->copyFrom(input->getFieldDataTyped()); + ros2Message.ranges.resize(input->getPointCount()); + size_t size = pointCount * Field::size; + CHECK_CUDA(cudaMemcpyAsync(ros2Message.ranges.data(), ranges->getRawReadPtr(), size, cudaMemcpyDefault, getStreamHandle())); + CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); + + auto minRange = ranges->at(0); + auto maxRange = minRange; + for (size_t i = 1; i < pointCount; ++i) { + if (ranges->at(i) < minRange) { + minRange = ranges->at(i); + } + if (ranges->at(i) > maxRange) { + maxRange = ranges->at(i); + } + } + + intensities->copyFrom(input->getFieldDataTyped()); + ros2Message.intensities.resize(pointCount); + size = pointCount * Field::size; + CHECK_CUDA(cudaMemcpyAsync(ros2Message.intensities.data(), intensities->getRawReadPtr(), size, cudaMemcpyDefault, + getStreamHandle())); + CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); + + ros2Publisher->publish(ros2Message); +} diff --git a/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp b/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp index 573307157..9d0c99027 100644 --- a/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp +++ b/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp @@ -62,7 +62,6 @@ void Ros2PublishPointsNode::ros2EnqueueExecImpl() ros2Publisher->publish(ros2Message); } - void Ros2PublishPointsNode::updateRos2Message(const std::vector& fields, bool isDense) { ros2Message.fields.clear(); diff --git a/extensions/ros2/src/tape/TapeRos2.hpp b/extensions/ros2/src/tape/TapeRos2.hpp index 1bc4f9325..b42bb2932 100644 --- a/extensions/ros2/src/tape/TapeRos2.hpp +++ b/extensions/ros2/src/tape/TapeRos2.hpp @@ -22,6 +22,7 @@ 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); + static void tape_node_publish_ros2_laserscan(const YAML::Node& yamlNode, PlaybackState& state); // Called once in the translation unit static inline bool autoExtendTapeFunctions = std::invoke([]() { @@ -29,6 +30,7 @@ class TapeRos2 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), + TAPE_CALL_MAPPING("tape_node_publish_ros2_laserscan", TapeRos2::tape_node_publish_ros2_laserscan) }; TapePlayer::extendTapeFunctions(tapeFunctions); return true; From 8623e4c0517aff179e5f157988d5837e3ce65f9f Mon Sep 17 00:00:00 2001 From: nebraszka Date: Thu, 28 Mar 2024 12:39:24 +0100 Subject: [PATCH 2/3] Implement tests --- .../ros2/include/rgl/api/extensions/ros2.h | 16 +-- .../src/graph/Ros2PublishLaserScanNode.cpp | 2 + test/CMakeLists.txt | 5 + .../nodes/Ros2PublishLaserScanNodeTest.cpp | 116 ++++++++++++++++++ 4 files changed, 125 insertions(+), 14 deletions(-) create mode 100644 test/src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp diff --git a/extensions/ros2/include/rgl/api/extensions/ros2.h b/extensions/ros2/include/rgl/api/extensions/ros2.h index da63fe2b7..c2ef705cd 100644 --- a/extensions/ros2/include/rgl/api/extensions/ros2.h +++ b/extensions/ros2/include/rgl/api/extensions/ros2.h @@ -110,20 +110,8 @@ RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const cha /** * Creates or modifies Ros2PublishLaserScanNode. * The node publishes a LaserScan message to the ROS2 topic using default Quality of Service settings. - * TODO - * Graph input: FormatNode - * 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. - */ -RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id); - -/** - * Creates or modifies Ros2PublishLaserScanNode. - * The node publishes a LaserScan message to the ROS2 topic using default Quality of Service settings. - * TODO - * Graph input: FormatNode + * 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. diff --git a/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp b/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp index 3b0da868d..14a01ef90 100644 --- a/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp +++ b/extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp @@ -80,6 +80,8 @@ void Ros2PublishLaserScanNode::ros2EnqueueExecImpl() maxRange = ranges->at(i); } } + ros2Message.range_min = minRange; + ros2Message.range_max = maxRange; intensities->copyFrom(input->getFieldDataTyped()); ros2Message.intensities.resize(pointCount); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 45ceec849..a1dd2dbbe 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -55,6 +55,8 @@ if (RGL_BUILD_ROS2_EXTENSION) src/graph/Ros2NodesTest.cpp src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp src/graph/nodes/Ros2PublishPointsNodeTest.cpp + src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp + src/scene/rcsAngleDistributionTest.cpp src/scene/radarTest.cpp ) endif() @@ -108,7 +110,9 @@ if (RGL_BUILD_ROS2_EXTENSION) # It is needed by EntityVelocity test, which makes a use of non-public Ros2 node. find_package(rclcpp REQUIRED) find_package(radar_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) list(REMOVE_DUPLICATES radar_msgs_LIBRARIES) + list(REMOVE_DUPLICATES sensor_msgs_LIBRARIES) target_include_directories(RobotecGPULidar_test PRIVATE ${CMAKE_SOURCE_DIR}/extensions/ros2/src ${rclcpp_INCLUDE_DIRS} @@ -116,6 +120,7 @@ if (RGL_BUILD_ROS2_EXTENSION) target_link_libraries(RobotecGPULidar_test PRIVATE ${rclcpp_LIBRARIES} ${radar_msgs_LIBRARIES} + ${sensor_msgs_LIBRARIES} ) endif() diff --git a/test/src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp b/test/src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp new file mode 100644 index 000000000..71302b434 --- /dev/null +++ b/test/src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp @@ -0,0 +1,116 @@ +#include +#include +#include + +#include + +#include +#include + +class Ros2PublishLaserScanNodeTest : public RGLTest +{}; + +TEST_F(Ros2PublishLaserScanNodeTest, should_throw_invalid_pipeline_when_ros2_shutdown_laserscan) +{ + std::vector fields{AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32}; + TestPointCloud pointCloud(fields, 10); + rgl_node_t points = pointCloud.createUsePointsNode(); + + rgl_node_t yield = nullptr; + EXPECT_RGL_SUCCESS(rgl_node_points_yield(&yield, fields.data(), fields.size())); + + rgl_node_t ros2pub = nullptr; + + EXPECT_RGL_SUCCESS(rgl_node_publish_ros2_laserscan(&ros2pub, "laserscan", "rglFrame")); + + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(points, yield)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(yield, ros2pub)); + + EXPECT_RGL_SUCCESS(rgl_graph_run(points)); + + rclcpp::shutdown(); + EXPECT_RGL_INVALID_PIPELINE(rgl_graph_run(points), "Unable to execute Ros2Node because ROS2 has been shut down."); +} + +TEST_F(Ros2PublishLaserScanNodeTest, should_receive_sent_data) +{ + const auto POINT_COUNT = 5; + const auto TOPIC_NAME = "rgl_test_laser_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; + const auto START_ANGLE = 0.8f; + const auto ANGLE_INCREMENT = 0.11f; + const auto TIME_INCREMENT = 0.22f; + + TestPointCloud input({AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32}, POINT_COUNT); + + // Expected data + const auto expectedAngles = generateFieldValues( + POINT_COUNT, std::function::type(int)>( + [START_ANGLE, ANGLE_INCREMENT](int i) { return START_ANGLE + i * ANGLE_INCREMENT; })); + input.setFieldValues(expectedAngles); + const auto expectedTimes = generateFieldValues(POINT_COUNT, std::function::type(int)>( + [TIME_INCREMENT](int i) { return i * TIME_INCREMENT; })); + input.setFieldValues(expectedTimes); + const auto expectedRanges = input.getFieldValues(); + const auto expectedIntensities = input.getFieldValues(); + const auto [minRange, maxRange] = std::minmax_element(expectedRanges.begin(), expectedRanges.end()); + const auto expectedScanTime = expectedTimes.at(expectedTimes.size() - 1) - expectedTimes.at(0); + + // Create nodes + rgl_node_t inputNode = input.createUsePointsNode(), laserScanNode = nullptr; + ASSERT_RGL_SUCCESS(rgl_node_publish_ros2_laserscan(&laserScanNode, TOPIC_NAME, FRAME_ID)); + + // Connect nodes + ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(inputNode, laserScanNode)); + + // Synchronization primitives + std::atomic messageCount = 0; + + + // Create ROS2 subscriber to receive LaserScan messages on topic "laser_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 sensor_msgs::msg::LaserScan::ConstSharedPtr msg) { + EXPECT_EQ(msg->header.frame_id, FRAME_ID); + EXPECT_NE(msg->header.stamp.sec + msg->header.stamp.nanosec, 0); + + EXPECT_NEAR(msg->angle_min, START_ANGLE, EPSILON_F); + EXPECT_NEAR(msg->angle_max, START_ANGLE + (POINT_COUNT - 1) * ANGLE_INCREMENT, EPSILON_F); + EXPECT_NEAR(msg->range_min, *minRange, EPSILON_F); + EXPECT_NEAR(msg->range_max, *maxRange, EPSILON_F); + EXPECT_NEAR(msg->angle_increment, ANGLE_INCREMENT, EPSILON_F); + EXPECT_NEAR(msg->time_increment, TIME_INCREMENT, EPSILON_F); + EXPECT_NEAR(msg->scan_time, expectedScanTime, EPSILON_F); + + ASSERT_EQ(msg->ranges.size(), POINT_COUNT); + ASSERT_EQ(msg->intensities.size(), POINT_COUNT); + for (int i = 0; i < POINT_COUNT; ++i) { + EXPECT_NEAR(msg->ranges[i], expectedRanges[i], EPSILON_F); + EXPECT_NEAR(msg->intensities[i], expectedIntensities[i], EPSILON_F); + } + ++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 From 8e1546db3dd64bc46ea9c8748f7682e17142374a Mon Sep 17 00:00:00 2001 From: nebraszka Date: Thu, 28 Mar 2024 13:02:38 +0100 Subject: [PATCH 3/3] Implement tests --- test/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a1dd2dbbe..6313a43d0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -56,7 +56,6 @@ if (RGL_BUILD_ROS2_EXTENSION) src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp src/graph/nodes/Ros2PublishPointsNodeTest.cpp src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp - src/scene/rcsAngleDistributionTest.cpp src/scene/radarTest.cpp ) endif()