diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml
index 8d1e847b..5404632b 100644
--- a/.github/workflows/docker-image.yml
+++ b/.github/workflows/docker-image.yml
@@ -18,8 +18,9 @@ jobs:
- rolling
- humble
- iron
+ - jazzy
steps:
- - uses: actions/checkout@v3
+ - uses: actions/checkout@v4
with:
submodules: true
- name: Build the Docker image
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 4b6bef02..9c5255b8 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -15,9 +15,15 @@ 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.xml`` file.
+* Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg
+ documentation and the associated params to enable. Known Issues:
+ - Doesn't handle detect and handle invalid configurations
+* Added an automatic start mode to make it easier to start the node without using time actions.
+ - To disable set ``auto_start`` to ``false`` during launch
+
ouster_ros v0.12.0
==================
diff --git a/README.md b/README.md
index d5b6cb40..93d8dd56 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
# Official ROS driver for Ouster sensors
[ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) |
-[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
+[ROS2 (rolling/humble/iron/jazzy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
[ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
@@ -9,7 +9,7 @@
| ROS Version | Build Status (Linux) |
|:-----------:|:------:|
| ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
-| ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
+| ROS2 (rolling/humble/iron/jazzy) | [![rolling/humble/iron/jazzy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
| ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
- [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors)
@@ -57,9 +57,9 @@ You can obtain detailed specs sheet about the sensors and obtain updated FW thro
[downloads](https://ouster.com/downloads) section.
## Requirements
-This branch is only intended for use with **Rolling**, **Humble** and **Iron** ROS 2 distros. Please
-refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with the
-remainder of this guide.
+This branch is only intended for use with **Rolling**, **Humble**, **Iron** and **Jazzy** ROS 2 distros.
+Please refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with
+the remainder of this guide.
> **Note**
> If you have _rosdep_ tool installed on your system you can then use the following command to get all
@@ -77,7 +77,7 @@ sudo apt install -y \
ros-$ROS_DISTRO-tf2-eigen \
ros-$ROS_DISTRO-rviz2
```
-where `$ROS_DISTRO` can be either ``rolling``, ``humble`` or ``iron``.
+where `$ROS_DISTRO` can be either ``rolling``, ``humble``, ``iron`` or ``jazzy``.
> **Note**
> Installing `ros-$ROS_DISTRO-rviz` package is optional in case you didn't need to visualize the
@@ -120,7 +120,7 @@ git clone -b ros2 --recurse-submodules https://github.com/ouster-lidar/ouster-ro
Next to compile the driver you need to source the ROS environemt into the active termainl:
```bash
-source /opt/ros//setup.bash # replace ros-distro with 'rolling', 'humble', or 'iron'
+source /opt/ros//setup.bash # replace ros-distro with 'rolling', 'humble', 'iron' or 'jazzy'
```
Finally, invoke `colcon build` command from within the catkin workspace as shown below:
diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml
index bab4451a..b69d7ca8 100644
--- a/ouster-ros/config/driver_params.yaml
+++ b/ouster-ros/config/driver_params.yaml
@@ -93,3 +93,12 @@ ouster/os_driver:
azimuth_window_end: 360000
# persist_config[optional]: request the sensor to persist settings
persist_config: false
+ # attempt_config[optional]: attempting to reconnect to the sensor after
+ # connection loss or sensor powered down
+ attempt_reconnect: false
+ # dormant_period_between_reconnects[optional]: wait time in seconds between
+ # reconnection attempts
+ dormant_period_between_reconnects: 1.0
+ # max_failed_reconnect_attempts[optional]: maximum number of attempts trying
+ # to communicate with the sensor. Counter resets upon successful connection.
+ max_failed_reconnect_attempts: 2147483647
diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml
index 76999cfa..52296c1d 100644
--- a/ouster-ros/config/os_sensor_cloud_image_params.yaml
+++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml
@@ -14,10 +14,13 @@ ouster/os_sensor:
metadata: ''
lidar_port: 0
imu_port: 0
- use_system_default_qos: False
+ use_system_default_qos: false
azimuth_window_start: 0
azimuth_window_end: 360000
persist_config: false
+ attempt_reconnect: false
+ dormant_period_between_reconnects: 1.0
+ max_failed_reconnect_attempts: 2147483647
ouster/os_cloud:
ros__parameters:
sensor_frame: os_sensor
@@ -27,9 +30,9 @@ ouster/os_cloud:
timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode
ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588
proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options
- use_system_default_qos: False # needs to match the value defined for os_sensor node
+ use_system_default_qos: false # needs to match the value defined for os_sensor node
scan_ring: 0 # Use this parameter in conjunction with the SCAN flag and choose a
# value the range [0, sensor_beams_count)
point_type: original # choose from: {original, native, xyz, xyzi, xyzir}
ouster/os_image:
- use_system_default_qos: False # needs to match the value defined for os_sensor node
+ use_system_default_qos: false # needs to match the value defined for os_sensor node
diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h
index 4de4cbb5..cb303a48 100644
--- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h
+++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h
@@ -30,7 +30,7 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
void create_get_metadata_service();
- void create_metadata_publisher();
+ void create_metadata_pub();
void publish_metadata();
diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml
index 42bf96d6..0908ada5 100644
--- a/ouster-ros/launch/record.composite.launch.xml
+++ b/ouster-ros/launch/record.composite.launch.xml
@@ -60,9 +60,7 @@
description="Use the default system QoS settings"/>
+ use any combination of the 4 flags to enable or disable specific processors"/>
+
+
+
+
+
+
@@ -103,6 +113,12 @@
+
+
+
+
@@ -118,17 +134,11 @@
+
-
-
-
-
diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml
index 2f5ce29c..1625f16e 100644
--- a/ouster-ros/launch/replay.composite.launch.xml
+++ b/ouster-ros/launch/replay.composite.launch.xml
@@ -47,9 +47,7 @@
description="Use the default system QoS settings"/>
+ use any combination of the 4 flags to enable or disable specific processors"/>
+
diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
index 20ba21fb..6c90f40d 100644
--- a/ouster-ros/launch/replay_pcap.launch.xml
+++ b/ouster-ros/launch/replay_pcap.launch.xml
@@ -38,9 +38,7 @@
description="Use the default system QoS settings"/>
+ use any combination of the 4 flags to enable or disable specific processors"/>
+
diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py
index 5932403f..7be2e150 100644
--- a/ouster-ros/launch/sensor.composite.launch.py
+++ b/ouster-ros/launch/sensor.composite.launch.py
@@ -36,12 +36,16 @@ def generate_launch_description():
rviz_enable = LaunchConfiguration('viz')
rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True')
+ auto_start = LaunchConfiguration('auto_start')
+ auto_start_arg = DeclareLaunchArgument('auto_start', default_value='True')
+
os_sensor = ComposableNode(
package='ouster_ros',
plugin='ouster_ros::OusterSensor',
name='os_sensor',
namespace=ouster_ns,
- parameters=[params_file]
+ parameters=[params_file,
+ {'auto_start': auto_start}]
)
os_cloud = ComposableNode(
@@ -80,25 +84,11 @@ def generate_launch_description():
condition=IfCondition(rviz_enable)
)
- # HACK: to configure and activate the the sensor since state transition
- # API doesn't seem to support composable nodes yet.
-
- def invoke_lifecycle_cmd(node_name, verb):
- ros2_exec = FindExecutable(name='ros2')
- return ExecuteProcess(
- cmd=[[ros2_exec, ' lifecycle set ',
- ouster_ns, '/', node_name, ' ', verb]],
- shell=True)
-
- sensor_configure_cmd = invoke_lifecycle_cmd('os_sensor', 'configure')
- sensor_activate_cmd = invoke_lifecycle_cmd('os_sensor', 'activate')
-
return launch.LaunchDescription([
params_file_arg,
ouster_ns_arg,
rviz_enable_arg,
+ auto_start_arg,
rviz_launch,
- os_container,
- sensor_configure_cmd,
- TimerAction(period=1.0, actions=[sensor_activate_cmd])
+ os_container
])
diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml
index 19119494..cdc73854 100644
--- a/ouster-ros/launch/sensor.composite.launch.xml
+++ b/ouster-ros/launch/sensor.composite.launch.xml
@@ -81,6 +81,18 @@
+
+
+
+
+
+
@@ -106,16 +118,15 @@
+
+
+
+
-
-
-
-
diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml
index 43f26b4a..21cbeac1 100644
--- a/ouster-ros/launch/sensor.independent.launch.xml
+++ b/ouster-ros/launch/sensor.independent.launch.xml
@@ -58,9 +58,7 @@
description="Use the default system QoS settings"/>
+ use any combination of the 4 flags to enable or disable specific processors"/>
+
+
+
+
+
+
@@ -100,6 +110,12 @@
+
+
+
+
@@ -115,16 +131,10 @@
+
-
-
-
-
diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml
index 97892956..ab721f82 100644
--- a/ouster-ros/launch/sensor_mtp.launch.xml
+++ b/ouster-ros/launch/sensor_mtp.launch.xml
@@ -89,6 +89,18 @@
+
+
+
+
+
+
@@ -114,16 +126,15 @@
+
+
+
+
-
-
-
-
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index d32bcd86..20d79eb3 100644
--- a/ouster-ros/package.xml
+++ b/ouster-ros/package.xml
@@ -2,7 +2,7 @@
ouster_ros
- 0.12.6
+ 0.12.7
Ouster ROS2 driver
ouster developers
BSD
diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h
index 138cf477..9400e16e 100644
--- a/ouster-ros/src/lidar_packet_handler.h
+++ b/ouster-ros/src/lidar_packet_handler.h
@@ -126,8 +126,14 @@ class LidarPacketHandler {
LidarPacketHandler(const LidarPacketHandler&) = delete;
LidarPacketHandler& operator=(const LidarPacketHandler&) = delete;
- ~LidarPacketHandler() = default;
-
+ ~LidarPacketHandler() {
+ RCLCPP_DEBUG(rclcpp::get_logger(getName()),
+ "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);
}
@@ -153,10 +159,13 @@ 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/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp
index 730ac899..0265027c 100644
--- a/ouster-ros/src/os_pcap_node.cpp
+++ b/ouster-ros/src/os_pcap_node.cpp
@@ -48,7 +48,7 @@ class OusterPcap : public OusterSensorNodeBase {
try {
auto meta_file = get_meta_file();
auto pcap_file = get_pcap_file();
- create_metadata_publisher();
+ create_metadata_pub();
load_metadata_from_file(meta_file);
open_pcap(pcap_file);
publish_metadata();
diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp
index 4f78397f..f630dc64 100644
--- a/ouster-ros/src/os_replay_node.cpp
+++ b/ouster-ros/src/os_replay_node.cpp
@@ -28,7 +28,7 @@ class OusterReplay : public OusterSensorNodeBase {
try {
auto meta_file = parse_parameters();
- create_metadata_publisher();
+ create_metadata_pub();
load_metadata_from_file(meta_file);
publish_metadata();
create_get_metadata_service();
diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp
index 46f60ca4..343127c9 100644
--- a/ouster-ros/src/os_sensor_node.cpp
+++ b/ouster-ros/src/os_sensor_node.cpp
@@ -32,6 +32,25 @@ OusterSensor::OusterSensor(const std::string& name,
change_state_client{
create_client(get_name() + "/change_state"s)} {
declare_parameters();
+ attempt_reconnect = get_parameter("attempt_reconnect").as_bool();
+ dormant_period_between_reconnects =
+ get_parameter("dormant_period_between_reconnects").as_double();
+ reconnect_attempts_available =
+ get_parameter("max_failed_reconnect_attempts").as_int();
+
+ bool auto_start = get_parameter("auto_start").as_bool();
+
+ if (auto_start) {
+ RCLCPP_INFO(get_logger(), "auto start requested");
+ reconnect_timer = create_wall_timer(1s, [this]() {
+ reconnect_timer->cancel();
+ auto request_transitions = std::vector{
+ lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE,
+ lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE};
+ execute_transitions_sequence(request_transitions, 0);
+ RCLCPP_INFO(get_logger(), "auto start initiated");
+ });
+ }
}
OusterSensor::OusterSensor(const rclcpp::NodeOptions& options)
@@ -64,6 +83,43 @@ void OusterSensor::declare_parameters() {
declare_parameter("azimuth_window_start", MIN_AZW);
declare_parameter("azimuth_window_end", MAX_AZW);
declare_parameter("persist_config", false);
+ declare_parameter("attempt_reconnect", false);
+ declare_parameter("dormant_period_between_reconnects", 1.0);
+ declare_parameter("max_failed_reconnect_attempts", INT_MAX);
+ declare_parameter("auto_start", false);
+}
+
+bool OusterSensor::start() {
+ sensor_hostname = get_sensor_hostname();
+ 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;
+
+ RCLCPP_INFO(get_logger(), "Retrived sensor active config");
+ // 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;
+ }
+
+ reset_last_init_id = true;
+ sensor_client = create_sensor_client(sensor_hostname, config);
+ if (!sensor_client)
+ return false;
+
+ create_metadata_pub();
+ update_metadata(*sensor_client);
+ create_services();
+
+ return true;
}
LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure(
@@ -71,17 +127,26 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure(
RCLCPP_DEBUG(get_logger(), "on_configure() is called.");
try {
- sensor_hostname = get_sensor_hostname();
- auto config = staged_config.empty()
- ? parse_config_from_ros_parameters()
- : parse_config_from_staged_config_string();
- configure_sensor(sensor_hostname, config);
- sensor_client = create_sensor_client(sensor_hostname, config);
- if (!sensor_client)
+ if (!start()) {
+ auto sleep_duration = std::chrono::duration(dormant_period_between_reconnects);
+ reconnect_timer = create_wall_timer(sleep_duration, [this]() {
+ reconnect_timer->cancel();
+ if (attempt_reconnect && reconnect_attempts_available-- > 0) {
+ RCLCPP_INFO_STREAM(get_logger(), "Attempting to communicate with the sensor, "
+ "remaining attempts: " << reconnect_attempts_available);
+
+ auto request_transitions = std::vector{
+ lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE,
+ lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE};
+ execute_transitions_sequence(request_transitions, 0);
+ }
+ });
return LifecycleNodeInterface::CallbackReturn::FAILURE;
- create_metadata_publisher();
- update_config_and_metadata(*sensor_client);
- create_services();
+ } else {
+ // reset counter
+ reconnect_attempts_available =
+ get_parameter("max_failed_reconnect_attempts").as_int();
+ }
} catch (const std::exception& ex) {
RCLCPP_ERROR_STREAM(
get_logger(),
@@ -98,8 +163,8 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_activate(
const rclcpp_lifecycle::State& state) {
RCLCPP_DEBUG(get_logger(), "on_activate() is called.");
LifecycleNode::on_activate(state);
- if (active_config.empty() || cached_metadata.empty())
- update_config_and_metadata(*sensor_client);
+ if (cached_metadata.empty())
+ update_metadata(*sensor_client);
create_publishers();
allocate_buffers();
start_packet_processing_threads();
@@ -118,8 +183,7 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_deactivate(
const rclcpp_lifecycle::State& state) {
RCLCPP_DEBUG(get_logger(), "on_deactivate() is called.");
LifecycleNode::on_deactivate(state);
- stop_packet_processing_threads();
- stop_sensor_connection_thread();
+ halt();
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
@@ -181,19 +245,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 = "Failed to collect sensor config";
- RCLCPP_ERROR_STREAM(get_logger(), 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) {
@@ -277,9 +329,9 @@ bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback,
request->transition.id = transition_id;
// send an async request to perform the transition
change_state_client->async_send_request(
- request, [callback,
- callback_args...](rclcpp::Client::SharedFuture) {
- callback(callback_args...);
+ request, [this, callback,
+ callback_args...](rclcpp::Client::SharedFuture ff) {
+ callback(callback_args..., ff.get()->success);
});
return true;
}
@@ -292,15 +344,22 @@ void OusterSensor::execute_transitions_sequence(
RCLCPP_DEBUG_STREAM(
get_logger(), "transition: [" << transition_id_to_string(transition_id)
<< "] started");
- change_state(transition_id, [this, transitions_sequence, at]() {
- RCLCPP_DEBUG_STREAM(
- get_logger(),
- "transition: [" << transition_id_to_string(transitions_sequence[at])
- << "] completed");
- if (at < transitions_sequence.size() - 1) {
- execute_transitions_sequence(transitions_sequence, at + 1);
+ change_state(transition_id, [this, transitions_sequence, at](bool success) {
+ if (success) {
+ RCLCPP_DEBUG_STREAM(
+ get_logger(),
+ "transition: [" << transition_id_to_string(transitions_sequence[at])
+ << "] completed");
+ if (at < transitions_sequence.size() - 1) {
+ execute_transitions_sequence(transitions_sequence, at + 1);
+ } else {
+ RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
+ }
} else {
- RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
+ RCLCPP_DEBUG_STREAM(
+ get_logger(),
+ "transition: [" << transition_id_to_string(transitions_sequence[at])
+ << "] failed");
}
});
}
@@ -335,7 +394,6 @@ void OusterSensor::reactivate_sensor(bool init_id_reset) {
}
reset_last_init_id = init_id_reset;
- active_config.clear();
cached_metadata.clear();
auto request_transitions = std::vector{
lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE,
@@ -354,10 +412,32 @@ void OusterSensor::create_reset_service() {
RCLCPP_INFO(get_logger(), "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&) {
+ RCLCPP_ERROR_STREAM(
+ get_logger(),
+ "Couldn't get active config for: " << sensor_hostname);
+ return false;
+ }
+
+ RCLCPP_ERROR_STREAM(
+ get_logger(),
+ "Couldn't get active config for: " << sensor_hostname);
+ return false;
+}
+
void OusterSensor::create_get_config_service() {
get_config_srv = create_service(
"get_config", [this](const std::shared_ptr,
std::shared_ptr 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;
});
@@ -370,10 +450,10 @@ void OusterSensor::create_set_config_service() {
"set_config", [this](const std::shared_ptr request,
std::shared_ptr 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()) {
RCLCPP_ERROR_STREAM(
get_logger(),
"provided config file: "
@@ -389,7 +469,8 @@ void OusterSensor::create_set_config_service() {
return false;
}
- response->config = staged_config;
+ staged_config = sensor::parse_config(config_str);
+ response->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
@@ -402,13 +483,15 @@ void OusterSensor::create_set_config_service() {
std::shared_ptr OusterSensor::create_sensor_client(
const std::string& hostname, const sensor::sensor_config& config) {
- RCLCPP_INFO_STREAM(get_logger(),
- "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() : "";
+ RCLCPP_INFO_STREAM(get_logger(),
+ "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 receive data via multicast
@@ -421,9 +504,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);
}
if (!cli) {
@@ -548,6 +630,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 ||
@@ -563,12 +647,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;
@@ -600,6 +678,7 @@ uint8_t OusterSensor::compose_config_flags(
}
if (persist_config) {
+ persist_config = false; // avoid persisting configs implicitly on restarts
RCLCPP_INFO(get_logger(), "Configuration will be persisted");
config_flags |= ouster::sensor::CONFIG_PERSIST;
}
@@ -607,25 +686,32 @@ 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) {
if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) &&
!mtp_main) {
if (!get_config(hostname, config, true)) {
RCLCPP_ERROR(get_logger(), "Error getting active config");
- } else {
- RCLCPP_INFO(get_logger(), "Retrived active config of sensor");
+ return false;
}
- return;
+
+ RCLCPP_INFO(get_logger(), "Retrived active config of sensor");
+ return true;
}
uint8_t config_flags = compose_config_flags(config);
- if (!set_config(hostname, config, config_flags)) {
- throw std::runtime_error("Error connecting to sensor " + hostname);
+ RCLCPP_INFO_STREAM(get_logger(), "Contacting sensor " << hostname << " ...");
+ try {
+ set_config(hostname, config, config_flags);
+ } catch (const std::exception& ex) {
+ RCLCPP_ERROR_STREAM(get_logger(), "Error connecting to sensor " << hostname <<
+ ", details: " << ex.what());
+ return false;
}
RCLCPP_INFO_STREAM(get_logger(),
"Sensor " << hostname << " configured successfully");
+ return true;
}
// fill in values that could not be parsed from metadata
@@ -716,7 +802,7 @@ bool OusterSensor::init_id_changed(const sensor::packet_format& pf,
void OusterSensor::handle_poll_client_error() {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100,
- "sensor::poll_client()) returned error");
+ "sensor::poll_client()) returned 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) {
@@ -790,7 +876,7 @@ void OusterSensor::connection_loop(sensor::client& cli,
RCLCPP_INFO(get_logger(), "poll_client: caught signal, exiting!");
return;
}
- if (state & sensor::CLIENT_ERROR) {
+ if (state & sensor::CLIENT_ERROR || state == sensor::TIMEOUT) {
handle_poll_client_error();
return;
}
@@ -806,6 +892,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]() {
+ RCLCPP_DEBUG(get_logger(), "sensor_connection_thread active.");
auto& pf = sensor::get_format(info);
while (sensor_connection_active) {
connection_loop(*sensor_client, pf);
diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h
index ee90dd6d..e27e2ea4 100644
--- a/ouster-ros/src/os_sensor_node.h
+++ b/ouster-ros/src/os_sensor_node.h
@@ -67,6 +67,8 @@ class OusterSensor : public OusterSensorNodeBase {
virtual void cleanup();
+ bool start();
+
void halt();
private:
@@ -74,7 +76,7 @@ class OusterSensor : public OusterSensorNodeBase {
std::string get_sensor_hostname();
- void update_config_and_metadata(sensor::client& client);
+ void update_metadata(sensor::client& client);
void save_metadata();
@@ -104,11 +106,9 @@ class OusterSensor : public OusterSensorNodeBase {
sensor::sensor_config parse_config_from_ros_parameters();
- sensor::sensor_config parse_config_from_staged_config_string();
-
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);
@@ -141,10 +141,12 @@ class OusterSensor : public OusterSensorNodeBase {
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;
@@ -172,9 +174,9 @@ class OusterSensor : public OusterSensorNodeBase {
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;
@@ -188,6 +190,11 @@ class OusterSensor : public OusterSensorNodeBase {
const int MIN_AZW = 0;
const int MAX_AZW = 360000;
+
+ bool attempt_reconnect;
+ double dormant_period_between_reconnects;
+ int reconnect_attempts_available;
+ rclcpp::TimerBase::SharedPtr reconnect_timer;
};
} // namespace ouster_ros
\ No newline at end of file
diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp
index a238d974..a0dadedd 100644
--- a/ouster-ros/src/os_sensor_node_base.cpp
+++ b/ouster-ros/src/os_sensor_node_base.cpp
@@ -30,7 +30,7 @@ void OusterSensorNodeBase::create_get_metadata_service() {
RCLCPP_INFO(get_logger(), "get_metadata service created");
}
-void OusterSensorNodeBase::create_metadata_publisher() {
+void OusterSensorNodeBase::create_metadata_pub() {
auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1));
latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);