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);