diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 8f10b501..22662a94 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -19,9 +19,11 @@ Changelog
explicitly by turning on the ``BUILD_PCAP`` cmake option and having ``libpcap-dev`` installed.
* [BREAKING] Added new launch files args ``azimuth_window_start`` and ``azimuth_window_end`` to
allow users to set LIDAR FOV on startup. The new options will reset the current azimuth window
- to the default
+ to the default [0, 360] azimuth if not configured.
* Added a new launch ``persist_config`` option to request the sensor persist the current config
* Added a new ``loop`` option to the ``replay.launch`` file.
+* Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg
+ documentation and the associated params to enable.
ouster_ros v0.10.0
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24d25bac..994b2bc3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -75,13 +75,20 @@ include_directories(${_ouster_ros_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
# use only MPL-licensed parts of eigen
add_definitions(-DEIGEN_MPL2_ONLY)
+set(NODELET_SRC
+ src/os_sensor_nodelet_base.cpp
+ src/os_sensor_nodelet.cpp
+ src/os_replay_nodelet.cpp
+ src/os_cloud_nodelet.cpp
+ src/os_image_nodelet.cpp
+ src/os_driver_nodelet.cpp
+)
+
set(OUSTER_TARGET_LINKS ouster_client)
+
if (BUILD_PCAP)
list(APPEND OUSTER_TARGET_LINKS ouster_pcap)
- # Include os_pcap_nodelet.cpp only if BUILD_PCAP is defined
- set(PCAP_NODELET_SRC src/os_pcap_nodelet.cpp)
-else()
- set(PCAP_NODELET_SRC "")
+ list(APPEND NODELET_SRC src/os_pcap_nodelet.cpp)
endif()
add_library(ouster_ros src/os_ros.cpp)
@@ -91,14 +98,7 @@ target_link_libraries(ouster_ros
add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp)
# ==== Executables ====
-add_library(${PROJECT_NAME}_nodelets
- src/os_sensor_nodelet_base.cpp
- src/os_sensor_nodelet.cpp
- src/os_replay_nodelet.cpp
- ${PCAP_NODELET_SRC} # Include os_pcap_nodelet.cpp only if BUILD_PCAP is defined
- src/os_cloud_nodelet.cpp
- src/os_image_nodelet.cpp
- src/os_driver_nodelet.cpp)
+add_library(${PROJECT_NAME}_nodelets ${NODELET_SRC})
target_link_libraries(${PROJECT_NAME}_nodelets ouster_ros ${catkin_LIBRARIES})
add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp)
diff --git a/include/ouster_ros/os_sensor_nodelet_base.h b/include/ouster_ros/os_sensor_nodelet_base.h
index 44925f10..a6e7cb06 100644
--- a/include/ouster_ros/os_sensor_nodelet_base.h
+++ b/include/ouster_ros/os_sensor_nodelet_base.h
@@ -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();
diff --git a/launch/common.launch b/launch/common.launch
index ef3725e2..1ed5c7d7 100644
--- a/launch/common.launch
+++ b/launch/common.launch
@@ -67,6 +67,7 @@
+
@@ -77,12 +78,4 @@
launch-prefix="bash -c 'sleep 5; $0 $@' "
args="-d $(arg rviz_config)"/>
-
-
-
diff --git a/launch/driver.launch b/launch/driver.launch
index 23506b9e..820d7563 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -73,6 +73,15 @@
+
+
+
+
@@ -103,6 +112,11 @@
+
+
+
diff --git a/launch/record.launch b/launch/record.launch
index ad49ed2f..edd74a2d 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -76,6 +76,15 @@
+
+
+
+
@@ -97,6 +106,11 @@
+
+
+
diff --git a/launch/sensor.launch b/launch/sensor.launch
index 46839d4e..46ffeb1e 100644
--- a/launch/sensor.launch
+++ b/launch/sensor.launch
@@ -81,6 +81,15 @@
+
+
+
+
@@ -101,6 +110,11 @@
+
+
+
diff --git a/package.xml b/package.xml
index f91d6bc5..306e1d34 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
ouster_ros
- 0.12.5
+ 0.12.6
Ouster ROS driver
ouster developers
BSD
diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h
index 207c41c3..ed858978 100644
--- a/src/lidar_packet_handler.h
+++ b/src/lidar_packet_handler.h
@@ -18,6 +18,7 @@
#include "lock_free_ring_buffer.h"
#include
+#include
namespace {
@@ -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);
@@ -151,11 +158,17 @@ class LidarPacketHandler {
void process_scans() {
+ using namespace std::chrono;
+
{
std::unique_lock 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 lock(*mutexes[ring_buffer.read_head()]);
diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp
index f808ce59..92c7e02f 100644
--- a/src/os_cloud_nodelet.cpp
+++ b/src/os_cloud_nodelet.cpp
@@ -39,6 +39,18 @@ class OusterCloud : public nodelet::Nodelet {
private:
virtual void onInit() override {
+ auto& pnh = getPrivateNodeHandle();
+ auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
+ auto tokens = impl::parse_tokens(proc_mask, '|');
+ if (impl::check_token(tokens, "IMU"))
+ create_imu_pub_sub();
+ if (impl::check_token(tokens, "PCL"))
+ create_point_cloud_pubs();
+ if (impl::check_token(tokens, "SCAN"))
+ create_laser_scan_pubs();
+ if (impl::check_token(tokens, "PCL") ||
+ impl::check_token(tokens, "SCAN"))
+ create_lidar_packets_sub();
create_metadata_subscriber();
NODELET_INFO("OusterCloud: nodelet created!");
}
@@ -49,15 +61,12 @@ class OusterCloud : public nodelet::Nodelet {
}
void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) {
- // TODO: handle sensor reconfigurtion
NODELET_INFO("OusterCloud: retrieved new sensor metadata!");
-
auto info = sensor::parse_metadata(metadata_msg->data);
- tf_bcast.parse_parameters(getPrivateNodeHandle());
-
- auto dynamic_transforms =
- getPrivateNodeHandle().param("dynamic_transforms_broadcast", false);
+ auto pnh = getPrivateNodeHandle();
+ tf_bcast.parse_parameters(pnh);
+ auto dynamic_transforms = pnh.param("dynamic_transforms_broadcast", false);
auto dynamic_transforms_rate = getPrivateNodeHandle().param(
"dynamic_transforms_broadcast_rate", 1.0);
if (dynamic_transforms && dynamic_transforms_rate < 1.0) {
@@ -72,9 +81,10 @@ class OusterCloud : public nodelet::Nodelet {
tf_bcast.broadcast_transforms(info);
} else {
NODELET_INFO_STREAM(
- "OusterCloud: dynamic transforms broadcast enabled wit "
+ "OusterCloud: dynamic transforms broadcast enabled with "
"broadcast rate of: "
<< dynamic_transforms_rate << " Hz");
+ timer_.stop();
timer_ = getNodeHandle().createTimer(
ros::Duration(1.0 / dynamic_transforms_rate),
[this, info](const ros::TimerEvent&) {
@@ -82,10 +92,48 @@ class OusterCloud : public nodelet::Nodelet {
});
}
- create_publishers_subscribers(info);
+ create_handlers(info);
+ }
+
+ void create_imu_pub_sub() {
+ imu_pub = getNodeHandle().advertise("imu", 100);
+ imu_packet_sub = getNodeHandle().subscribe(
+ "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(
+ 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(
+ topic_for_return("scan", i), 10);
+ }
+ }
+
+ void create_lidar_packets_sub() {
+ lidar_packet_sub = getNodeHandle().subscribe(
+ "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
+ if (lidar_packet_handler) lidar_packet_handler(msg->buf.data());
+ });
}
- void create_publishers_subscribers(const sensor::sensor_info& info) {
+ 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, '|');
@@ -93,32 +141,14 @@ class OusterCloud : public nodelet::Nodelet {
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")) {
- imu_pub = nh.advertise("imu", 100);
imu_packet_handler = ImuPacketHandler::create_handler(
info, tf_bcast.imu_frame_id(), timestamp_mode,
static_cast(ptp_utc_tai_offset * 1e+9));
- imu_packet_sub = nh.subscribe(
- "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 processors;
if (impl::check_token(tokens, "PCL")) {
- lidar_pubs.resize(num_returns);
- for (int i = 0; i < num_returns; ++i) {
- lidar_pubs[i] = nh.advertise(
- 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,
@@ -145,12 +175,6 @@ class OusterCloud : public nodelet::Nodelet {
}
if (impl::check_token(tokens, "SCAN")) {
- scan_pubs.resize(num_returns);
- for (int i = 0; i < num_returns; ++i) {
- scan_pubs[i] = nh.advertise(
- topic_for_return("scan", i), 10);
- }
-
// TODO: avoid this duplication in os_cloud_node
int beams_count = static_cast(get_beams_count(info));
int scan_ring = pnh.param("scan_ring", 0);
@@ -177,11 +201,8 @@ class OusterCloud : public nodelet::Nodelet {
lidar_packet_handler = LidarPacketHandler::create_handler(
info, processors, timestamp_mode,
static_cast(ptp_utc_tai_offset * 1e+9));
- lidar_packet_sub = nh.subscribe(
- "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
- lidar_packet_handler(msg->buf.data());
- });
}
+
}
private:
diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp
index 8f71217b..3eea44a1 100644
--- a/src/os_driver_nodelet.cpp
+++ b/src/os_driver_nodelet.cpp
@@ -38,18 +38,72 @@ class OusterDriver : public OusterSensor {
halt();
}
- private:
+ protected:
+ virtual void onInit() override {
+ auto& pnh = getPrivateNodeHandle();
+ auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
+ auto tokens = impl::parse_tokens(proc_mask, '|');
+ if (impl::check_token(tokens, "IMU"))
+ create_imu_pub();
+ if (impl::check_token(tokens, "PCL"))
+ create_point_cloud_pubs();
+ if (impl::check_token(tokens, "SCAN"))
+ create_laser_scan_pubs();
+ if (impl::check_token(tokens, "IMG"))
+ 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();
}
- virtual void create_publishers() override {
+ void create_imu_pub() {
+ imu_pub = getNodeHandle().advertise("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(
+ 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(
+ topic_for_return("scan", i), 10);
+ }
+ }
+
+ void create_image_pubs() {
+ // NOTE: always create the 2nd topics
+ const std::map
+ 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(it.second, 100);
+ }
+ }
+
+ virtual void create_handlers() {
auto& pnh = getPrivateNodeHandle();
auto proc_mask =
pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"});
@@ -58,25 +112,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")) {
- imu_pub = nh.advertise("imu", 100);
imu_packet_handler = ImuPacketHandler::create_handler(
info, tf_bcast.imu_frame_id(), timestamp_mode,
static_cast(ptp_utc_tai_offset * 1e+9));
}
- int num_returns = get_n_returns(info);
-
std::vector processors;
if (impl::check_token(tokens, "PCL")) {
- lidar_pubs.resize(num_returns);
- for (int i = 0; i < num_returns; ++i) {
- lidar_pubs[i] = nh.advertise(
- 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,
@@ -98,12 +141,6 @@ class OusterDriver : public OusterSensor {
}
if (impl::check_token(tokens, "SCAN")) {
- scan_pubs.resize(num_returns);
- for (int i = 0; i < num_returns; ++i) {
- scan_pubs[i] = nh.advertise(
- topic_for_return("scan", i), 10);
- }
-
// TODO: avoid duplication in os_cloud_node
int beams_count = static_cast(get_beams_count(info));
int scan_ring = pnh.param("scan_ring", 0);
@@ -125,30 +162,6 @@ class OusterDriver : public OusterSensor {
}
if (impl::check_token(tokens, "IMG")) {
- const std::map
- 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
- 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) {
- image_pubs[it->first] =
- nh.advertise(it->second, 10);
- }
-
processors.push_back(ImageProcessor::create(
info, tf_bcast.point_cloud_frame_id(),
[this](ImageProcessor::OutputType msgs) {
@@ -171,7 +184,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:
diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp
index 736fae69..ff8e8685 100644
--- a/src/os_image_nodelet.cpp
+++ b/src/os_image_nodelet.cpp
@@ -33,39 +33,33 @@ using ouster_ros::PacketMsg;
class OusterImage : public nodelet::Nodelet {
private:
virtual void onInit() override {
- create_metadata_subscriber(getNodeHandle());
+ auto& pnh = getPrivateNodeHandle();
+ auto proc_mask = pnh.param("proc_mask", std::string{"IMG"});
+ auto tokens = impl::parse_tokens(proc_mask, '|');
+ if (impl::check_token(tokens, "IMG")) {
+ create_lidar_packets_subscriber();
+ create_image_publishers();
+ }
+ create_metadata_subscriber();
NODELET_INFO("OusterImage: node initialized!");
}
- void create_metadata_subscriber(ros::NodeHandle& nh) {
- metadata_sub = nh.subscribe(
+ void create_metadata_subscriber() {
+ metadata_sub = getNodeHandle().subscribe(
"metadata", 1, &OusterImage::metadata_handler, this);
}
- private:
- void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) {
- NODELET_INFO("OusterImage: retrieved new sensor metadata!");
- info = sensor::parse_metadata(metadata_msg->data);
- create_publishers_subscribers(get_n_returns(info));
+ void create_lidar_packets_subscriber() {
+ lidar_packet_sub = getNodeHandle().subscribe(
+ "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
+ if (lidar_packet_handler) lidar_packet_handler(msg->buf.data());
+ });
}
- void create_publishers_subscribers(int n_returns) {
- // TODO: avoid having to replicate the parameters:
- // timestamp_mode, ptp_utc_tai_offset, use_system_default_qos in yet
- // another node.
- auto& pnh = getPrivateNodeHandle();
- auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
- double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
-
- const std::map
- 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"}};
-
+ void create_image_publishers() {
+ // NOTE: always create the 2nd topics
const std::map
- channel_field_topic_map_2 {
+ channel_field_topic_map {
{sensor::ChanField::RANGE, "range_image"},
{sensor::ChanField::SIGNAL, "signal_image"},
{sensor::ChanField::REFLECTIVITY, "reflec_image"},
@@ -74,15 +68,29 @@ class OusterImage : public nodelet::Nodelet {
{sensor::ChanField::SIGNAL2, "signal_image2"},
{sensor::ChanField::REFLECTIVITY2, "reflec_image2"}};
- auto which_map = n_returns == 1 ? &channel_field_topic_map_1
- : &channel_field_topic_map_2;
+ for (auto it : channel_field_topic_map) {
+ image_pubs[it.first] = getNodeHandle().advertise(it.second, 100);
+ }
+ }
+
+ void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) {
+ NODELET_INFO("OusterImage: retrieved new sensor metadata!");
+ auto info = sensor::parse_metadata(metadata_msg->data);
- auto& nh = getNodeHandle();
+ auto& pnh = getPrivateNodeHandle();
+ auto proc_mask = pnh.param("proc_mask", std::string{"IMG"});
+ auto tokens = impl::parse_tokens(proc_mask, '|');
+ if (impl::check_token(tokens, "IMG"))
+ create_handlers(info);
+ }
- for (auto it = which_map->begin(); it != which_map->end(); ++it) {
- image_pubs[it->first] =
- nh.advertise(it->second, 100);
- }
+ void create_handlers(const sensor::sensor_info& info) {
+ // TODO: avoid having to replicate the parameters:
+ // timestamp_mode, ptp_utc_tai_offset, use_system_default_qos in yet
+ // another node.
+ auto& pnh = getPrivateNodeHandle();
+ auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
+ double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
std::vector processors {
ImageProcessor::create(
@@ -97,22 +105,16 @@ class OusterImage : public nodelet::Nodelet {
lidar_packet_handler = LidarPacketHandler::create_handler(
info, processors, timestamp_mode,
static_cast(ptp_utc_tai_offset * 1e+9));
- lidar_packet_sub = nh.subscribe(
- "lidar_packets", 100,
- [this](const PacketMsg::ConstPtr msg) {
- lidar_packet_handler(msg->buf.data());
- });
}
private:
ros::Subscriber metadata_sub;
- sensor::sensor_info info;
-
ros::Subscriber lidar_packet_sub;
std::map image_pubs;
LidarPacketHandler::HandlerType lidar_packet_handler;
};
+
} // namespace ouster_ros
PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterImage, nodelet::Nodelet)
diff --git a/src/os_pcap_nodelet.cpp b/src/os_pcap_nodelet.cpp
index da5c5855..a5f38077 100644
--- a/src/os_pcap_nodelet.cpp
+++ b/src/os_pcap_nodelet.cpp
@@ -36,7 +36,7 @@ class OusterPcap : public OusterSensorNodeletBase {
virtual void onInit() override {
auto meta_file = get_meta_file();
auto pcap_file = get_pcap_file();
- create_metadata_publisher();
+ create_metadata_pub();
load_metadata_from_file(meta_file);
allocate_buffers();
create_publishers();
diff --git a/src/os_replay_nodelet.cpp b/src/os_replay_nodelet.cpp
index 3cb2e9a3..d913039f 100644
--- a/src/os_replay_nodelet.cpp
+++ b/src/os_replay_nodelet.cpp
@@ -19,7 +19,7 @@ class OusterReplay : public OusterSensorNodeletBase {
private:
virtual void onInit() override {
auto meta_file = get_meta_file();
- create_metadata_publisher();
+ create_metadata_pub();
load_metadata_from_file(meta_file);
publish_metadata();
create_get_metadata_service();
diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp
index c193cda8..8e618ad4 100644
--- a/src/os_sensor_nodelet.cpp
+++ b/src/os_sensor_nodelet.cpp
@@ -27,6 +27,7 @@ using namespace std::chrono_literals;
using namespace std::string_literals;
using std::to_string;
+
namespace ouster_ros {
OusterSensor::~OusterSensor() {
@@ -39,26 +40,92 @@ void OusterSensor::halt() {
stop_sensor_connection_thread();
}
-void OusterSensor::onInit() {
+bool OusterSensor::start() {
sensor_hostname = get_sensor_hostname();
- sensor::sensor_config config = parse_config_from_ros_parameters();
- configure_sensor(sensor_hostname, config);
+
+ sensor::sensor_config config;
+ if (staged_config) {
+ if (!configure_sensor(sensor_hostname, staged_config.value()))
+ return false;
+ config = staged_config.value();
+ staged_config.reset();
+ }
+ else {
+ if (!get_active_config_no_throw(sensor_hostname, config))
+ return false;
+
+ // Unfortunately it seems we need to invoke this to force the auto
+ // TODO[UN]: find a shortcut
+ // Only reset udp_dest if auto_udp was allowed on startup
+ if (auto_udp_allowed) config.udp_dest.reset();
+ if (!configure_sensor(sensor_hostname, config))
+ return false;
+
+ NODELET_INFO("Retrived sensor active config");
+ }
+
+ reset_last_init_id = true;
sensor_client = create_sensor_client(sensor_hostname, config);
if (!sensor_client) {
- auto error_msg = "Failed to initialize client";
- NODELET_ERROR_STREAM(error_msg);
- throw std::runtime_error(error_msg);
+ NODELET_ERROR_STREAM("Failed to initialize client");
+ return false;
}
- create_metadata_publisher();
- update_config_and_metadata(*sensor_client);
- create_services();
-
- // activate
- create_publishers();
+ update_metadata(*sensor_client);
allocate_buffers();
start_packet_processing_threads();
start_sensor_connection_thread();
+ return true;
+}
+
+void OusterSensor::stop() {
+ // deactivate
+ halt();
+ sensor_client.reset();
+}
+
+void OusterSensor::attempt_start() {
+ if (!start()) {
+ if (attempt_reconnect && reconnect_attempts_available-- > 0) {
+ reconnect_timer = getNodeHandle().createTimer(
+ ros::Duration(dormant_period_between_reconnects),
+ [this](const ros::TimerEvent&) {
+ NODELET_INFO_STREAM("Attempting to communicate with the sensor, "
+ "remaining attempts: " << reconnect_attempts_available);
+ attempt_start();
+ }, true);
+ }
+ } else {
+ // reset counter
+ reconnect_attempts_available =
+ getPrivateNodeHandle().param("max_failed_reconnect_attempts", INT_MAX);
+ }
+}
+
+void OusterSensor::schedule_stop() {
+ sensor_connection_active = false;
+ reconnect_timer = getNodeHandle().createTimer(
+ ros::Duration(0.0), [this](const ros::TimerEvent&) {
+ stop();
+ if (attempt_reconnect && reconnect_attempts_available-- > 0) {
+ NODELET_INFO_STREAM("Attempting to communicate with the sensor, "
+ "remaining attempts: " << reconnect_attempts_available);
+ attempt_start();
+ }
+ }, true);
+}
+
+void OusterSensor::onInit() {
+ staged_config = parse_config_from_ros_parameters();
+ create_metadata_pub();
+ create_services();
+ create_publishers();
+ attempt_reconnect = getPrivateNodeHandle().param("attempt_reconnect", false);
+ dormant_period_between_reconnects =
+ getPrivateNodeHandle().param("dormant_period_between_reconnects", 1.0);
+ reconnect_attempts_available =
+ getPrivateNodeHandle().param("max_failed_reconnect_attempts", INT_MAX);
+ attempt_start();
}
std::string OusterSensor::get_sensor_hostname() {
@@ -73,19 +140,7 @@ std::string OusterSensor::get_sensor_hostname() {
return hostname;
}
-void OusterSensor::update_config_and_metadata(sensor::client& cli) {
- sensor::sensor_config config;
- auto success = get_config(sensor_hostname, config);
- if (!success) {
- active_config.clear();
- cached_metadata.clear();
- auto error_msg = std::string{"Failed to collect sensor config"};
- NODELET_ERROR_STREAM(error_msg);
- throw std::runtime_error(error_msg);
- }
-
- active_config = sensor::to_string(config);
-
+void OusterSensor::update_metadata(sensor::client& cli) {
try {
cached_metadata = sensor::get_metadata(cli, 60, false);
} catch (const std::exception& e) {
@@ -132,25 +187,47 @@ void OusterSensor::save_metadata() {
}
void OusterSensor::create_reset_service() {
- reset_srv = getNodeHandle()
- .advertiseService(
- "reset", [this](std_srvs::Empty::Request&,
- std_srvs::Empty::Response&) {
- NODELET_INFO("reset service invoked");
- // reset_sensor(true);
- return true;
- });
+ reset_srv =
+ getNodeHandle()
+ .advertiseService(
+ "reset", [this](std_srvs::Empty::Request&,
+ std_srvs::Empty::Response&) {
+ NODELET_INFO("reset service invoked");
+ reset_sensor(true);
+ return true;
+ });
NODELET_INFO("reset service created");
}
+bool OusterSensor::get_active_config_no_throw(
+ const std::string& sensor_hostname, sensor::sensor_config& config) {
+ try {
+ if (get_config(sensor_hostname, config, true))
+ return true;
+ } catch(const std::exception&) {
+ NODELET_ERROR_STREAM(
+ "Couldn't get active config for: " << sensor_hostname);
+ return false;
+ }
+
+ NODELET_ERROR_STREAM(
+ "Couldn't get active config for: " << sensor_hostname);
+ return false;
+}
+
+
void OusterSensor::create_get_config_service() {
get_config_srv =
getNodeHandle()
.advertiseService(
"get_config",
[this](GetConfig::Request&, GetConfig::Response& response) {
+ std::string active_config;
+ sensor::sensor_config config;
+ if (get_active_config_no_throw(sensor_hostname, config))
+ active_config = to_string(config);
response.config = active_config;
return active_config.size() > 0;
});
@@ -166,9 +243,10 @@ void OusterSensor::create_set_config_service() {
SetConfig::Response& response) {
response.config = "";
+ std::string config_str;
try {
- staged_config = read_text_file(request.config_file);
- if (staged_config.empty()) {
+ config_str = read_text_file(request.config_file);
+ if (config_str.empty()) {
NODELET_ERROR_STREAM(
"provided config file: "
<< request.config_file
@@ -183,12 +261,13 @@ void OusterSensor::create_set_config_service() {
return false;
}
- response.config = staged_config;
+ staged_config = sensor::parse_config(config_str);
// TODO: this is currently set to force_reinit but it
// doesn't need to be the case if it was possible to know
// that the new config would result in a reinit when a
// reinit is not forced
reset_sensor(true);
+ response.config = config_str;
return true;
});
@@ -197,12 +276,14 @@ void OusterSensor::create_set_config_service() {
std::shared_ptr OusterSensor::create_sensor_client(
const std::string& hostname, const sensor::sensor_config& config) {
- NODELET_INFO_STREAM("Starting sensor " << hostname << " initialization...");
int lidar_port = config.udp_port_lidar ? config.udp_port_lidar.value() : 0;
int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0;
auto udp_dest = config.udp_dest ? config.udp_dest.value() : "";
+ NODELET_INFO_STREAM("Starting sensor " << hostname << " initialization..."
+ " Using ports: " << lidar_port << "/" << imu_port);
+
std::shared_ptr cli;
if (sensor::in_multicast(udp_dest)) {
// use the mtp_init_client to recieve data via multicast
@@ -215,9 +296,8 @@ std::shared_ptr OusterSensor::create_sensor_client(
} else {
// use the full init_client to generate and assign random ports to
// sensor
- cli =
- sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC,
- sensor::TIME_FROM_UNSPEC, lidar_port, imu_port);
+ cli = sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC,
+ sensor::TIME_FROM_UNSPEC, lidar_port, imu_port);
}
return cli;
@@ -324,7 +404,6 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
"event each time");
}
-
config.udp_profile_lidar = udp_profile_lidar;
config.operating_mode = sensor::OPERATING_NORMAL;
if (lidar_mode) config.ld_mode = lidar_mode;
@@ -335,6 +414,8 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
mtp_dest = is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{};
mtp_main = mtp_main_arg;
}
+ } else {
+ auto_udp_allowed = true;
}
if (azimuth_window_start < MIN_AZW || azimuth_window_start > MAX_AZW ||
@@ -350,12 +431,6 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
return config;
}
-sensor::sensor_config OusterSensor::parse_config_from_staged_config_string() {
- sensor::sensor_config config = sensor::parse_config(staged_config);
- staged_config.clear();
- return config;
-}
-
uint8_t OusterSensor::compose_config_flags(
const sensor::sensor_config& config) {
uint8_t config_flags = 0;
@@ -385,6 +460,7 @@ uint8_t OusterSensor::compose_config_flags(
}
if (persist_config) {
+ persist_config = false; // avoid persisting configs implicitly on restarts
NODELET_INFO("Configuration will be persisted");
config_flags |= ouster::sensor::CONFIG_PERSIST;
}
@@ -392,27 +468,33 @@ uint8_t OusterSensor::compose_config_flags(
return config_flags;
}
-void OusterSensor::configure_sensor(const std::string& hostname,
+bool OusterSensor::configure_sensor(const std::string& hostname,
sensor::sensor_config& config) {
+ // TODO[UN]: in future always get_config
if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) &&
!mtp_main) {
if (!get_config(hostname, config, true)) {
NODELET_ERROR("Error getting active config");
+ return false;
} else {
NODELET_INFO("Retrived active config of sensor");
+ return true;
}
- return;
}
uint8_t config_flags = compose_config_flags(config);
- if (!set_config(hostname, config, config_flags)) {
- auto error_msg = "Error connecting to sensor " + hostname;
- NODELET_ERROR_STREAM(error_msg);
- throw std::runtime_error(error_msg);
+ NODELET_INFO_STREAM("Contacting sensor " << hostname << " ...");
+ try {
+ set_config(hostname, config, config_flags);
+ } catch (const std::exception& ex) {
+ NODELET_ERROR_STREAM("Error connecting to sensor " << hostname <<
+ ", details: " << ex.what());
+ return false;
}
NODELET_INFO_STREAM("Sensor " << hostname
<< " was configured successfully");
+ return true;
}
void OusterSensor::populate_metadata_defaults(
@@ -491,7 +573,7 @@ bool OusterSensor::init_id_changed(const sensor::packet_format& pf,
}
void OusterSensor::handle_poll_client_error() {
- NODELET_WARN_THROTTLE(1, "sensor::poll_client()) returned error");
+ NODELET_WARN_THROTTLE(1, "sensor::poll_client()) returned an error or timed out");
// in case error continues for a while attempt to recover by
// performing sensor reset
if (++poll_client_error_count > max_poll_client_error_count) {
@@ -512,7 +594,7 @@ void OusterSensor::handle_lidar_packet(sensor::client& cli,
if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) {
// TODO: short circut reset if no breaking changes occured?
NODELET_WARN("sensor init_id has changed! reactivating..");
- reactivate_sensor();
+ reset_sensor(false);
}
} else {
if (++read_lidar_packet_errors > max_read_lidar_packet_errors) {
@@ -549,7 +631,7 @@ void OusterSensor::connection_loop(sensor::client& cli,
NODELET_INFO("poll_client: caught signal, exiting!");
return;
}
- if (state & sensor::CLIENT_ERROR) {
+ if (state & sensor::CLIENT_ERROR || state == sensor::TIMEOUT) {
handle_poll_client_error();
return;
}
@@ -565,6 +647,7 @@ void OusterSensor::connection_loop(sensor::client& cli,
void OusterSensor::start_sensor_connection_thread() {
sensor_connection_active = true;
sensor_connection_thread = std::make_unique([this]() {
+ NODELET_DEBUG("sensor_connection_thread active.");
auto& pf = sensor::get_format(info);
while (sensor_connection_active) {
connection_loop(*sensor_client, pf);
@@ -585,9 +668,9 @@ void OusterSensor::start_packet_processing_threads() {
imu_packets_processing_thread_active = true;
imu_packets_processing_thread = std::make_unique([this]() {
while (imu_packets_processing_thread_active) {
- imu_packets->read([this](const uint8_t* buffer) {
- on_imu_packet_msg(buffer);
- });
+ imu_packets->read_timeout([this](const uint8_t* buffer) {
+ if (buffer) on_imu_packet_msg(buffer);
+ }, 1s);
}
NODELET_DEBUG("imu_packets_processing_thread done.");
});
@@ -595,9 +678,9 @@ void OusterSensor::start_packet_processing_threads() {
lidar_packets_processing_thread_active = true;
lidar_packets_processing_thread = std::make_unique([this]() {
while (lidar_packets_processing_thread_active) {
- lidar_packets->read([this](const uint8_t* buffer) {
- on_lidar_packet_msg(buffer);
- });
+ lidar_packets->read_timeout([this](const uint8_t* buffer) {
+ if (buffer) on_lidar_packet_msg(buffer);
+ }, 1s);
}
NODELET_DEBUG("lidar_packets_processing_thread done.");
@@ -639,7 +722,7 @@ void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) {
// param init_id_reset is overriden to true when force_reinit is true
void OusterSensor::reset_sensor(bool /*force_reinit*/, bool /*init_id_reset*/) {
- NODELET_WARN("sensor reset is invoked but sensor it is not implemented");
+ schedule_stop();
}
void OusterSensor::reactivate_sensor(bool /*init_id_reset*/) {
diff --git a/src/os_sensor_nodelet.h b/src/os_sensor_nodelet.h
index d45fc1a6..93a09822 100644
--- a/src/os_sensor_nodelet.h
+++ b/src/os_sensor_nodelet.h
@@ -32,10 +32,9 @@ class OusterSensor : public OusterSensorNodeletBase {
public:
~OusterSensor() override;
- private:
+ protected:
virtual void onInit() override;
- protected:
virtual void on_metadata_updated(const sensor::sensor_info& info);
virtual void create_services();
@@ -46,12 +45,20 @@ class OusterSensor : public OusterSensorNodeletBase {
virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet);
+ bool start();
+
+ void stop();
+
+ void attempt_start();
+
+ void schedule_stop();
+
void halt();
private:
std::string get_sensor_hostname();
- void update_config_and_metadata(sensor::client& client);
+ void update_metadata(sensor::client& client);
void save_metadata();
@@ -75,7 +82,7 @@ class OusterSensor : public OusterSensorNodeletBase {
uint8_t compose_config_flags(const sensor::sensor_config& config);
- void configure_sensor(const std::string& hostname,
+ bool configure_sensor(const std::string& hostname,
sensor::sensor_config& config);
std::string load_config_file(const std::string& config_file);
@@ -110,10 +117,12 @@ class OusterSensor : public OusterSensorNodeletBase {
void stop_packet_processing_threads();
+ bool get_active_config_no_throw(const std::string& sensor_hostname,
+ sensor::sensor_config& config);
+
private:
std::string sensor_hostname;
- std::string staged_config;
- std::string active_config;
+ std::optional staged_config;
std::string mtp_dest;
bool mtp_main;
std::shared_ptr sensor_client;
@@ -140,9 +149,10 @@ class OusterSensor : public OusterSensorNodeletBase {
bool persist_config = false;
bool force_sensor_reinit = false;
+ bool auto_udp_allowed = false;
bool reset_last_init_id = true;
- nonstd::optional last_init_id;
+ std::optional last_init_id;
// TODO: add as a ros parameter
const int max_poll_client_error_count = 10;
@@ -153,6 +163,11 @@ class OusterSensor : public OusterSensorNodeletBase {
// TODO: add as a ros parameter
const int max_read_imu_packet_errors = 60;
int read_imu_packet_errors = 0;
+
+ bool attempt_reconnect;
+ double dormant_period_between_reconnects;
+ int reconnect_attempts_available;
+ ros::Timer reconnect_timer;
};
} // namespace ouster_ros
diff --git a/src/os_sensor_nodelet_base.cpp b/src/os_sensor_nodelet_base.cpp
index 6f4b1aec..3a7f00f8 100644
--- a/src/os_sensor_nodelet_base.cpp
+++ b/src/os_sensor_nodelet_base.cpp
@@ -32,7 +32,7 @@ void OusterSensorNodeletBase::create_get_metadata_service() {
NODELET_INFO("get_metadata service created");
}
-void OusterSensorNodeletBase::create_metadata_publisher() {
+void OusterSensorNodeletBase::create_metadata_pub() {
auto& nh = getNodeHandle();
metadata_pub = nh.advertise("metadata", 1, true);
}