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