From 43ee7cbb8b1a4d1a809cc07033b389a214d576b3 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 30 May 2024 13:47:40 -0700 Subject: [PATCH] Format --- .../actions/status_light_publisher.hpp | 86 +++--- .../src/actions/status_light_publisher.cpp | 29 +- .../status_light_controller.hpp | 69 ++--- .../src/status_light_controller.cpp | 197 +++++++------- .../hardware_interfaces/status_light.hpp | 79 +++--- .../src/hardware_interfaces/status_light.cpp | 256 +++++++++--------- 6 files changed, 351 insertions(+), 365 deletions(-) diff --git a/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp b/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp index b82e9fd8..61a6d5da 100644 --- a/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp +++ b/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp @@ -14,59 +14,53 @@ namespace behavior::actions { - int stringToColor(std::string color) - { - if (color == "red") - { - return urc_msgs::msg::StatusLightCommand::RED; - } - if (color == "green") - { - return urc_msgs::msg::StatusLightCommand::GREEN; - } - if (color == "blue") - { - return urc_msgs::msg::StatusLightCommand::BLUE; - } - - throw std::invalid_argument("Invalid color!"); +int stringToColor(std::string color) +{ + if (color == "red") { + return urc_msgs::msg::StatusLightCommand::RED; + } + if (color == "green") { + return urc_msgs::msg::StatusLightCommand::GREEN; + } + if (color == "blue") { + return urc_msgs::msg::StatusLightCommand::BLUE; } - int stringToState(std::string state) - { - if (state == "on") - { - return urc_msgs::msg::StatusLightCommand::ON; - } - if (state == "off") - { - return urc_msgs::msg::StatusLightCommand::OFF; - } - if (state == "blink") - { - return urc_msgs::msg::StatusLightCommand::BLINK; - } + throw std::invalid_argument("Invalid color!"); +} - throw std::invalid_argument("Invalid state!"); +int stringToState(std::string state) +{ + if (state == "on") { + return urc_msgs::msg::StatusLightCommand::ON; + } + if (state == "off") { + return urc_msgs::msg::StatusLightCommand::OFF; + } + if (state == "blink") { + return urc_msgs::msg::StatusLightCommand::BLINK; } - class StatusLightPublisher : public BT::RosTopicPubNode - { - public: - StatusLightPublisher( - const std::string &instance_name, const BT::NodeConfig &conf, - const BT::RosNodeParams ¶ms) - : BT::RosTopicPubNode(instance_name, conf, params) {} + throw std::invalid_argument("Invalid state!"); +} - static BT::PortsList providedPorts() - { - return providedBasicPorts( - {BT::InputPort("color"), - BT::InputPort("state")}); - } +class StatusLightPublisher : public BT::RosTopicPubNode +{ +public: + StatusLightPublisher( + const std::string & instance_name, const BT::NodeConfig & conf, + const BT::RosNodeParams & params) + : BT::RosTopicPubNode(instance_name, conf, params) {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts( + {BT::InputPort("color"), + BT::InputPort("state")}); + } - bool setMessage(urc_msgs::msg::StatusLightCommand &msg) override; - }; + bool setMessage(urc_msgs::msg::StatusLightCommand & msg) override; +}; } #endif diff --git a/urc_bt_nodes/src/actions/status_light_publisher.cpp b/urc_bt_nodes/src/actions/status_light_publisher.cpp index e3f9ee3a..f4c7a0d2 100644 --- a/urc_bt_nodes/src/actions/status_light_publisher.cpp +++ b/urc_bt_nodes/src/actions/status_light_publisher.cpp @@ -4,24 +4,21 @@ namespace behavior::actions { - bool StatusLightPublisher::setMessage(urc_msgs::msg::StatusLightCommand &msg) - { - std::string color = getInput("color").value(); - std::string state = getInput("state").value(); - - try - { - msg.color = stringToColor(color); - msg.state = stringToState(state); - } - catch (std::invalid_argument &e) - { - RCLCPP_ERROR(node_->get_logger(), "Invalid argument: %s", e.what()); - return false; - } +bool StatusLightPublisher::setMessage(urc_msgs::msg::StatusLightCommand & msg) +{ + std::string color = getInput("color").value(); + std::string state = getInput("state").value(); - return true; + try { + msg.color = stringToColor(color); + msg.state = stringToState(state); + } catch (std::invalid_argument & e) { + RCLCPP_ERROR(node_->get_logger(), "Invalid argument: %s", e.what()); + return false; } + + return true; +} } #include "behaviortree_ros2/plugins.hpp" diff --git a/urc_controllers/include/urc_controllers/status_light_controller.hpp b/urc_controllers/include/urc_controllers/status_light_controller.hpp index 3ed8a815..6171af05 100644 --- a/urc_controllers/include/urc_controllers/status_light_controller.hpp +++ b/urc_controllers/include/urc_controllers/status_light_controller.hpp @@ -28,51 +28,52 @@ namespace urc_controllers { - class StatusLightController : public controller_interface::ControllerInterface - { - public: - StatusLightController(); +class StatusLightController : public controller_interface::ControllerInterface +{ +public: + StatusLightController(); - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update( - const rclcpp::Time &time, - const rclcpp::Duration &period) override; + controller_interface::return_type update( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; - controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) - override; + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) + override; - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) - override; + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) + override; - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) - override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) + override; - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) - override; + controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) + override; - controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) - override; + controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) + override; - controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state) - override; + controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) + override; - protected: - // status_light related - std::string status_light_name; - std::shared_ptr> status_light_command_subscriber_; - realtime_tools::RealtimeBuffer color_command_; - realtime_tools::RealtimeBuffer state_command_; +protected: + // status_light related + std::string status_light_name; + std::shared_ptr> + status_light_command_subscriber_; + realtime_tools::RealtimeBuffer color_command_; + realtime_tools::RealtimeBuffer state_command_; - // command interfaces - std::unordered_map>> - command_interface_map; - const std::vector STATUS_LIGHT_INTERFACES{"color", "state"}; - }; + // command interfaces + std::unordered_map>> + command_interface_map; + const std::vector STATUS_LIGHT_INTERFACES{"color", "state"}; +}; } // namespace urc_controllers diff --git a/urc_controllers/src/status_light_controller.cpp b/urc_controllers/src/status_light_controller.cpp index b364e5ff..7602f6d5 100644 --- a/urc_controllers/src/status_light_controller.cpp +++ b/urc_controllers/src/status_light_controller.cpp @@ -13,125 +13,122 @@ #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - urc_controllers::StatusLightController, - controller_interface::ControllerInterface); + urc_controllers::StatusLightController, + controller_interface::ControllerInterface); namespace urc_controllers { - StatusLightController::StatusLightController() - : status_light_command_subscriber_(nullptr), color_command_(0), state_command_(0) {} +StatusLightController::StatusLightController() +: status_light_command_subscriber_(nullptr), color_command_(0), state_command_(0) {} - controller_interface::CallbackReturn StatusLightController::on_init() - { - return controller_interface::CallbackReturn::SUCCESS; - } +controller_interface::CallbackReturn StatusLightController::on_init() +{ + return controller_interface::CallbackReturn::SUCCESS; +} - controller_interface::CallbackReturn StatusLightController::on_configure( - const rclcpp_lifecycle::State &) - { - status_light_name = get_node()->get_parameter("status_light_name").as_string(); - if (status_light_name.empty()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Need to have 'status_light_name' parameter in the configuration file. Fail to find one."); - return controller_interface::CallbackReturn::FAILURE; - } - return controller_interface::CallbackReturn::SUCCESS; +controller_interface::CallbackReturn StatusLightController::on_configure( + const rclcpp_lifecycle::State &) +{ + status_light_name = get_node()->get_parameter("status_light_name").as_string(); + if (status_light_name.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Need to have 'status_light_name' parameter in the configuration file. Fail to find one."); + return controller_interface::CallbackReturn::FAILURE; } + return controller_interface::CallbackReturn::SUCCESS; +} - controller_interface::InterfaceConfiguration StatusLightController::command_interface_configuration() - const - { - controller_interface::InterfaceConfiguration command_interfaces_configuration; - command_interfaces_configuration.type = - controller_interface::interface_configuration_type::INDIVIDUAL; - - command_interfaces_configuration.names.push_back(status_light_name + "/" + "color"); - command_interfaces_configuration.names.push_back(status_light_name + "/" + "state"); - return command_interfaces_configuration; - } +controller_interface::InterfaceConfiguration StatusLightController::command_interface_configuration() +const +{ + controller_interface::InterfaceConfiguration command_interfaces_configuration; + command_interfaces_configuration.type = + controller_interface::interface_configuration_type::INDIVIDUAL; - controller_interface::InterfaceConfiguration StatusLightController::state_interface_configuration() - const - { - return controller_interface::InterfaceConfiguration(); - } + command_interfaces_configuration.names.push_back(status_light_name + "/" + "color"); + command_interfaces_configuration.names.push_back(status_light_name + "/" + "state"); + return command_interfaces_configuration; +} - controller_interface::CallbackReturn StatusLightController::on_activate( - const rclcpp_lifecycle::State &) - { - for (auto &interface : command_interfaces_) - { - const auto interface_ptr = - std::find( - STATUS_LIGHT_INTERFACES.begin(), - STATUS_LIGHT_INTERFACES.end(), interface.get_interface_name()); - if (interface_ptr == STATUS_LIGHT_INTERFACES.end()) - { - continue; - } - - command_interface_map.emplace( - interface.get_interface_name(), - std::make_shared>( - interface)); - } +controller_interface::InterfaceConfiguration StatusLightController::state_interface_configuration() +const +{ + return controller_interface::InterfaceConfiguration(); +} - if (command_interface_map.size() != 2) - { - RCLCPP_ERROR(get_node()->get_logger(), "Not all interfaces are initialized!"); - return controller_interface::CallbackReturn::ERROR; +controller_interface::CallbackReturn StatusLightController::on_activate( + const rclcpp_lifecycle::State &) +{ + for (auto & interface : command_interfaces_) { + const auto interface_ptr = + std::find( + STATUS_LIGHT_INTERFACES.begin(), + STATUS_LIGHT_INTERFACES.end(), interface.get_interface_name()); + if (interface_ptr == STATUS_LIGHT_INTERFACES.end()) { + continue; } - status_light_command_subscriber_ = get_node()->create_subscription( - "/command/status_light", rclcpp::SystemDefaultsQoS(), - [this](std::shared_ptr msg) - { - color_command_.writeFromNonRT(static_cast(msg->color)); - state_command_.writeFromNonRT(static_cast(msg->state)); - }); - - RCLCPP_INFO(get_node()->get_logger(), "StatusLightController activated!"); - return controller_interface::CallbackReturn::SUCCESS; + command_interface_map.emplace( + interface.get_interface_name(), + std::make_shared>( + interface)); } - controller_interface::CallbackReturn StatusLightController::on_deactivate( - const rclcpp_lifecycle::State &) - { - status_light_command_subscriber_.reset(); - return controller_interface::CallbackReturn::SUCCESS; + if (command_interface_map.size() != 2) { + RCLCPP_ERROR(get_node()->get_logger(), "Not all interfaces are initialized!"); + return controller_interface::CallbackReturn::ERROR; } - controller_interface::CallbackReturn StatusLightController::on_cleanup( - const rclcpp_lifecycle::State &) - { - status_light_command_subscriber_.reset(); - return controller_interface::CallbackReturn::SUCCESS; - } + status_light_command_subscriber_ = + get_node()->create_subscription( + "/command/status_light", rclcpp::SystemDefaultsQoS(), + [this](std::shared_ptr msg) + { + color_command_.writeFromNonRT(static_cast(msg->color)); + state_command_.writeFromNonRT(static_cast(msg->state)); + }); - controller_interface::CallbackReturn StatusLightController::on_error( - const rclcpp_lifecycle::State &) - { - status_light_command_subscriber_.reset(); - return controller_interface::CallbackReturn::SUCCESS; - } + RCLCPP_INFO(get_node()->get_logger(), "StatusLightController activated!"); + return controller_interface::CallbackReturn::SUCCESS; +} - controller_interface::CallbackReturn StatusLightController::on_shutdown( - const rclcpp_lifecycle::State &) - { - status_light_command_subscriber_.reset(); - return controller_interface::CallbackReturn::SUCCESS; - } +controller_interface::CallbackReturn StatusLightController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + status_light_command_subscriber_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} - controller_interface::return_type StatusLightController::update( - const rclcpp::Time &, - const rclcpp::Duration &) - { - command_interface_map["color"]->get().set_value(*color_command_.readFromRT()); - command_interface_map["state"]->get().set_value(*state_command_.readFromRT()); - return controller_interface::return_type::OK; - } +controller_interface::CallbackReturn StatusLightController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + status_light_command_subscriber_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn StatusLightController::on_error( + const rclcpp_lifecycle::State &) +{ + status_light_command_subscriber_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn StatusLightController::on_shutdown( + const rclcpp_lifecycle::State &) +{ + status_light_command_subscriber_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type StatusLightController::update( + const rclcpp::Time &, + const rclcpp::Duration &) +{ + command_interface_map["color"]->get().set_value(*color_command_.readFromRT()); + command_interface_map["state"]->get().set_value(*state_command_.readFromRT()); + return controller_interface::return_type::OK; +} } // namespace urc_controllers diff --git a/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp b/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp index 62ec2f3a..09afdc05 100644 --- a/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp +++ b/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp @@ -18,10 +18,9 @@ namespace urc_hardware::hardware_interfaces { - std::string num_to_state(uint8_t num) - { - switch (num) - { +std::string num_to_state(uint8_t num) +{ + switch (num) { case 0: return "off"; case 1: @@ -30,49 +29,49 @@ namespace urc_hardware::hardware_interfaces return "blinking"; default: return "unknown"; - } } +} - class StatusLight : public hardware_interface::SystemInterface - { - public: - StatusLight(); - ~StatusLight(); +class StatusLight : public hardware_interface::SystemInterface +{ +public: + StatusLight(); + ~StatusLight(); - hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &hardware_info) - override; - std::vector export_command_interfaces() override; - std::vector export_state_interfaces() override; - hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) - override; - hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) - override; - hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) - override; - hardware_interface::return_type read( - const rclcpp::Time &time, - const rclcpp::Duration &period) override; - hardware_interface::return_type write( - const rclcpp::Time &time, - const rclcpp::Duration &period) override; + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) + override; + std::vector export_command_interfaces() override; + std::vector export_state_interfaces() override; + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) + override; + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) + override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) + override; + hardware_interface::return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + hardware_interface::return_type write( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; - private: - // basic info - const std::string hardware_interface_name; - // states - std::vector signals; // [0]: color selection, [1]: state (on, off, blinking) +private: + // basic info + const std::string hardware_interface_name; + // states + std::vector signals; // [0]: color selection, [1]: state (on, off, blinking) - // hardware resources - std::shared_ptr> udp_; - std::string udp_address; - std::string udp_port; + // hardware resources + std::shared_ptr> udp_; + std::string udp_address; + std::string udp_port; - // nanopb - uint8_t buffer[TeensyMessage_size]; - size_t message_length; + // nanopb + uint8_t buffer[TeensyMessage_size]; + size_t message_length; - uint8_t lightStates[3]; // current state of each light (red, green, blue). - }; + uint8_t lightStates[3]; // current state of each light (red, green, blue). +}; } // namespace urc_hardware::hardware_interfaces diff --git a/urc_hw/src/hardware_interfaces/status_light.cpp b/urc_hw/src/hardware_interfaces/status_light.cpp index e808a297..50667202 100644 --- a/urc_hw/src/hardware_interfaces/status_light.cpp +++ b/urc_hw/src/hardware_interfaces/status_light.cpp @@ -14,159 +14,157 @@ #include PLUGINLIB_EXPORT_CLASS( - urc_hardware::hardware_interfaces::StatusLight, - hardware_interface::SystemInterface); + urc_hardware::hardware_interfaces::StatusLight, + hardware_interface::SystemInterface); namespace urc_hardware::hardware_interfaces { - StatusLight::StatusLight() - : hardware_interface_name("Status Light"), signals(2, 0) {} - StatusLight::~StatusLight() = default; +StatusLight::StatusLight() +: hardware_interface_name("Status Light"), signals(2, 0) {} +StatusLight::~StatusLight() = default; - hardware_interface::CallbackReturn StatusLight::on_init( - const hardware_interface::HardwareInfo &info) +hardware_interface::CallbackReturn StatusLight::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - if (hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - if (info_.hardware_parameters.find("udp_address") == info_.hardware_parameters.end()) - { - RCLCPP_ERROR( - rclcpp::get_logger(hardware_interface_name), - "Error during initialization: 'udp_address' configuration not " - "found. Expect to enter the udp server address."); - return hardware_interface::CallbackReturn::ERROR; - } - if (info_.hardware_parameters.find("udp_port") == info_.hardware_parameters.end()) - { - RCLCPP_ERROR( - rclcpp::get_logger(hardware_interface_name), - "Error during initialization: 'udp_port' configuration not " - "found. Expect to enter the port number."); - return hardware_interface::CallbackReturn::ERROR; - } + return hardware_interface::CallbackReturn::ERROR; + } - udp_address = info_.hardware_parameters["udp_address"]; - udp_port = info_.hardware_parameters["udp_port"]; + if (info_.hardware_parameters.find("udp_address") == info_.hardware_parameters.end()) { + RCLCPP_ERROR( + rclcpp::get_logger(hardware_interface_name), + "Error during initialization: 'udp_address' configuration not " + "found. Expect to enter the udp server address."); + return hardware_interface::CallbackReturn::ERROR; + } + if (info_.hardware_parameters.find("udp_port") == info_.hardware_parameters.end()) { + RCLCPP_ERROR( + rclcpp::get_logger(hardware_interface_name), + "Error during initialization: 'udp_port' configuration not " + "found. Expect to enter the port number."); + return hardware_interface::CallbackReturn::ERROR; + } - if (info.gpios.size() == 0) - { - RCLCPP_ERROR( - rclcpp::get_logger( - hardware_interface_name), - "Should have at least one gpio to be the Status Light."); - return CallbackReturn::ERROR; - } + udp_address = info_.hardware_parameters["udp_address"]; + udp_port = info_.hardware_parameters["udp_port"]; - bool find_light = false; - for (hardware_interface::ComponentInfo component : info_.gpios) - { - if (component.name == "status_light") - { - find_light = true; - break; - } - } + if (info.gpios.size() == 0) { + RCLCPP_ERROR( + rclcpp::get_logger( + hardware_interface_name), + "Should have at least one gpio to be the Status Light."); + return CallbackReturn::ERROR; + } - if (!find_light) - { - RCLCPP_ERROR( - rclcpp::get_logger( - hardware_interface_name), - "Not able to find sensor named 'status_light'."); - return CallbackReturn::ERROR; + bool find_light = false; + for (hardware_interface::ComponentInfo component : info_.gpios) { + if (component.name == "status_light") { + find_light = true; + break; } - - RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light HW interface initialized!"); - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn StatusLight::on_configure(const rclcpp_lifecycle::State &) - { - std::fill(signals.begin(), signals.end(), 0.0); - return hardware_interface::CallbackReturn::SUCCESS; + if (!find_light) { + RCLCPP_ERROR( + rclcpp::get_logger( + hardware_interface_name), + "Not able to find sensor named 'status_light'."); + return CallbackReturn::ERROR; } - std::vector StatusLight::export_command_interfaces() - { - std::vector command_interfaces; - command_interfaces.emplace_back("status_light", "color", &this->signals[0]); - command_interfaces.emplace_back("status_light", "state", &this->signals[1]); - return command_interfaces; - } + RCLCPP_INFO( + rclcpp::get_logger(hardware_interface_name), + "Status light HW interface initialized!"); + return hardware_interface::CallbackReturn::SUCCESS; +} - std::vector StatusLight::export_state_interfaces() - { - std::vector state_interfaces; - return state_interfaces; - } +hardware_interface::CallbackReturn StatusLight::on_configure(const rclcpp_lifecycle::State &) +{ + std::fill(signals.begin(), signals.end(), 0.0); + return hardware_interface::CallbackReturn::SUCCESS; +} - hardware_interface::CallbackReturn StatusLight::on_activate(const rclcpp_lifecycle::State &) - { - udp_ = std::make_shared>(true); - udp_->Connect(udp_address, std::stoi(udp_port)); - RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light activated!"); - return hardware_interface::CallbackReturn::SUCCESS; - } +std::vector StatusLight::export_command_interfaces() +{ + std::vector command_interfaces; + command_interfaces.emplace_back("status_light", "color", &this->signals[0]); + command_interfaces.emplace_back("status_light", "state", &this->signals[1]); + return command_interfaces; +} - hardware_interface::CallbackReturn StatusLight::on_deactivate(const rclcpp_lifecycle::State &) - { - udp_->Close(); - RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light deactivated!"); - return hardware_interface::CallbackReturn::SUCCESS; - } +std::vector StatusLight::export_state_interfaces() +{ + std::vector state_interfaces; + return state_interfaces; +} - hardware_interface::return_type StatusLight::read(const rclcpp::Time &, const rclcpp::Duration &) - { - RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Red: %s, Green: %s, Blue: %s", num_to_state(lightStates[0]).c_str(), num_to_state(lightStates[1]).c_str(), num_to_state(lightStates[2]).c_str()); +hardware_interface::CallbackReturn StatusLight::on_activate(const rclcpp_lifecycle::State &) +{ + udp_ = std::make_shared>(true); + udp_->Connect(udp_address, std::stoi(udp_port)); + RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light activated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} - return hardware_interface::return_type::OK; - } +hardware_interface::CallbackReturn StatusLight::on_deactivate(const rclcpp_lifecycle::State &) +{ + udp_->Close(); + RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} - hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const rclcpp::Duration &) - { +hardware_interface::return_type StatusLight::read(const rclcpp::Time &, const rclcpp::Duration &) +{ + RCLCPP_INFO( + rclcpp::get_logger( + hardware_interface_name), "Red: %s, Green: %s, Blue: %s", num_to_state( + lightStates[0]).c_str(), num_to_state(lightStates[1]).c_str(), num_to_state( + lightStates[2]).c_str()); - TeensyMessage message = TeensyMessage_init_zero; - pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); + return hardware_interface::return_type::OK; +} - uint8_t selected_color = static_cast(signals[0]); - uint8_t selected_state = static_cast(signals[1]); - if (selected_color >= 0 && selected_color <= 2 && selected_state >= 0 && selected_state <= 2) - { - lightStates[selected_color] = selected_state; - } +hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const rclcpp::Duration &) +{ - message.redEnabled = (lightStates[0] != 0); - message.redBlink = (lightStates[0] == 2); - message.greenEnabled = (lightStates[1] != 0); - message.greenBlink = (lightStates[1] == 2); - message.blueEnabled = (lightStates[2] != 0); - message.blueBlink = (lightStates[2] == 2); - - // Fill Required Fields - message.m1Setpoint = 0; - message.m2Setpoint = 0; - message.m3Setpoint = 0; - message.m4Setpoint = 0; - message.m5Setpoint = 0; - message.m6Setpoint = 0; - - // Set message id to status light - message.messageID = 1; - - bool status = pb_encode(&stream, TeensyMessage_fields, &message); - message_length = stream.bytes_written; - - if (!status) - { - return hardware_interface::return_type::ERROR; - } - udp_->Send((char *)buffer, sizeof(buffer)); - return hardware_interface::return_type::OK; + TeensyMessage message = TeensyMessage_init_zero; + pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); + + uint8_t selected_color = static_cast(signals[0]); + uint8_t selected_state = static_cast(signals[1]); + if (selected_color >= 0 && selected_color <= 2 && selected_state >= 0 && selected_state <= 2) { + lightStates[selected_color] = selected_state; + } + + message.redEnabled = (lightStates[0] != 0); + message.redBlink = (lightStates[0] == 2); + message.greenEnabled = (lightStates[1] != 0); + message.greenBlink = (lightStates[1] == 2); + message.blueEnabled = (lightStates[2] != 0); + message.blueBlink = (lightStates[2] == 2); + + // Fill Required Fields + message.m1Setpoint = 0; + message.m2Setpoint = 0; + message.m3Setpoint = 0; + message.m4Setpoint = 0; + message.m5Setpoint = 0; + message.m6Setpoint = 0; + + // Set message id to status light + message.messageID = 1; + + bool status = pb_encode(&stream, TeensyMessage_fields, &message); + message_length = stream.bytes_written; + + if (!status) { + return hardware_interface::return_type::ERROR; } + udp_->Send((char *)buffer, sizeof(buffer)); + return hardware_interface::return_type::OK; +} } // namespace urc_hardware::hardware_interfaces