diff --git a/nebula_ros/config/lidar/tutorial/Tutorial.param.yaml b/nebula_ros/config/lidar/tutorial/Tutorial.param.yaml index 6aa0821ca..a0ba24813 100644 --- a/nebula_ros/config/lidar/tutorial/Tutorial.param.yaml +++ b/nebula_ros/config/lidar/tutorial/Tutorial.param.yaml @@ -18,9 +18,5 @@ calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 return_mode: LastStrongest - ptp_profile: "1588v2" - ptp_domain: 0 - ptp_transport_type: UDP - ptp_switch_type: TSN retry_hw: false dual_return_distance_threshold: 0.1 \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/tutorial/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/tutorial/decoder_wrapper.hpp index 7158fe083..4ad60d3ae 100644 --- a/nebula_ros/include/nebula_ros/tutorial/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/tutorial/decoder_wrapper.hpp @@ -41,13 +41,6 @@ class TutorialDecoderWrapper void OnConfigChange( const std::shared_ptr & new_config); - void OnCalibrationChange( - const std::shared_ptr & - new_calibration); - - rcl_interfaces::msg::SetParametersResult OnParameterChange( - const std::vector & p); - nebula::Status Status(); private: diff --git a/nebula_ros/schema/Tutorial.schema.json b/nebula_ros/schema/Tutorial.schema.json index 78cc79e23..e8ad2f6c9 100644 --- a/nebula_ros/schema/Tutorial.schema.json +++ b/nebula_ros/schema/Tutorial.schema.json @@ -71,18 +71,6 @@ "Dual" ] }, - "ptp_profile": { - "$ref": "sub/communication.json#/definitions/ptp_profile" - }, - "ptp_domain": { - "$ref": "sub/communication.json#/definitions/ptp_domain" - }, - "ptp_transport_type": { - "$ref": "sub/communication.json#/definitions/ptp_transport_type" - }, - "ptp_switch_type": { - "$ref": "sub/communication.json#/definitions/ptp_switch_type" - }, "retry_hw": { "$ref": "sub/hardware.json#/definitions/retry_hw" }, @@ -107,10 +95,6 @@ "calibration_file", "rotation_speed", "return_mode", - "ptp_profile", - "ptp_domain", - "ptp_transport_type", - "ptp_switch_type", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/src/tutorial/decoder_wrapper.cpp b/nebula_ros/src/tutorial/decoder_wrapper.cpp index 3814e0921..28d7c388d 100644 --- a/nebula_ros/src/tutorial/decoder_wrapper.cpp +++ b/nebula_ros/src/tutorial/decoder_wrapper.cpp @@ -73,114 +73,11 @@ void TutorialDecoderWrapper::OnConfigChange( sensor_cfg_ = new_config; } -void TutorialDecoderWrapper::OnCalibrationChange( - const std::shared_ptr& new_calibration) -{ - std::lock_guard lock(mtx_driver_ptr_); - auto new_driver = std::make_shared(sensor_cfg_, new_calibration); - driver_ptr_ = new_driver; - calibration_cfg_ptr_ = new_calibration; - calibration_file_path_ = calibration_cfg_ptr_->calibration_file; -} - -rcl_interfaces::msg::SetParametersResult TutorialDecoderWrapper::OnParameterChange(const std::vector& p) -{ - using namespace rcl_interfaces::msg; - - std::string calibration_path = ""; - - // Only one of the two parameters is defined, so not checking for sensor model explicitly here is fine - bool got_any = get_param(p, "calibration_file", calibration_path) | get_param(p, "correction_file", calibration_path); - if (!got_any) - { - return rcl_interfaces::build().successful(true).reason(""); - } - - if (!std::filesystem::exists(calibration_path)) - { - auto result = SetParametersResult(); - result.successful = false; - result.reason = "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; - return result; - } - - auto get_calibration_result = GetCalibrationData(calibration_path, true); - if (!get_calibration_result.has_value()) - { - auto result = SetParametersResult(); - result.successful = false; - result.reason = (std::stringstream() << "Could not change calibration file to '" << calibration_path - << "': " << get_calibration_result.error()) - .str(); - return result; - } - - OnCalibrationChange(get_calibration_result.value()); - RCLCPP_INFO_STREAM(logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); - return rcl_interfaces::build().successful(true).reason(""); -} - get_calibration_result_t TutorialDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path, bool ignore_others) { auto calib = std::make_shared(); - bool hw_connected = hw_interface_ != nullptr; - std::string calibration_file_path_from_sensor; - - { - int ext_pos = calibration_file_path.find_last_of('.'); - calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); - // TODO: if multiple different sensors of the same type are used, this will mix up their calibration data - calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); - } - - // If a sensor is connected, try to download and save its calibration data - if (!ignore_others && hw_connected) - { - try - { - auto raw_data = hw_interface_->GetLidarCalibrationBytes(); - RCLCPP_INFO(logger_, "Downloaded calibration data from sensor."); - auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); - if (status != Status::OK) - { - RCLCPP_ERROR_STREAM(logger_, "Could not save calibration data: " << status); - } - else - { - RCLCPP_INFO_STREAM(logger_, "Saved downloaded data to " << calibration_file_path_from_sensor); - } - } - catch (std::runtime_error& e) - { - RCLCPP_ERROR_STREAM(logger_, "Could not download calibration data: " << e.what()); - } - } - - // If saved calibration data from a sensor exists (either just downloaded above, or previously), try to load it - if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) - { - auto status = calib->LoadFromFile(calibration_file_path_from_sensor); - if (status == Status::OK) - { - calib->calibration_file = calibration_file_path_from_sensor; - return calib; - } - - RCLCPP_ERROR_STREAM(logger_, "Could not load downloaded calibration data: " << status); - } - else if (!ignore_others) - { - RCLCPP_ERROR(logger_, "No downloaded calibration data found."); - } - - if (!ignore_others) - { - RCLCPP_WARN(logger_, "Falling back to generic calibration file."); - } - // If downloaded data did not exist or could not be loaded, fall back to a generic file. // If that file does not exist either, return an error code if (!std::filesystem::exists(calibration_file_path)) diff --git a/nebula_ros/src/tutorial/hw_monitor_wrapper.cpp b/nebula_ros/src/tutorial/hw_monitor_wrapper.cpp index facd5c4a4..04469a735 100644 --- a/nebula_ros/src/tutorial/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/tutorial/hw_monitor_wrapper.cpp @@ -1,5 +1,7 @@ #include "nebula_ros/tutorial/hw_monitor_wrapper.hpp" +#include + namespace nebula { namespace ros @@ -20,7 +22,8 @@ TutorialHwMonitorWrapper::TutorialHwMonitorWrapper(rclcpp::Node* const parent_no void TutorialHwMonitorWrapper::InitializeDiagnostics() { - std::string hardware_id = ""; // get hardware ID (model:serial_number) from sensor here + // get hardware ID (model:serial_number) from sensor here using HW interface + std::string hardware_id = "Tutorial:ABC-12345-DEF"; diagnostics_updater_.setHardwareID(hardware_id); RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hardware_id); @@ -39,30 +42,39 @@ void TutorialHwMonitorWrapper::InitializeDiagnostics() void TutorialHwMonitorWrapper::ParseAndAddDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + // //////////////////////////////////////// + // Add diagnostics values to ROS object + // //////////////////////////////////////// diagnostics.add("voltage", current_diag_info_->get("status.voltage")); - diagnostics.add("error", current_diag_info_->get("status.error")); - // etc. - diagnostics.summary(level, "OK"); + auto error = current_diag_info_->get("status.error"); + diagnostics.add("error", error); + + // //////////////////////////////////////// + // Check error conditions and set status + // //////////////////////////////////////// + + if (error != "No error") { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, error); + return; + } + + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); } void TutorialHwMonitorWrapper::TriggerDiagnosticsUpdate() { - RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); auto now = parent_node_->get_clock()->now(); auto dif = (now - *current_diag_info_time_).seconds(); - RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); - if (diag_span_ * 2.0 < dif * 1000) { - RCLCPP_DEBUG_STREAM(logger_, "STALE"); + RCLCPP_WARN_STREAM(logger_, "Diagnostics STALE for " << dif << " s"); } else { - RCLCPP_DEBUG_STREAM(logger_, "OK"); + RCLCPP_DEBUG_STREAM(logger_, "Diagnostics OK (up to date)"); } diagnostics_updater_.force_update(); @@ -70,13 +82,20 @@ void TutorialHwMonitorWrapper::TriggerDiagnosticsUpdate() void TutorialHwMonitorWrapper::FetchDiagnosticInfo() { - current_diag_info_ = {}; // fetch from sensor here + // This is where you would fetch the newest diagnostics from the sensor using the hardware + // interface. The below code is just a dummy. + + const char status[] = "{ \"status\": { \"voltage\": 14.00, \"error\": \"No error\" } }"; + boost::iostreams::stream stream(status, sizeof(status)); + boost::property_tree::ptree parsed_diag_info; + boost::property_tree::read_json(stream, parsed_diag_info); + current_diag_info_time_ = parent_node_->now(); } Status TutorialHwMonitorWrapper::Status() { - return Status::NOT_IMPLEMENTED; // TODO + return Status::OK; } } // namespace ros } // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/tutorial/tutorial_ros_wrapper.cpp b/nebula_ros/src/tutorial/tutorial_ros_wrapper.cpp index dc1ac900e..000529511 100644 --- a/nebula_ros/src/tutorial/tutorial_ros_wrapper.cpp +++ b/nebula_ros/src/tutorial/tutorial_ros_wrapper.cpp @@ -135,32 +135,6 @@ nebula::Status TutorialRosWrapper::DeclareAndGetSensorConfigParams() declare_parameter("dual_return_distance_threshold", descriptor); } - auto _ptp_profile = declare_parameter("ptp_profile", param_read_only()); - config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); - - auto _ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); - config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); - - if (config.ptp_transport_type != drivers::PtpTransportType::L2 && - config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && - config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) - { - RCLCPP_WARN_STREAM(get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" - << _ptp_profile - << "' only supports 'L2'. Setting it to 'L2'."); - config.ptp_transport_type = drivers::PtpTransportType::L2; - set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); - } - - auto _ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); - config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); - - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.integer_range = int_range(0, 127, 1); - config.ptp_domain = declare_parameter("ptp_domain", descriptor); - } - auto new_cfg_ptr = std::make_shared(config); return ValidateAndSetConfig(new_cfg_ptr); } @@ -179,23 +153,6 @@ Status TutorialRosWrapper::ValidateAndSetConfig(std::shared_ptrptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) - { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); - return Status::SENSOR_CONFIG_ERROR; - } - if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) - { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " - "using the '1588v2' PTP Profile"); - return Status::SENSOR_CONFIG_ERROR; - } - if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) - { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); - return Status::SENSOR_CONFIG_ERROR; - } if (hw_interface_wrapper_) { @@ -219,7 +176,7 @@ void TutorialRosWrapper::ReceiveScanMessageCallback(std::unique_ptrOnParameterChange(p); - if (!result.successful) { - return result; - } - } + // Currently, all wrappers have only read-only parameters, so their update logic is not implemented // //////////////////////////////////////// // Create new, updated sensor configuration