Skip to content

Commit

Permalink
chore(tutorial): remove superfluous features, make wrapper code clearer
Browse files Browse the repository at this point in the history
  • Loading branch information
@mojomex
mojomex committed Jun 1, 2024
1 parent 79bc3a1 commit cd26997
Show file tree
Hide file tree
Showing 6 changed files with 33 additions and 195 deletions.
4 changes: 0 additions & 4 deletions nebula_ros/config/lidar/tutorial/Tutorial.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
7 changes: 0 additions & 7 deletions nebula_ros/include/nebula_ros/tutorial/decoder_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,13 +41,6 @@ class TutorialDecoderWrapper
void OnConfigChange(
const std::shared_ptr<const TutorialSensorConfiguration> & new_config);

void OnCalibrationChange(
const std::shared_ptr<const TutorialCalibrationConfiguration> &
new_calibration);

rcl_interfaces::msg::SetParametersResult OnParameterChange(
const std::vector<rclcpp::Parameter> & p);

nebula::Status Status();

private:
Expand Down
16 changes: 0 additions & 16 deletions nebula_ros/schema/Tutorial.schema.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
},
Expand All @@ -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"
],
Expand Down
103 changes: 0 additions & 103 deletions nebula_ros/src/tutorial/decoder_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,114 +73,11 @@ void TutorialDecoderWrapper::OnConfigChange(
sensor_cfg_ = new_config;
}

void TutorialDecoderWrapper::OnCalibrationChange(
const std::shared_ptr<const TutorialCalibrationConfiguration>& new_calibration)
{
std::lock_guard lock(mtx_driver_ptr_);
auto new_driver = std::make_shared<TutorialDriver>(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<rclcpp::Parameter>& 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<SetParametersResult>().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<SetParametersResult>().successful(true).reason("");
}

get_calibration_result_t
TutorialDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path, bool ignore_others)
{
auto calib = std::make_shared<TutorialCalibrationConfiguration>();

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))
Expand Down
43 changes: 31 additions & 12 deletions nebula_ros/src/tutorial/hw_monitor_wrapper.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include "nebula_ros/tutorial/hw_monitor_wrapper.hpp"

#include <boost/iostreams/stream.hpp>

namespace nebula
{
namespace ros
Expand All @@ -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);

Expand All @@ -39,44 +42,60 @@ 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<std::string>("status.voltage"));
diagnostics.add("error", current_diag_info_->get<std::string>("status.error"));
// etc.

diagnostics.summary(level, "OK");
auto error = current_diag_info_->get<std::string>("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();
}

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<boost::iostreams::array_source> 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
55 changes: 2 additions & 53 deletions nebula_ros/src/tutorial/tutorial_ros_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,32 +135,6 @@ nebula::Status TutorialRosWrapper::DeclareAndGetSensorConfigParams()
declare_parameter<double>("dual_return_distance_threshold", descriptor);
}

auto _ptp_profile = declare_parameter<std::string>("ptp_profile", param_read_only());
config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile);

auto _ptp_transport = declare_parameter<std::string>("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<std::string>("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<uint8_t>("ptp_domain", descriptor);
}

auto new_cfg_ptr = std::make_shared<const TutorialSensorConfiguration>(config);
return ValidateAndSetConfig(new_cfg_ptr);
}
Expand All @@ -179,23 +153,6 @@ Status TutorialRosWrapper::ValidateAndSetConfig(std::shared_ptr<const TutorialSe
{
return Status::SENSOR_CONFIG_ERROR;
}
if (new_config->ptp_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_)
{
Expand All @@ -219,7 +176,7 @@ void TutorialRosWrapper::ReceiveScanMessageCallback(std::unique_ptr<nebula_msgs:
if (hw_interface_wrapper_)
{
RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000,
"Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay.");
"Ignoring received packets message. Launch with launch_hw:=false to enable replay.");
return;
}

Expand Down Expand Up @@ -267,15 +224,7 @@ rcl_interfaces::msg::SetParametersResult TutorialRosWrapper::OnParameterChange(c
// Update sub-wrapper parameters (if any)
// ////////////////////////////////////////

// Currently, HW interface and monitor wrappers have only read-only parameters, so their update logic is not
// implemented
if (decoder_wrapper_)
{
auto result = decoder_wrapper_->OnParameterChange(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
Expand Down

0 comments on commit cd26997

Please sign in to comment.