Skip to content

Commit 990a453

Browse files
SteveMacenskiSteveMacenski
authored
Merge pull request #22 from SteveMacenski/scan_processor
Scan processor
2 parents fb38376 + b642619 commit 990a453

File tree

5 files changed

+286
-14
lines changed

5 files changed

+286
-14
lines changed

‎README.md

+14-13
Original file line numberDiff line numberDiff line change
@@ -25,19 +25,20 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md).
2525

2626
<center>
2727

28-
| Topic | Type |
29-
|----------------------|-------------------------|
30-
| `range_image` | sensor_msgs/Image |
31-
| `intensity_image` | sensor_msgs/Image |
32-
| `noise_image` | sensor_msgs/Image |
33-
| `reflectivity_image` | sensor_msgs/Image |
34-
| `points` | sensor_msgs/PointCloud2 |
35-
| `imu` | sensor_msgs/Imu |
36-
37-
| Service | Type |
38-
|-------------------|-------------------------|
39-
| `reset` | std_srvs/Empty |
40-
| `GetMetadata` | ouster_msgs/GetMetadata |
28+
| Topic | Type | Description |
29+
|----------------------|-------------------------|--------------------------------------------------|
30+
| `scan` | sensor_msgs/LaserScan | 2D laser scan of the 0-angle ring |
31+
| `range_image` | sensor_msgs/Image | Image of the range values from the sensor |
32+
| `intensity_image` | sensor_msgs/Image | Image of the Intensity values from the sensor |
33+
| `noise_image` | sensor_msgs/Image | Image of the noise values from the sensor |
34+
| `reflectivity_image` | sensor_msgs/Image | Image of the reflectivity values from the sensor |
35+
| `points` | sensor_msgs/PointCloud2 | 3D Pointcloud generated from a 360 rotation |
36+
| `imu` | sensor_msgs/Imu | IMU values at transmission rate |
37+
38+
| Service | Type | Description |
39+
|-------------------|-------------------------|-----------------------------------|
40+
| `reset` | std_srvs/Empty | Reset the sensor's connection |
41+
| `GetMetadata` | ouster_msgs/GetMetadata | Get information about the sensor |
4142

4243
| Parameter | Type | Description |
4344
|-------------------|-------------------------|-----------------------------------------------------|

‎ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp

+18-1
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "ros2_ouster/OS1/processors/image_processor.hpp"
2323
#include "ros2_ouster/OS1/processors/imu_processor.hpp"
2424
#include "ros2_ouster/OS1/processors/pointcloud_processor.hpp"
25+
#include "ros2_ouster/OS1/processors/scan_processor.hpp"
2526

2627
namespace ros2_ouster
2728
{
@@ -54,7 +55,7 @@ inline ros2_ouster::DataProcessorInterface * createPointcloudProcessor(
5455

5556
/**
5657
* @brief Factory method to get a pointer to a processor
57-
* to create the IMU, image, and pointcloud interfaces
58+
* to create the IMU interfaces
5859
* @return Raw pointer to a data processor interface to use
5960
*/
6061
inline ros2_ouster::DataProcessorInterface * createIMUProcessor(
@@ -65,6 +66,19 @@ inline ros2_ouster::DataProcessorInterface * createIMUProcessor(
6566
return new OS1::IMUProcessor(node, mdata, frame);
6667
}
6768

69+
/**
70+
* @brief Factory method to get a pointer to a processor
71+
* to create the scan interfaces
72+
* @return Raw pointer to a data processor interface to use
73+
*/
74+
inline ros2_ouster::DataProcessorInterface * createScanProcessor(
75+
const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
76+
const ros2_ouster::Metadata & mdata,
77+
const std::string & frame)
78+
{
79+
return new OS1::ScanProcessor(node, mdata, frame);
80+
}
81+
6882
inline std::multimap<ClientState, DataProcessorInterface *> createProcessors(
6983
const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
7084
const ros2_ouster::Metadata & mdata,
@@ -82,6 +96,9 @@ inline std::multimap<ClientState, DataProcessorInterface *> createProcessors(
8296
data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
8397
ClientState::LIDAR_DATA, createImageProcessor(
8498
node, mdata, laser_frame)));
99+
data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
100+
ClientState::LIDAR_DATA, createScanProcessor(
101+
node, mdata, laser_frame)));
85102

86103
return data_processors;
87104
}

‎ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp

+138
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
// Copyright 2020
2+
// Licensed under the Apache License, Version 2.0 (the "License");
3+
// you may not use this file except in compliance with the License.
4+
// You may obtain a copy of the License at
5+
//
6+
// http://www.apache.org/licenses/LICENSE-2.0
7+
//
8+
// Unless required by applicable law or agreed to in writing, software
9+
// distributed under the License is distributed on an "AS IS" BASIS,
10+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11+
// See the License for the specific language governing permissions and
12+
// limitations under the License.
13+
14+
#ifndef ROS2_OUSTER__OS1__PROCESSORS__SCAN_PROCESSOR_HPP_
15+
#define ROS2_OUSTER__OS1__PROCESSORS__SCAN_PROCESSOR_HPP_
16+
17+
#include <vector>
18+
#include <memory>
19+
#include <string>
20+
21+
#include "ros2_ouster/conversions.hpp"
22+
23+
#include "sensor_msgs/msg/laser_scan.hpp"
24+
25+
#include "ros2_ouster/interfaces/data_processor_interface.hpp"
26+
#include "ros2_ouster/OS1/OS1.hpp"
27+
#include "ros2_ouster/OS1/OS1_util.hpp"
28+
29+
namespace OS1
30+
{
31+
/**
32+
* @class OS1::ScanProcessor
33+
* @brief A data processor interface implementation of a processor
34+
* for creating Scans in the
35+
* driver in ROS2.
36+
*/
37+
class ScanProcessor : public ros2_ouster::DataProcessorInterface
38+
{
39+
public:
40+
typedef std::vector<scan_os::ScanOS> OSScan;
41+
typedef OSScan::iterator OSScanIt;
42+
43+
/**
44+
* @brief A constructor for OS1::ScanProcessor
45+
* @param node Node for creating interfaces
46+
* @param mdata metadata about the sensor
47+
* @param frame frame_id to use for messages
48+
*/
49+
ScanProcessor(
50+
const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
51+
const ros2_ouster::Metadata & mdata,
52+
const std::string & frame)
53+
: DataProcessorInterface(), _node(node), _frame(frame)
54+
{
55+
_mdata = mdata;
56+
_pub = _node->create_publisher<sensor_msgs::msg::LaserScan>(
57+
"scan", rclcpp::SensorDataQoS());
58+
_height = OS1::pixels_per_column;
59+
_width = OS1::n_cols_of_lidar_mode(
60+
OS1::lidar_mode_of_string(mdata.mode));
61+
_xyz_lut = OS1::make_xyz_lut(_width, _height, mdata.beam_azimuth_angles,
62+
mdata.beam_altitude_angles);
63+
_aggregated_scans.resize(_width * _height);
64+
65+
double zero_angle = 9999.0;
66+
_ring = 0;
67+
for (uint i = 0; i != _mdata.beam_altitude_angles.size(); i++) {
68+
if (fabs(_mdata.beam_altitude_angles[i]) < zero_angle) {
69+
_ring = static_cast<uint8_t>(i);
70+
zero_angle = fabs(_mdata.beam_altitude_angles[i]);
71+
}
72+
}
73+
74+
_batch_and_publish =
75+
OS1::batch_to_iter<OSScanIt>(
76+
_xyz_lut, _width, _height, {}, &scan_os::ScanOS::make,
77+
[&](uint64_t scan_ts) mutable
78+
{
79+
if (_pub->get_subscription_count() > 0 && _pub->is_activated()) {
80+
sensor_msgs::msg::LaserScan msg = ros2_ouster::toMsg(_aggregated_scans,
81+
std::chrono::nanoseconds(scan_ts), _frame, _mdata, _ring);
82+
_pub->publish(msg);
83+
}
84+
});
85+
}
86+
87+
/**
88+
* @brief A destructor clearing memory allocated
89+
*/
90+
~ScanProcessor()
91+
{
92+
_pub.reset();
93+
}
94+
95+
/**
96+
* @brief Process method to create scan
97+
* @param data the packet data
98+
*/
99+
bool process(uint8_t * data) override
100+
{
101+
OSScanIt it = _aggregated_scans.begin();
102+
_batch_and_publish(data, it);
103+
return true;
104+
}
105+
106+
/**
107+
* @brief Activating processor from lifecycle state transitions
108+
*/
109+
void onActivate() override
110+
{
111+
_pub->on_activate();
112+
}
113+
114+
/**
115+
* @brief Deactivating processor from lifecycle state transitions
116+
*/
117+
void onDeactivate() override
118+
{
119+
_pub->on_deactivate();
120+
}
121+
122+
private:
123+
rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::LaserScan>::SharedPtr _pub;
124+
std::function<void(const uint8_t *, OSScanIt)> _batch_and_publish;
125+
std::shared_ptr<pcl::PointCloud<scan_os::ScanOS>> _cloud;
126+
rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
127+
std::vector<double> _xyz_lut;
128+
ros2_ouster::Metadata _mdata;
129+
OSScan _aggregated_scans;
130+
std::string _frame;
131+
uint32_t _height;
132+
uint32_t _width;
133+
uint8_t _ring;
134+
};
135+
136+
} // namespace OS1
137+
138+
#endif // ROS2_OUSTER__OS1__PROCESSORS__SCAN_PROCESSOR_HPP_

‎ros2_ouster/include/ros2_ouster/conversions.hpp

+58
Original file line numberDiff line numberDiff line change
@@ -16,15 +16,18 @@
1616

1717
#include <string>
1818
#include <vector>
19+
#include <algorithm>
1920

