Skip to content

Commit

Permalink
feat(aeva): support runtime parameter changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@mojomex
mojomex committed Jul 5, 2024
1 parent 5caa1d1 commit b2dfa50
Show file tree
Hide file tree
Showing 5 changed files with 119 additions and 53 deletions.
19 changes: 17 additions & 2 deletions ...w_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,12 +119,27 @@ class AevaHwInterface
throw std::runtime_error("Reached maximum retries while trying to fetch manifest");
}

tryReconfig(manifest, "scanner", "frame_sync_type", config->frame_sync_type);
tryReconfig(manifest, "scanner", "sensor_ip", config->sensor_ip);
tryReconfig(
manifest, "scanner", "dithering_enable_ego_speed", config->dithering_enable_ego_speed);
tryReconfig(manifest, "scanner", "dithering_pattern_option", config->dithering_pattern_option);
tryReconfig(manifest, "scanner", "ele_offset_rad", config->ele_offset_rad);
tryReconfig(
manifest, "scanner", "elevation_auto_adjustment", config->elevation_auto_adjustment);
tryReconfig(manifest, "scanner", "enable_frame_dithering", config->enable_frame_dithering);
tryReconfig(manifest, "scanner", "enable_frame_sync", config->enable_frame_sync);
tryReconfig(manifest, "scanner", "flip_pattern_vertically", config->flip_pattern_vertically);
tryReconfig(manifest, "scanner", "frame_sync_offset_in_ms", config->frame_sync_offset_in_ms);
tryReconfig(manifest, "scanner", "frame_sync_type", config->frame_sync_type);
tryReconfig(
manifest, "scanner", "frame_synchronization_on_rising_edge",
config->frame_synchronization_on_rising_edge);
tryReconfig(manifest, "scanner", "enable_frame_sync", config->enable_frame_sync);
tryReconfig(manifest, "scanner", "hfov_adjustment_deg", config->hfov_adjustment_deg);
tryReconfig(manifest, "scanner", "hfov_rotation_deg", config->hfov_rotation_deg);
tryReconfig(manifest, "scanner", "highlight_ROI", config->highlight_ROI);
tryReconfig(manifest, "scanner", "horizontal_fov_degrees", config->horizontal_fov_degrees);
tryReconfig(manifest, "scanner", "roi_az_offset_rad", config->roi_az_offset_rad);
tryReconfig(manifest, "scanner", "vertical_pattern", config->vertical_pattern);
}

void registerCloudPacketCallback(PointcloudParser::callback_t callback)
Expand Down
3 changes: 0 additions & 3 deletions ...nterfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@
#include <exception>
#include <iterator>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>
Expand Down Expand Up @@ -204,9 +203,7 @@ class AevaParser : public ObservableByteStream

private:
std::thread thread_;

std::shared_ptr<PullableByteStream> incoming_;

callback_t bytes_callback_;
};

Expand Down
1 change: 0 additions & 1 deletion ...nterfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,6 @@ class ReconfigParser : public AevaParser<AevaStreamType::kReconfig>,
json setParameter(std::string node_name, std::string key, json value)
{
json request_body = {{node_name, {{key, {{"value", value}}}}}};
std::cerr << "Sending request with body: \n" << request_body.dump(2) << std::endl;
ReconfigMessage request = {ReconfigRequestType::kChangeRequest, request_body};

ReconfigMessage response = doRequest(request);
Expand Down
15 changes: 10 additions & 5 deletions nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,22 +55,27 @@ class AevaRosWrapper final : public rclcpp::Node
Status declareAndGetSensorConfigParams();
Status validateAndSetConfig(std::shared_ptr<const drivers::aeva::Aeries2Config> & new_config);

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

void recordRawPacket(const std::vector<uint8_t> & bytes);

rclcpp::Publisher<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_pub_{};
rclcpp::Publisher<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_pub_;
std::mutex mtx_current_scan_msg_;
nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_{};
nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_;

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_{};
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
std::shared_ptr<nebula::ros::WatchdogTimer> cloud_watchdog_;

rclcpp::Subscription<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_sub_{};
rclcpp::Subscription<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_sub_;

std::shared_ptr<const drivers::aeva::Aeries2Config> sensor_cfg_ptr_;

std::optional<drivers::AevaHwInterface> hw_interface_;
std::optional<AevaHwMonitorWrapper> hw_monitor_;
drivers::AevaAries2Decoder decoder_{};
drivers::AevaAries2Decoder decoder_;

OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_;
};

} // namespace nebula::ros
134 changes: 92 additions & 42 deletions nebula_ros/src/aeva/aeva_ros_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@

