Skip to content

Commit

Permalink
Create all services/publishers/subscribers on startup
Browse files Browse the repository at this point in the history
  • Loading branch information
@Samahu
Samahu committed Aug 26, 2024
1 parent 431548b commit 7de286e
Show file tree
Hide file tree
Showing 10 changed files with 150 additions and 166 deletions.
2 changes: 1 addition & 1 deletion include/ouster_ros/os_sensor_nodelet_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ class OusterSensorNodeletBase : public nodelet::Nodelet {

void create_get_metadata_service();

void create_metadata_publisher();
void create_metadata_pub();

void publish_metadata();

Expand Down
17 changes: 15 additions & 2 deletions src/lidar_packet_handler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include "lock_free_ring_buffer.h"
#include <thread>
#include <chrono>

namespace {

Expand Down Expand Up @@ -125,7 +126,13 @@ class LidarPacketHandler {

LidarPacketHandler(const LidarPacketHandler&) = delete;
LidarPacketHandler& operator=(const LidarPacketHandler&) = delete;
~LidarPacketHandler() = default;
~LidarPacketHandler() {
NODELET_DEBUG("LidarPacketHandler::~LidarPacketHandler()");
if (lidar_scans_processing_thread->joinable()) {
lidar_scans_processing_active = false;
lidar_scans_processing_thread->join();
}
}

void register_lidar_scan_handler(LidarScanProcessor handler) {
lidar_scan_handlers.push_back(handler);
Expand All @@ -151,11 +158,17 @@ class LidarPacketHandler {

void process_scans() {

using namespace std::chrono;

{
std::unique_lock<std::mutex> index_lock(ring_buffer_mutex);
ring_buffer_has_elements.wait(index_lock, [this] {
ring_buffer_has_elements.wait_for(index_lock, 1s, [this] {
return !ring_buffer.empty();
});

if (ring_buffer.empty()) {
return;
}
}

std::unique_lock<std::mutex> lock(*mutexes[ring_buffer.read_head()]);
Expand Down
90 changes: 44 additions & 46 deletions src/os_cloud_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ class OusterCloud : public nodelet::Nodelet {

private:
virtual void onInit() override {
create_imu_pub_sub();
create_point_cloud_pubs();
create_laser_scan_pubs();
create_lidar_packets_sub();
create_metadata_subscriber();
NODELET_INFO("OusterCloud: nodelet created!");
}
Expand Down Expand Up @@ -80,53 +84,63 @@ class OusterCloud : public nodelet::Nodelet {
});
}

create_publishers_subscribers(info);
create_handlers(info);
}

void create_publishers_subscribers(const sensor::sensor_info& info) {
void create_imu_pub_sub() {
imu_pub = getNodeHandle().advertise<sensor_msgs::Imu>("imu", 100);
imu_packet_sub = getNodeHandle().subscribe<PacketMsg>(
"imu_packets", 100, [this](const PacketMsg::ConstPtr msg) {
if (imu_packet_handler) {
auto imu_msg = imu_packet_handler(msg->buf.data());
if (imu_msg.header.stamp > last_msg_ts)
last_msg_ts = imu_msg.header.stamp;
imu_pub.publish(imu_msg);
}
});
}

void create_point_cloud_pubs() {
// NOTE: always create the 2nd topic
lidar_pubs.resize(2);
for (int i = 0; i < 2; ++i) {
lidar_pubs[i] = getNodeHandle().advertise<sensor_msgs::PointCloud2>(
topic_for_return("points", i), 10);
}
}

void create_laser_scan_pubs() {
// NOTE: always create the 2nd topic
scan_pubs.resize(2);
for (int i = 0; i < 2; ++i) {
scan_pubs[i] = getNodeHandle().advertise<sensor_msgs::LaserScan>(
topic_for_return("scan", i), 10);
}
}

void create_lidar_packets_sub() {
lidar_packet_sub = getNodeHandle().subscribe<PacketMsg>(
"lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
if (lidar_packet_handler) lidar_packet_handler(msg->buf.data());
});
}

void create_handlers(const sensor::sensor_info& info) {
auto& pnh = getPrivateNodeHandle();
auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
auto tokens = impl::parse_tokens(proc_mask, '|');

auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);

auto& nh = getNodeHandle();

if (impl::check_token(tokens, "IMU")) {
if (!services_publishers_created) {
imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 100);
}
imu_packet_handler = ImuPacketHandler::create_handler(
info, tf_bcast.imu_frame_id(), timestamp_mode,
static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));

if (!services_publishers_created) {
imu_packet_sub = nh.subscribe<PacketMsg>(
"imu_packets", 100, [this](const PacketMsg::ConstPtr msg) {
auto imu_msg = imu_packet_handler(msg->buf.data());
if (imu_msg.header.stamp > last_msg_ts)
last_msg_ts = imu_msg.header.stamp;
imu_pub.publish(imu_msg);
});
}
}

int num_returns = get_n_returns(info);

std::vector<LidarScanProcessor> processors;
if (impl::check_token(tokens, "PCL")) {

if (!services_publishers_created) {
// TODO: do we need to resize the number of publishers if the
// number of returns changed per profile? (revist)
lidar_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
lidar_pubs[i] = nh.advertise<sensor_msgs::PointCloud2>(
topic_for_return("points", i), 10);
}
}

auto point_type = pnh.param("point_type", std::string{"original"});
processors.push_back(
PointCloudProcessorFactory::create_point_cloud_processor(point_type,
Expand All @@ -153,15 +167,6 @@ class OusterCloud : public nodelet::Nodelet {
}

if (impl::check_token(tokens, "SCAN")) {

if (!services_publishers_created) {
scan_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
scan_pubs[i] = nh.advertise<sensor_msgs::LaserScan>(
topic_for_return("scan", i), 10);
}
}

// TODO: avoid this duplication in os_cloud_node
int beams_count = static_cast<int>(get_beams_count(info));
int scan_ring = pnh.param("scan_ring", 0);
Expand All @@ -188,13 +193,8 @@ class OusterCloud : public nodelet::Nodelet {
lidar_packet_handler = LidarPacketHandler::create_handler(
info, processors, timestamp_mode,
static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
lidar_packet_sub = nh.subscribe<PacketMsg>(
"lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
lidar_packet_handler(msg->buf.data());
});
}

services_publishers_created = true;
}

private:
Expand All @@ -212,8 +212,6 @@ class OusterCloud : public nodelet::Nodelet {

ros::Timer timer_;
ros::Time last_msg_ts;

bool services_publishers_created = false;
};

} // namespace ouster_ros
Expand Down
107 changes: 54 additions & 53 deletions src/os_driver_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,18 +38,66 @@ class OusterDriver : public OusterSensor {
halt();
}

protected:
virtual void onInit() override {
create_imu_pub();
create_point_cloud_pubs();
create_laser_scan_pubs();
create_image_pubs();
OusterSensor::onInit();
}

private:

virtual void on_metadata_updated(const sensor::sensor_info& info) override {
OusterSensor::on_metadata_updated(info);

// for OusterDriver we are going to always assume static broadcast
// at least for now
tf_bcast.parse_parameters(getPrivateNodeHandle());
tf_bcast.broadcast_transforms(info);
create_handlers();
}

void create_imu_pub() {
imu_pub = getNodeHandle().advertise<sensor_msgs::Imu>("imu", 100);
}

void create_point_cloud_pubs() {
// NOTE: always create the 2nd topic
lidar_pubs.resize(2);
for (int i = 0; i < 2; ++i) {
lidar_pubs[i] = getNodeHandle().advertise<sensor_msgs::PointCloud2>(
topic_for_return("points", i), 10);
}
}

virtual void create_publishers() override {
void create_laser_scan_pubs() {
// NOTE: always create the 2nd topic
scan_pubs.resize(2);
for (int i = 0; i < 2; ++i) {
scan_pubs[i] = getNodeHandle().advertise<sensor_msgs::LaserScan>(
topic_for_return("scan", i), 10);
}
}

void create_image_pubs() {
// NOTE: always create the 2nd topics
const std::map<sensor::ChanField, std::string>
channel_field_topic_map {
{sensor::ChanField::RANGE, "range_image"},
{sensor::ChanField::SIGNAL, "signal_image"},
{sensor::ChanField::REFLECTIVITY, "reflec_image"},
{sensor::ChanField::NEAR_IR, "nearir_image"},
{sensor::ChanField::RANGE2, "range_image2"},
{sensor::ChanField::SIGNAL2, "signal_image2"},
{sensor::ChanField::REFLECTIVITY2, "reflec_image2"}};

for (auto it : channel_field_topic_map) {
image_pubs[it.first] = getNodeHandle().advertise<sensor_msgs::Image>(it.second, 100);
}
}

virtual void create_handlers() {
auto& pnh = getPrivateNodeHandle();
auto proc_mask =
pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"});
Expand All @@ -58,29 +106,14 @@ class OusterDriver : public OusterSensor {
auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);

auto& nh = getNodeHandle();

if (impl::check_token(tokens, "IMU")) {
if (!services_publishers_created)
imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 100);
imu_packet_handler = ImuPacketHandler::create_handler(
info, tf_bcast.imu_frame_id(), timestamp_mode,
static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
}

int num_returns = get_n_returns(info);

std::vector<LidarScanProcessor> processors;
if (impl::check_token(tokens, "PCL")) {

if (!services_publishers_created) {
lidar_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
lidar_pubs[i] = nh.advertise<sensor_msgs::PointCloud2>(
topic_for_return("points", i), 10);
}
}

auto point_type = pnh.param("point_type", std::string{"original"});
processors.push_back(
PointCloudProcessorFactory::create_point_cloud_processor(point_type, info,
Expand All @@ -102,14 +135,6 @@ class OusterDriver : public OusterSensor {
}

if (impl::check_token(tokens, "SCAN")) {
if (!services_publishers_created) {
scan_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
scan_pubs[i] = nh.advertise<sensor_msgs::LaserScan>(
topic_for_return("scan", i), 10);
}
}

// TODO: avoid duplication in os_cloud_node
int beams_count = static_cast<int>(get_beams_count(info));
int scan_ring = pnh.param("scan_ring", 0);
Expand All @@ -131,33 +156,6 @@ class OusterDriver : public OusterSensor {
}

if (impl::check_token(tokens, "IMG")) {
const std::map<sensor::ChanField, std::string>
channel_field_topic_map_1{
{sensor::ChanField::RANGE, "range_image"},
{sensor::ChanField::SIGNAL, "signal_image"},
{sensor::ChanField::REFLECTIVITY, "reflec_image"},
{sensor::ChanField::NEAR_IR, "nearir_image"}};

const std::map<sensor::ChanField, std::string>
channel_field_topic_map_2{
{sensor::ChanField::RANGE, "range_image"},
{sensor::ChanField::SIGNAL, "signal_image"},
{sensor::ChanField::REFLECTIVITY, "reflec_image"},
{sensor::ChanField::NEAR_IR, "nearir_image"},
{sensor::ChanField::RANGE2, "range_image2"},
{sensor::ChanField::SIGNAL2, "signal_image2"},
{sensor::ChanField::REFLECTIVITY2, "reflec_image2"}};

auto which_map = num_returns == 1 ? &channel_field_topic_map_1
: &channel_field_topic_map_2;
for (auto it = which_map->begin(); it != which_map->end(); ++it) {
if (!services_publishers_created ||
image_pubs.find(it->first) == image_pubs.end()) {
image_pubs[it->first] =
nh.advertise<sensor_msgs::Image>(it->second, 100);
}
}

processors.push_back(ImageProcessor::create(
info, tf_bcast.point_cloud_frame_id(),
[this](ImageProcessor::OutputType msgs) {
Expand All @@ -180,7 +178,10 @@ class OusterDriver : public OusterSensor {

virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override {
if (imu_packet_handler)
imu_pub.publish(imu_packet_handler(raw_imu_packet));
if (imu_packet_handler) {
auto imu_msg = imu_packet_handler(raw_imu_packet);
imu_pub.publish(imu_msg);
}
}

private:
Expand Down
Loading

0 comments on commit 7de286e

Please sign in to comment.