2021
#include "pcl/point_types.h"
2122
#include "pcl/point_cloud.h"
2223
#include "pcl_conversions/pcl_conversions.h"
2324
#include "ros2_ouster/point_os.hpp"
2425
#include "ros2_ouster/image_os.hpp"
26+
#include "ros2_ouster/scan_os.hpp"
2527

2628
#include "geometry_msgs/msg/transform_stamped.hpp"
2729
#include "sensor_msgs/msg/imu.hpp"
30+
#include "sensor_msgs/msg/laser_scan.hpp"
2831
#include "tf2/LinearMath/Transform.h"
2932
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
3033
#include "ouster_msgs/msg/metadata.hpp"
@@ -154,6 +157,61 @@ inline sensor_msgs::msg::PointCloud2 toMsg(
154157
return msg;
155158
}
156159

160+
/**
161+
* @brief Convert Scan to message format
162+
*/
163+
inline sensor_msgs::msg::LaserScan toMsg(
164+
const std::vector<scan_os::ScanOS> & scans,
165+
std::chrono::nanoseconds timestamp,
166+
const std::string & frame,
167+
const ros2_ouster::Metadata & mdata,
168+
const uint8_t ring_to_use)
169+
{
170+
sensor_msgs::msg::LaserScan msg;
171+
rclcpp::Time t(timestamp.count());
172+
msg.header.stamp = t;
173+
msg.header.frame_id = frame;
174+
msg.angle_min = -M_PI;
175+
msg.angle_max = M_PI;
176+
msg.range_min = 0.1;
177+
msg.range_max = 120.0;
178+
179+
double resolution, rate;
180+
if (mdata.mode == "512x10") {
181+
resolution = 512.0;
182+
rate = 10.0;
183+
} else if (mdata.mode == "512x20") {
184+
resolution = 512.0;
185+
rate = 20.0;
186+
} else if (mdata.mode == "1024x10") {
187+
resolution = 1024.0;
188+
rate = 10.0;
189+
} else if (mdata.mode == "1024x20") {
190+
resolution = 1024.0;
191+
rate = 20.0;
192+
} else if (mdata.mode == "2048x10") {
193+
resolution = 2048.0;
194+
rate = 10.0;
195+
} else {
196+
std::cout << "Error: Could not determine lidar mode!" << std::endl;
197+
resolution = 512.0;
198+
rate = 10.0;
199+
}
200+
201+
msg.scan_time = 1.0 / rate;
202+
msg.time_increment = 1.0 / rate / resolution;
203+
msg.angle_increment = 2 * M_PI / resolution;
204+
205+
for (uint i = 0; i != scans.size(); i++) {
206+
if (scans[i].ring == ring_to_use) {
207+
msg.ranges.push_back(scans[i].range * 5e-3);
208+
msg.intensities.push_back(std::min(scans[i].intensity, 255.0f));
209+
}
210+
}
211+
212+
return msg;
213+
}
214+
157215
} // namespace ros2_ouster
158216

159217
#endif // ROS2_OUSTER__CONVERSIONS_HPP_

‎ros2_ouster/include/ros2_ouster/scan_os.hpp

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
// Copyright 2020
2+
// Licensed under the Apache License, Version 2.0 (the "License");
3+
// you may not use this file except in compliance with the License.
4+
// You may obtain a copy of the License at
5+
//
6+
// http://www.apache.org/licenses/LICENSE-2.0
7+
//
8+
// Unless required by applicable law or agreed to in writing, software
9+
// distributed under the License is distributed on an "AS IS" BASIS,
10+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11+
// See the License for the specific language governing permissions and
12+
// limitations under the License.
13+
14+
#ifndef ROS2_OUSTER__SCAN_OS_HPP_
15+
#define ROS2_OUSTER__SCAN_OS_HPP_
16+
17+
#define PCL_NO_PRECOMPILE
18+
#include "pcl/point_types.h"
19+
20+
namespace scan_os
21+
{
22+
23+
/**
24+
* @struct scan_os::ScanOS
25+
* @brief The Point template for PCL to hold the information for
26+
* publishing in ROS2.
27+
*/
28+
struct EIGEN_ALIGN16 ScanOS
29+
{
30+
float intensity;
31+
uint32_t range;
32+
uint8_t ring;
33+
34+
/**
35+
* @brief A factory to create the structure given a set of information
36+
* @param x position in m
37+
* @param y position in m
38+
* @param z position in m
39+
* @param intensity Intensity value from reading
40+
* @param t time in ns
41+
* @param reflectivity Reflectivity value from reading
42+
* @param ring The ring ID the measurement came from
43+
* @param column The column of the reading in the ring
44+
* @param noise Noise value from reading
45+
* @param range Range value from reading
46+
*/
47+
static inline ScanOS make(
48+
float x, float y, float z, float intensity,
49+
uint32_t t, uint16_t reflectivity, uint8_t ring, uint8_t col,
50+
uint16_t noise, uint32_t range)
51+
{
52+
return {intensity, range, ring};
53+
}
54+
};
55+
56+
} // namespace scan_os
57+
58+
#endif // ROS2_OUSTER__SCAN_OS_HPP_

0 commit comments

Comments
 (0)