#include <cstdint>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
Expand Down Expand Up @@ -131,31 +132,32 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options)
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Missed pointcloud output deadline");
});

{
AevaAries2Decoder::callback_t pointcloud_cb =
[&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) {
auto now = this->now();
cloud_watchdog_->update();

if (
cloud_pub_->get_subscription_count() > 0 ||
cloud_pub_->get_intra_process_subscription_count() > 0) {
auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*cloud_ptr, *ros_pc_msg_ptr);
ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id;
ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast<int64_t>(timestamp));
cloud_pub_->publish(std::move(ros_pc_msg_ptr));
}

std::lock_guard lock(mtx_current_scan_msg_);
if (current_scan_msg_ && packets_pub_) {
packets_pub_->publish(std::move(current_scan_msg_));
current_scan_msg_ = {};
}
};

decoder_.registerPointCloudCallback(std::move(pointcloud_cb));
}
AevaAries2Decoder::callback_t pointcloud_cb =
[&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) {
auto now = this->now();
cloud_watchdog_->update();

if (
cloud_pub_->get_subscription_count() > 0 ||
cloud_pub_->get_intra_process_subscription_count() > 0) {
auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*cloud_ptr, *ros_pc_msg_ptr);
ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id;
ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast<int64_t>(timestamp));
cloud_pub_->publish(std::move(ros_pc_msg_ptr));
}

std::lock_guard lock(mtx_current_scan_msg_);
if (current_scan_msg_ && packets_pub_) {
packets_pub_->publish(std::move(current_scan_msg_));
current_scan_msg_ = {};
}
};

decoder_.registerPointCloudCallback(std::move(pointcloud_cb));

parameter_event_cb_ =
add_on_set_parameters_callback([this](const auto & p) { return onParameterChange(p); });
}

Status AevaRosWrapper::declareAndGetSensorConfigParams()
Expand All @@ -167,29 +169,29 @@ Status AevaRosWrapper::declareAndGetSensorConfigParams()
config.sensor_ip = declare_parameter<std::string>("sensor_ip", param_read_only());
config.frame_id = declare_parameter<std::string>("frame_id", param_read_only());
config.dithering_enable_ego_speed =
declare_parameter<float>("dithering_enable_ego_speed", param_read_only());
declare_parameter<float>("dithering_enable_ego_speed", param_read_write());
config.dithering_pattern_option =
declare_parameter<std::string>("dithering_pattern_option", param_read_only());
config.ele_offset_rad = declare_parameter<float>("ele_offset_rad", param_read_only());
declare_parameter<std::string>("dithering_pattern_option", param_read_write());
config.ele_offset_rad = declare_parameter<float>("ele_offset_rad", param_read_write());
config.elevation_auto_adjustment =
declare_parameter<bool>("elevation_auto_adjustment", param_read_only());
declare_parameter<bool>("elevation_auto_adjustment", param_read_write());
config.enable_frame_dithering =
declare_parameter<bool>("enable_frame_dithering", param_read_only());
config.enable_frame_sync = declare_parameter<bool>("enable_frame_sync", param_read_only());
declare_parameter<bool>("enable_frame_dithering", param_read_write());
config.enable_frame_sync = declare_parameter<bool>("enable_frame_sync", param_read_write());
config.flip_pattern_vertically =
declare_parameter<bool>("flip_pattern_vertically", param_read_only());
declare_parameter<bool>("flip_pattern_vertically", param_read_write());
config.frame_sync_offset_in_ms =
declare_parameter<int>("frame_sync_offset_in_ms", param_read_only());
config.frame_sync_type = declare_parameter<std::string>("frame_sync_type", param_read_only());
declare_parameter<int>("frame_sync_offset_in_ms", param_read_write());
config.frame_sync_type = declare_parameter<std::string>("frame_sync_type", param_read_write());
config.frame_synchronization_on_rising_edge =
declare_parameter<bool>("frame_synchronization_on_rising_edge", param_read_only());
config.hfov_adjustment_deg = declare_parameter<float>("hfov_adjustment_deg", param_read_only());
config.hfov_rotation_deg = declare_parameter<float>("hfov_rotation_deg", param_read_only());
config.highlight_ROI = declare_parameter<bool>("highlight_ROI", param_read_only());
declare_parameter<bool>("frame_synchronization_on_rising_edge", param_read_write());
config.hfov_adjustment_deg = declare_parameter<float>("hfov_adjustment_deg", param_read_write());
config.hfov_rotation_deg = declare_parameter<float>("hfov_rotation_deg", param_read_write());
config.highlight_ROI = declare_parameter<bool>("highlight_ROI", param_read_write());
config.horizontal_fov_degrees =
declare_parameter<std::string>("horizontal_fov_degrees", param_read_only());
config.roi_az_offset_rad = declare_parameter<float>("roi_az_offset_rad", param_read_only());
config.vertical_pattern = declare_parameter<std::string>("vertical_pattern", param_read_only());
declare_parameter<std::string>("horizontal_fov_degrees", param_read_write());
config.roi_az_offset_rad = declare_parameter<float>("roi_az_offset_rad", param_read_write());
config.vertical_pattern = declare_parameter<std::string>("vertical_pattern", param_read_write());

