Skip to content

Commit 71dfc94

Browse files
nebraszkanebraszka
committed
Implement rgl_node_publish_ros2_laserscan
1 parent 6fb6224 commit 71dfc94

File tree

7 files changed

+171
-1
lines changed

7 files changed

+171
-1
lines changed

‎extensions/ros2/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@ target_sources(RobotecGPULidar PRIVATE
1313
src/graph/Ros2PublishPointsNode.cpp
1414
src/graph/Ros2PublishPointVelocityMarkersNode.cpp
1515
src/graph/Ros2PublishRadarScanNode.cpp
16+
src/graph/Ros2PublishLaserScanNode.cpp
1617
)
1718

1819
target_include_directories(RobotecGPULidar PUBLIC

‎extensions/ros2/include/rgl/api/extensions/ros2.h‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,3 +106,27 @@ RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const cha
106106
rgl_qos_policy_reliability_t qos_reliability,
107107
rgl_qos_policy_durability_t qos_durability,
108108
rgl_qos_policy_history_t qos_history, int32_t qos_history_depth);
109+
110+
/**
111+
* Creates or modifies Ros2PublishLaserScanNode.
112+
* The node publishes a LaserScan message to the ROS2 topic using default Quality of Service settings.
113+
* TODO
114+
* Graph input: FormatNode
115+
* Graph output: point cloud
116+
* @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified.
117+
* @param topic_name Topic name to publish on.
118+
* @param frame_id Frame this data is associated with.
119+
*/
120+
RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id);
121+
122+
/**
123+
* Creates or modifies Ros2PublishLaserScanNode.
124+
* The node publishes a LaserScan message to the ROS2 topic using default Quality of Service settings.
125+
* TODO
126+
* Graph input: FormatNode
127+
* Graph output: point cloud
128+
* @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified.
129+
* @param topic_name Topic name to publish on.
130+
* @param frame_id Frame this data is associated with.
131+
*/
132+
RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id);

‎extensions/ros2/src/api/apiRos2.cpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -125,4 +125,27 @@ void TapeRos2::tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, Play
125125
(rgl_qos_policy_history_t) yamlNode[5].as<int>(), yamlNode[6].as<int32_t>());
126126
state.nodes.insert(std::make_pair(nodeId, node));
127127
}
128+
129+
RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id)
130+
{
131+
auto status = rglSafeCall([&]() {
132+
RGL_DEBUG("rgl_node_publish_ros2_laserscan(node={}, topic_name={}, frame_id={})", repr(node), topic_name, frame_id);
133+
CHECK_ARG(topic_name != nullptr);
134+
CHECK_ARG(topic_name[0] != '\0');
135+
CHECK_ARG(frame_id != nullptr);
136+
CHECK_ARG(frame_id[0] != '\0');
137+
138+
createOrUpdateNode<Ros2PublishLaserScanNode>(node, topic_name, frame_id);
139+
});
140+
TAPE_HOOK(node, topic_name, frame_id);
141+
return status;
142+
}
143+
144+
void TapeRos2::tape_node_publish_ros2_laserscan(const YAML::Node& yamlNode, PlaybackState& state)
145+
{
146+
auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
147+
rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr;
148+
rgl_node_publish_ros2_laserscan(&node, yamlNode[1].as<std::string>().c_str(), yamlNode[2].as<std::string>().c_str());
149+
state.nodes.insert(std::make_pair(nodeId, node));
150+
}
128151
}

‎extensions/ros2/src/graph/NodesRos2.hpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <visualization_msgs/msg/marker_array.hpp>
2727
#include <Ros2InitGuard.hpp>
2828
#include <radar_msgs/msg/radar_scan.hpp>
29+
#include <sensor_msgs/msg/laser_scan.hpp>
2930

3031
struct Ros2Node : IPointsNodeSingleInput
3132
{
@@ -122,3 +123,31 @@ struct Ros2PublishRadarScanNode : Ros2Node
122123
DeviceAsyncArray<char>::Ptr formattedData = DeviceAsyncArray<char>::create(arrayMgr);
123124
GPUFieldDescBuilder fieldDescBuilder;
124125
};
126+
127+
struct Ros2PublishLaserScanNode : Ros2Node
128+
{
129+
void setParameters(const char* topicName, const char* frameId,
130+
rgl_qos_policy_reliability_t qosReliability = QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
131+
rgl_qos_policy_durability_t qosDurability = QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
132+
rgl_qos_policy_history_t qosHistory = QOS_POLICY_HISTORY_SYSTEM_DEFAULT, int32_t qosHistoryDepth = 10);
133+
134+
std::vector<rgl_field_t> getRequiredFieldList() const override
135+
{
136+
return {AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32};
137+
}
138+
139+
// Ros2Node
140+
void ros2ValidateImpl() override;
141+
void ros2EnqueueExecImpl() override;
142+
143+
private:
144+
DeviceAsyncArray<char>::Ptr inputFmtData = DeviceAsyncArray<char>::create(arrayMgr);
145+
146+
sensor_msgs::msg::LaserScan ros2Message;
147+
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr ros2Publisher;
148+
149+
HostPinnedArray<Field<AZIMUTH_F32>::type>::Ptr angles = HostPinnedArray<Field<AZIMUTH_F32>::type>::create();
150+
HostPinnedArray<Field<TIME_STAMP_F64>::type>::Ptr times = HostPinnedArray<Field<TIME_STAMP_F64>::type>::create();
151+
HostPinnedArray<Field<DISTANCE_F32>::type>::Ptr ranges = HostPinnedArray<Field<DISTANCE_F32>::type>::create();
152+
HostPinnedArray<Field<INTENSITY_F32>::type>::Ptr intensities = HostPinnedArray<Field<INTENSITY_F32>::type>::create();
153+
};