auto new_cfg_ptr = std::make_shared<const Aeries2Config>(config);
return validateAndSetConfig(new_cfg_ptr);
Expand All @@ -210,13 +212,61 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr<const Aeries2Config>
}

if (hw_interface_) {
hw_interface_->onConfigChange(new_config);
try {
hw_interface_->onConfigChange(new_config);
} catch (const std::runtime_error & e) {
RCLCPP_ERROR_STREAM(get_logger(), "Sending configuration to sensor failed: " << e.what());
return Status::SENSOR_CONFIG_ERROR;
}
}

sensor_cfg_ptr_ = new_config;
return Status::OK;
}

rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange(
const std::vector<rclcpp::Parameter> & p)
{
using rcl_interfaces::msg::SetParametersResult;
Aeries2Config config;

bool got_any =
get_param(p, "dithering_enable_ego_speed", config.dithering_enable_ego_speed) |
get_param(p, "dithering_pattern_option", config.dithering_pattern_option) |
get_param(p, "ele_offset_rad", config.ele_offset_rad) |
get_param(p, "elevation_auto_adjustment", config.elevation_auto_adjustment) |
get_param(p, "enable_frame_dithering", config.enable_frame_dithering) |
get_param(p, "enable_frame_sync", config.enable_frame_sync) |
get_param(p, "flip_pattern_vertically", config.flip_pattern_vertically) |
get_param(p, "frame_sync_offset_in_ms", config.frame_sync_offset_in_ms) |
get_param(p, "frame_sync_type", config.frame_sync_type) |
get_param(
p, "frame_synchronization_on_rising_edge", config.frame_synchronization_on_rising_edge) |
get_param(p, "hfov_adjustment_deg", config.hfov_adjustment_deg) |
get_param(p, "hfov_rotation_deg", config.hfov_rotation_deg) |
get_param(p, "highlight_ROI", config.highlight_ROI) |
get_param(p, "horizontal_fov_degrees", config.horizontal_fov_degrees) |
get_param(p, "roi_az_offset_rad", config.roi_az_offset_rad) |
get_param(p, "vertical_pattern", config.vertical_pattern);

if (!got_any) {
return rcl_interfaces::build<SetParametersResult>().successful(true).reason("");
}

auto new_cfg_ptr = std::make_shared<const Aeries2Config>(config);
auto status = validateAndSetConfig(new_cfg_ptr);

if (status != Status::OK) {
RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status);
auto result = SetParametersResult();
result.successful = false;
result.reason = (std::stringstream() << "Invalid configuration: " << status).str();
return result;
}

return rcl_interfaces::build<SetParametersResult>().successful(true).reason("");
}

void AevaRosWrapper::recordRawPacket(const std::vector<uint8_t> & vector)
{
std::lock_guard lock(mtx_current_scan_msg_);
Expand Down

0 comments on commit b2dfa50

Please sign in to comment.