‎extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
// Copyright 2024 Robotec.AI
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+
#include <graph/NodesRos2.hpp>
16+
#include <scene/Scene.hpp>
17+
#include <RGLFields.hpp>
18+
19+
void Ros2PublishLaserScanNode::setParameters(const char* topicName, const char* frameId,
20+
rgl_qos_policy_reliability_t qosReliability,
21+
rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory,
22+
int32_t qosHistoryDepth)
23+
{
24+
ros2Message.header.frame_id = frameId;
25+
auto qos = rclcpp::QoS(qosHistoryDepth);
26+
qos.reliability(static_cast<rmw_qos_reliability_policy_t>(qosReliability));
27+
qos.durability(static_cast<rmw_qos_durability_policy_t>(qosDurability));
28+
qos.history(static_cast<rmw_qos_history_policy_t>(qosHistory));
29+
ros2Publisher = ros2InitGuard->createUniquePublisher<sensor_msgs::msg::LaserScan>(topicName, qos);
30+
}
31+
32+
void Ros2PublishLaserScanNode::ros2ValidateImpl()
33+
{
34+
if (input->getHeight() != 1) {
35+
throw InvalidPipeline("ROS2 publish supports unorganized pointclouds only");
36+
}
37+
}
38+
39+
void Ros2PublishLaserScanNode::ros2EnqueueExecImpl()
40+
{
41+
const auto currentTime = Scene::instance().getTime();
42+
ros2Message.header.stamp = currentTime.has_value() ?
43+
currentTime.value().asRos2Msg() :
44+
static_cast<builtin_interfaces::msg::Time>(ros2InitGuard->getNode().get_clock()->now());
45+
46+
const size_t pointCount = input->getPointCount();
47+
48+
angles->copyFrom(input->getFieldDataTyped<AZIMUTH_F32>());
49+
auto minAng = angles->at(0);
50+
auto maxAng = minAng;
51+
for (size_t i = 1; i < pointCount; ++i) {
52+
if (angles->at(i) < minAng) {
53+
minAng = angles->at(i);
54+
}
55+
if (angles->at(i) > maxAng) {
56+
maxAng = angles->at(i);
57+
}
58+
}
59+
ros2Message.angle_min = minAng;
60+
ros2Message.angle_max = maxAng;
61+
ros2Message.angle_increment = angles->at(1) - angles->at(0);
62+
63+
times->copyFrom(input->getFieldDataTyped<TIME_STAMP_F64>());
64+
ros2Message.time_increment = static_cast<float>(times->at(1) - times->at(0));
65+
ros2Message.scan_time = static_cast<float>(times->at(pointCount - 1) - times->at(0));
66+
67+
ranges->copyFrom(input->getFieldDataTyped<DISTANCE_F32>());
68+
ros2Message.ranges.resize(input->getPointCount());
69+
size_t size = pointCount * Field<DISTANCE_F32>::size;
70+
CHECK_CUDA(cudaMemcpyAsync(ros2Message.ranges.data(), ranges->getRawReadPtr(), size, cudaMemcpyDefault, getStreamHandle()));
71+
CHECK_CUDA(cudaStreamSynchronize(getStreamHandle()));
72+
73+
auto minRange = ranges->at(0);
74+
auto maxRange = minRange;
75+
for (size_t i = 1; i < pointCount; ++i) {
76+
if (ranges->at(i) < minRange) {
77+
minRange = ranges->at(i);
78+
}
79+
if (ranges->at(i) > maxRange) {
80+
maxRange = ranges->at(i);
81+
}
82+
}
83+
84+
intensities->copyFrom(input->getFieldDataTyped<INTENSITY_F32>());
85+
ros2Message.intensities.resize(pointCount);
86+
size = pointCount * Field<INTENSITY_F32>::size;
87+
CHECK_CUDA(cudaMemcpyAsync(ros2Message.intensities.data(), intensities->getRawReadPtr(), size, cudaMemcpyDefault,
88+
getStreamHandle()));
89+
CHECK_CUDA(cudaStreamSynchronize(getStreamHandle()));
90+
91+
ros2Publisher->publish(ros2Message);
92+
}

‎extensions/ros2/src/graph/Ros2PublishPointsNode.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,6 @@ void Ros2PublishPointsNode::ros2EnqueueExecImpl()
6262
ros2Publisher->publish(ros2Message);
6363
}
6464

65-
6665
void Ros2PublishPointsNode::updateRos2Message(const std::vector<rgl_field_t>& fields, bool isDense)
6766
{
6867
ros2Message.fields.clear();

‎extensions/ros2/src/tape/TapeRos2.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,13 +22,15 @@ class TapeRos2
2222
static void tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state);
2323
static void tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state);
2424
static void tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state);
25+
static void tape_node_publish_ros2_laserscan(const YAML::Node& yamlNode, PlaybackState& state);
2526

2627
// Called once in the translation unit
2728
static inline bool autoExtendTapeFunctions = std::invoke([]() {
2829
std::map<std::string, TapeFunction> tapeFunctions = {
2930
TAPE_CALL_MAPPING("rgl_node_points_ros2_publish", TapeRos2::tape_node_points_ros2_publish),
3031
TAPE_CALL_MAPPING("rgl_node_points_ros2_publish_with_qos", TapeRos2::tape_node_points_ros2_publish_with_qos),
3132
TAPE_CALL_MAPPING("rgl_node_publish_ros2_radarscan", TapeRos2::tape_node_publish_ros2_radarscan),
33+
TAPE_CALL_MAPPING("tape_node_publish_ros2_laserscan", TapeRos2::tape_node_publish_ros2_laserscan)
3234
};
3335
TapePlayer::extendTapeFunctions(tapeFunctions);
3436
return true;

0 commit comments

Comments
 (0)