diff --git a/.github/workflows/build_and_test_humble.yaml b/.github/workflows/build_and_test_humble.yaml index 18ba436..ab50589 100644 --- a/.github/workflows/build_and_test_humble.yaml +++ b/.github/workflows/build_and_test_humble.yaml @@ -1,6 +1,6 @@ # This is a basic workflow to help you get started with Actions -name: Build (humble) +name: Build and test (humble) # Controls when the action will run. on: @@ -28,16 +28,10 @@ jobs: steps: - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 - #with: - # use-ros2-testing: true + with: + use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: - skip-tests: true + #skip-tests: true target-ros2-distro: humble - colcon-defaults: | - { - "build": { - "symlink-install": false - } - } vcs-repo-file-url: dependencies.repos \ No newline at end of file diff --git a/.github/workflows/build_and_test_iron.yaml b/.github/workflows/build_and_test_iron.yaml index 3df1196..25a8b3c 100644 --- a/.github/workflows/build_and_test_iron.yaml +++ b/.github/workflows/build_and_test_iron.yaml @@ -1,6 +1,6 @@ # This is a basic workflow to help you get started with Actions -name: Build (iron) +name: Build and test (iron) # Controls when the action will run. on: @@ -28,16 +28,10 @@ jobs: steps: - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 - #with: - # use-ros2-testing: true + with: + use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: - skip-tests: true + #skip-tests: true target-ros2-distro: iron - colcon-defaults: | - { - "build": { - "symlink-install": false - } - } vcs-repo-file-url: dependencies.repos \ No newline at end of file diff --git a/.github/workflows/build_and_test_rolling.yaml b/.github/workflows/build_and_test_rolling.yaml index 11e45c0..7df8981 100644 --- a/.github/workflows/build_and_test_rolling.yaml +++ b/.github/workflows/build_and_test_rolling.yaml @@ -1,6 +1,6 @@ # This is a basic workflow to help you get started with Actions -name: Build (rolling) +name: Build and test (rolling) # Controls when the action will run. on: @@ -32,6 +32,6 @@ jobs: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@master with: - skip-tests: true + #skip-tests: true target-ros2-distro: rolling vcs-repo-file-url: dependencies.repos \ No newline at end of file diff --git a/README.md b/README.md index 7c8cce6..0ba0334 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -[![Build (humble)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_humble.yaml/badge.svg)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_humble.yaml) -[![Build (iron)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_iron.yaml/badge.svg)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_iron.yaml) -[![Build (rolling)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_rolling.yaml/badge.svg)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_rolling.yaml) +[![Build and test (humble)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_humble.yaml/badge.svg)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_humble.yaml) +[![Build and test (iron)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_iron.yaml/badge.svg)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_iron.yaml) +[![Build and test (rolling)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_rolling.yaml/badge.svg)](https://github.com/antbono/nao_led/actions/workflows/build_and_test_rolling.yaml) # nao_led diff --git a/nao_led_server/include/nao_led_server/led_action_client.hpp b/nao_led_server/include/nao_led_server/led_action_client.hpp index e661f25..e6b2d30 100644 --- a/nao_led_server/include/nao_led_server/led_action_client.hpp +++ b/nao_led_server/include/nao_led_server/led_action_client.hpp @@ -44,42 +44,41 @@ #include "std_msgs/msg/color_rgba.hpp" -namespace nao_led_action_client { - -class LedsPlayActionClient : public rclcpp::Node { - public: - explicit LedsPlayActionClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~LedsPlayActionClient(); - - void eyesStatic( bool flag ); - void headStatic( bool flag ); - void earsStatic( bool flag ); - void chestStatic( bool flag ); - void earsLoop( bool flag ); - void headLoop( bool flag ); - void eyesLoop( bool flag ); - - private: - - void goalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle); - void feedbackCallback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback); - void resultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result); - - - rclcpp_action::Client::SharedPtr client_ptr_; - - rclcpp_action::ClientGoalHandle::SharedPtr head_goal_handle_; - //std::shared_future head_goal_handle_; - rclcpp_action::ClientGoalHandle::SharedPtr eyes_goal_handle_; - rclcpp_action::ClientGoalHandle::SharedPtr ears_goal_handle_; - - -}; // LedsPlayActionClient - -} // nao_led_action_client - -#endif // NAO_LED_SERVER__LED_ACTION_CLIENT_HPP_ \ No newline at end of file +namespace nao_led_action_client +{ + +class LedsPlayActionClient : public rclcpp::Node +{ +public: + explicit LedsPlayActionClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~LedsPlayActionClient(); + + void eyesStatic(bool flag); + void headStatic(bool flag); + void earsStatic(bool flag); + void chestStatic(bool flag); + void earsLoop(bool flag); + void headLoop(bool flag); + void eyesLoop(bool flag); + +private: + void goalResponseCallback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void feedbackCallback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); + void + resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult& result); + + rclcpp_action::Client::SharedPtr client_ptr_; + + rclcpp_action::ClientGoalHandle::SharedPtr head_goal_handle_; + // std::shared_future + // head_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr eyes_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr ears_goal_handle_; + +}; // LedsPlayActionClient + +} // namespace nao_led_action_client + +#endif // NAO_LED_SERVER__LED_ACTION_CLIENT_HPP_ \ No newline at end of file diff --git a/nao_led_server/include/nao_led_server/led_action_server.hpp b/nao_led_server/include/nao_led_server/led_action_server.hpp index 89f2087..7a9bfce 100644 --- a/nao_led_server/include/nao_led_server/led_action_server.hpp +++ b/nao_led_server/include/nao_led_server/led_action_server.hpp @@ -21,9 +21,9 @@ //#include #include //#include -#include // std::string, std::stof +#include // std::string, std::stof #include -#include // std::cout +#include // std::cout #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -38,15 +38,13 @@ #include "nao_lola_command_msgs/msg/right_foot_led.hpp" #include "nao_lola_command_msgs/msg/left_foot_led.hpp" - #include "nao_led_interfaces/msg/led_indexes.hpp" #include "nao_led_interfaces/msg/led_modes.hpp" #include "nao_led_interfaces/action/leds_play.hpp" #include "std_msgs/msg/color_rgba.hpp" - -namespace nao_led_action_server +namespace nao_led_action_server { using LedsPlay = nao_led_interfaces::action::LedsPlay; @@ -54,64 +52,56 @@ using GoalHandleLedsPlay = rclcpp_action::ServerGoalHandle; using LedIndexes = nao_led_interfaces::msg::LedIndexes; using LedModes = nao_led_interfaces::msg::LedModes; - -class LedsPlayActionServer : public rclcpp::Node +class LedsPlayActionServer : public rclcpp::Node { - public: - explicit LedsPlayActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~LedsPlayActionServer(); - - private: - - rclcpp_action::Server::SharedPtr action_server_; - std::shared_ptr> goal_handle_; - - - rclcpp::Publisher::SharedPtr head_pub_; - rclcpp::Publisher::SharedPtr right_eye_pub_; - rclcpp::Publisher::SharedPtr left_eye_pub_; - rclcpp::Publisher::SharedPtr right_ear_pub_; - rclcpp::Publisher::SharedPtr left_ear_pub_; - rclcpp::Publisher::SharedPtr chest_pub_; - rclcpp::Publisher::SharedPtr right_foot_pub_; - rclcpp::Publisher::SharedPtr left_foot_pub_; - - std_msgs::msg::ColorRGBA color_off_; - //color_off_.r=0.0; color_off_.g=0.0; color_off_.b=0.0; - nao_led_interfaces::msg::LedIndexes led_indexes_msg_; - nao_led_interfaces::msg::LedModes led_modes_msg_; - - - rclcpp_action::GoalResponse handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - - rclcpp_action::CancelResponse handleCancel( - const std::shared_ptr> goal_handle); - - void handleAccepted( - const std::shared_ptr> goal_handle); - - void execute( - const std::shared_ptr> goal_handle); - - void steadyMode( - const std::shared_ptr> goal_handle, - std::array & leds, - float frequency, - std::array & colors, std::array & intensities); - - bool blinkingMode( - const std::shared_ptr> goal_handle, - std::array & leds, float frequency, - std::array & colors); - - bool loopMode( - const std::shared_ptr> goal_handle, - std::array & leds, float frequency, - std::array & colors); +public: + explicit LedsPlayActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~LedsPlayActionServer(); + +private: + rclcpp_action::Server::SharedPtr action_server_; + std::shared_ptr> goal_handle_; + + rclcpp::Publisher::SharedPtr head_pub_; + rclcpp::Publisher::SharedPtr right_eye_pub_; + rclcpp::Publisher::SharedPtr left_eye_pub_; + rclcpp::Publisher::SharedPtr right_ear_pub_; + rclcpp::Publisher::SharedPtr left_ear_pub_; + rclcpp::Publisher::SharedPtr chest_pub_; + rclcpp::Publisher::SharedPtr right_foot_pub_; + rclcpp::Publisher::SharedPtr left_foot_pub_; + + std_msgs::msg::ColorRGBA color_off_; + // color_off_.r=0.0; color_off_.g=0.0; color_off_.b=0.0; + nao_led_interfaces::msg::LedIndexes led_indexes_msg_; + nao_led_interfaces::msg::LedModes led_modes_msg_; + + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handleCancel( + const std::shared_ptr> goal_handle); + + void handleAccepted( + const std::shared_ptr> goal_handle); + + void + execute(const std::shared_ptr> goal_handle); + + void + steadyMode(const std::shared_ptr> goal_handle, + std::array& leds, float frequency, std::array& colors, + std::array& intensities); + + bool + blinkingMode(const std::shared_ptr> goal_handle, + std::array& leds, float frequency, std::array& colors); + + bool + loopMode(const std::shared_ptr> goal_handle, + std::array& leds, float frequency, std::array& colors); }; -} +} // namespace nao_led_action_server #endif // NAO_LED_SERVER__LED_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/nao_led_server/src/led_action_client.cpp b/nao_led_server/src/led_action_client.cpp index 54cf3b1..03bac2e 100644 --- a/nao_led_server/src/led_action_client.cpp +++ b/nao_led_server/src/led_action_client.cpp @@ -42,8 +42,8 @@ #include "std_msgs/msg/color_rgba.hpp" - -namespace nao_led_action_client { +namespace nao_led_action_client +{ using LedsPlay = nao_led_interfaces::action::LedsPlay; using GoalHandleLedsPlay = rclcpp_action::ClientGoalHandle; @@ -51,267 +51,292 @@ using GoalHandleLedsPlay = rclcpp_action::ClientGoalHandle; using LedIndexes = nao_led_interfaces::msg::LedIndexes; using LedModes = nao_led_interfaces::msg::LedModes; - -LedsPlayActionClient::LedsPlayActionClient(const rclcpp::NodeOptions & options) - : rclcpp::Node("led_action_client_node", options) { - - this->client_ptr_ = rclcpp_action::create_client( - this, - "leds_play"); +LedsPlayActionClient::LedsPlayActionClient(const rclcpp::NodeOptions& options) + : rclcpp::Node("led_action_client_node", options) +{ + this->client_ptr_ = rclcpp_action::create_client(this, "leds_play"); RCLCPP_INFO(this->get_logger(), "LedsPlayActionClient initialized"); - } -LedsPlayActionClient::~LedsPlayActionClient() {} - - -void LedsPlayActionClient::eyesStatic( bool flag ) { +LedsPlayActionClient::~LedsPlayActionClient() +{ +} +void LedsPlayActionClient::eyesStatic(bool flag) +{ using namespace std::placeholders; - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::REYE, LedIndexes::LEYE}; + goal_msg.leds = { LedIndexes::REYE, LedIndexes::LEYE }; goal_msg.mode = LedModes::STEADY; std_msgs::msg::ColorRGBA color; - if (flag) { - color.r = 1.0; color.g = 1.0; color.b = 1.0; - } else { - color.r = 0.0; color.g = 0.0; color.b = 0.0; + if (flag) + { + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; + } + else + { + color.r = 0.0; + color.g = 0.0; + color.b = 0.0; } - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { goal_msg.colors[i] = color; } auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } -void LedsPlayActionClient::chestStatic( bool flag ) { - +void LedsPlayActionClient::chestStatic(bool flag) +{ using namespace std::placeholders; - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::CHEST}; + goal_msg.leds = { LedIndexes::CHEST }; goal_msg.mode = LedModes::STEADY; std_msgs::msg::ColorRGBA color; - if (flag) { - color.r = 1.0; color.g = 1.0; color.b = 1.0; - } else { - color.r = 0.0; color.g = 0.0; color.b = 0.0; + if (flag) + { + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; + } + else + { + color.r = 0.0; + color.g = 0.0; + color.b = 0.0; } goal_msg.colors[0] = color; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } -void LedsPlayActionClient::headStatic(bool flag) { +void LedsPlayActionClient::headStatic(bool flag) +{ using namespace std::placeholders; - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::HEAD}; + goal_msg.leds = { LedIndexes::HEAD }; goal_msg.mode = LedModes::STEADY; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { - if (flag) { + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { + if (flag) + { goal_msg.intensities[i] = 1.0; - } else {goal_msg.intensities[i] = 0.0;} + } + else + { + goal_msg.intensities[i] = 0.0; + } } auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } -void LedsPlayActionClient::earsStatic(bool flag) { +void LedsPlayActionClient::earsStatic(bool flag) +{ using namespace std::placeholders; - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::REAR, LedIndexes::LEAR}; + goal_msg.leds = { LedIndexes::REAR, LedIndexes::LEAR }; goal_msg.mode = LedModes::STEADY; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - if (flag) { + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + if (flag) + { goal_msg.intensities[i] = 1.0; - } else {goal_msg.intensities[i] = 0.0;} + } + else + { + goal_msg.intensities[i] = 0.0; + } } auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } -void LedsPlayActionClient::headLoop( bool flag ) { +void LedsPlayActionClient::headLoop(bool flag) +{ using namespace std::placeholders; - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } - if (flag) { + if (flag) + { auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::HEAD}; + goal_msg.leds = { LedIndexes::HEAD }; goal_msg.mode = LedModes::LOOP; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { goal_msg.intensities[i] = 1.0; } goal_msg.frequency = 10.0; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); auto goal_handle_future = client_ptr_->async_send_goal(goal_msg, send_goal_options); head_goal_handle_ = goal_handle_future.get(); - //head_goal_handle_ = client_ptr_->async_send_goal(goal_msg, send_goal_options); - } else { - //rclcpp_action::ClientGoalHandle::SharedPtr handle = head_goal_handle_.get(); - //auto cancel_result_future = client_ptr_->async_cancel_goal(handle); + // head_goal_handle_ = client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + else + { + // rclcpp_action::ClientGoalHandle::SharedPtr handle = + // head_goal_handle_.get(); auto cancel_result_future = client_ptr_->async_cancel_goal(handle); auto cancel_result_future = client_ptr_->async_cancel_goal(head_goal_handle_); } } -void LedsPlayActionClient::earsLoop(bool flag) { +void LedsPlayActionClient::earsLoop(bool flag) +{ using namespace std::placeholders; - if (flag) { - if (!this->client_ptr_->wait_for_action_server()) { + if (flag) + { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::REAR, LedIndexes::LEAR}; + goal_msg.leds = { LedIndexes::REAR, LedIndexes::LEAR }; goal_msg.mode = LedModes::LOOP; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { goal_msg.intensities[i] = 1.0; } goal_msg.frequency = 10.0; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); auto goal_handle_future = client_ptr_->async_send_goal(goal_msg, send_goal_options); ears_goal_handle_ = goal_handle_future.get(); - } else { + } + else + { auto cancel_result_future = client_ptr_->async_cancel_goal(ears_goal_handle_); } } - -void LedsPlayActionClient::eyesLoop(bool flag) { +void LedsPlayActionClient::eyesLoop(bool flag) +{ using namespace std::placeholders; - if (flag) { - if (!this->client_ptr_->wait_for_action_server()) { + if (flag) + { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = LedsPlay::Goal(); - goal_msg.leds = {LedIndexes::REYE, LedIndexes::LEYE}; + goal_msg.leds = { LedIndexes::REYE, LedIndexes::LEYE }; goal_msg.mode = LedModes::LOOP; - + std_msgs::msg::ColorRGBA color; - color.r = 1.0; color.g = 1.0; color.b = 1.0; + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { goal_msg.colors[i] = color; } @@ -319,55 +344,57 @@ void LedsPlayActionClient::eyesLoop(bool flag) { auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&LedsPlayActionClient::goalResponseCallback, this, _1); - //send_goal_options.feedback_callback = - // std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); + // send_goal_options.feedback_callback = + // std::bind(&LedsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&LedsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&LedsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); auto goal_handle_future = client_ptr_->async_send_goal(goal_msg, send_goal_options); eyes_goal_handle_ = goal_handle_future.get(); - } else { + } + else + { auto cancel_result_future = client_ptr_->async_cancel_goal(eyes_goal_handle_); } } - - -void LedsPlayActionClient::goalResponseCallback(const GoalHandleLedsPlay::SharedPtr & goal_handle) { - if (!goal_handle) { +void LedsPlayActionClient::goalResponseCallback(const GoalHandleLedsPlay::SharedPtr& goal_handle) +{ + if (!goal_handle) + { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); } } -void LedsPlayActionClient::feedbackCallback( - GoalHandleLedsPlay::SharedPtr, - const std::shared_ptr feedback) { - - //TODO - +void LedsPlayActionClient::feedbackCallback(GoalHandleLedsPlay::SharedPtr, + const std::shared_ptr feedback) +{ + // TODO } -void LedsPlayActionClient::resultCallback(const GoalHandleLedsPlay::WrappedResult & result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; +void LedsPlayActionClient::resultCallback(const GoalHandleLedsPlay::WrappedResult& result) +{ + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; } if (result.result->success) @@ -376,7 +403,6 @@ void LedsPlayActionClient::resultCallback(const GoalHandleLedsPlay::WrappedResul rclcpp::shutdown(); } - } // namespace nao_led_action_client RCLCPP_COMPONENTS_REGISTER_NODE(nao_led_action_client::LedsPlayActionClient) \ No newline at end of file diff --git a/nao_led_server/src/led_action_server.cpp b/nao_led_server/src/led_action_server.cpp index 2345a58..b1ded15 100644 --- a/nao_led_server/src/led_action_server.cpp +++ b/nao_led_server/src/led_action_server.cpp @@ -18,9 +18,9 @@ //#include #include //#include -#include // std::string, std::stof +#include // std::string, std::stof #include -#include // std::cout +#include // std::cout #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -43,475 +43,513 @@ #include "nao_led_interfaces/msg/led_modes.hpp" #include "nao_led_interfaces/action/leds_play.hpp" - - - -namespace nao_led_action_server { - -LedsPlayActionServer::LedsPlayActionServer(const rclcpp::NodeOptions & options) - : rclcpp::Node("leds_play_action_server_node", options) { - using namespace std::placeholders; - - this->action_server_ = rclcpp_action::create_server( - this, - "leds_play", - std::bind(&LedsPlayActionServer::handleGoal, this, _1, _2), - std::bind(&LedsPlayActionServer::handleCancel, this, _1), - std::bind(&LedsPlayActionServer::handleAccepted, this, _1)); - - this->head_pub_ = this->create_publisher( - "effectors/head_leds", 10); - this->right_eye_pub_ = this->create_publisher( - "effectors/right_eye_leds", 10); - this->left_eye_pub_ = this->create_publisher( - "effectors/left_eye_leds", 10); - this->right_ear_pub_ = this->create_publisher( - "effectors/right_ear_leds", 10); - this->left_ear_pub_ = this->create_publisher( - "effectors/left_ear_leds", 10); - this->chest_pub_ = this->create_publisher( - "effectors/chest_led", 10); - this->right_foot_pub_ = this->create_publisher( - "effectors/right_foot_led", 10); - this->left_foot_pub_ = this->create_publisher( - "effectors/left_foot_led", 10); - - this->color_off_ = std_msgs::msg::ColorRGBA(); - this->color_off_.r = 0.0; - this->color_off_.g = 0.0; - this->color_off_.b = 0.0; - - RCLCPP_INFO(this->get_logger(), "LedsPlayActionServer Initialized"); - +namespace nao_led_action_server +{ + +LedsPlayActionServer::LedsPlayActionServer(const rclcpp::NodeOptions& options) + : rclcpp::Node("leds_play_action_server_node", options) +{ + using namespace std::placeholders; + + this->action_server_ = rclcpp_action::create_server( + this, "leds_play", std::bind(&LedsPlayActionServer::handleGoal, this, _1, _2), + std::bind(&LedsPlayActionServer::handleCancel, this, _1), + std::bind(&LedsPlayActionServer::handleAccepted, this, _1)); + + this->head_pub_ = this->create_publisher("effectors/head_leds", 10); + this->right_eye_pub_ = + this->create_publisher("effectors/right_eye_leds", 10); + this->left_eye_pub_ = this->create_publisher("effectors/left_eye_leds", 10); + this->right_ear_pub_ = + this->create_publisher("effectors/right_ear_leds", 10); + this->left_ear_pub_ = this->create_publisher("effectors/left_ear_leds", 10); + this->chest_pub_ = this->create_publisher("effectors/chest_led", 10); + this->right_foot_pub_ = + this->create_publisher("effectors/right_foot_led", 10); + this->left_foot_pub_ = this->create_publisher("effectors/left_foot_led", 10); + + this->color_off_ = std_msgs::msg::ColorRGBA(); + this->color_off_.r = 0.0; + this->color_off_.g = 0.0; + this->color_off_.b = 0.0; + + RCLCPP_INFO(this->get_logger(), "LedsPlayActionServer Initialized"); } -LedsPlayActionServer::~LedsPlayActionServer() {} - +LedsPlayActionServer::~LedsPlayActionServer() +{ +} rclcpp_action::GoalResponse LedsPlayActionServer::handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) { - - RCLCPP_INFO( this->get_logger(), "Received LED goal request"); - (void)uuid; - - //TODO checks on goal integrity + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), "Received LED goal request"); + (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + // TODO checks on goal integrity + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse LedsPlayActionServer::handleCancel( - const std::shared_ptr> goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel LED goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Received request to cancel LED goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; } void LedsPlayActionServer::handleAccepted( - const std::shared_ptr> goal_handle) { - using namespace std::placeholders; - // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&LedsPlayActionServer::execute, this, _1), goal_handle} .detach(); + const std::shared_ptr> goal_handle) +{ + using namespace std::placeholders; + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{ std::bind(&LedsPlayActionServer::execute, this, _1), goal_handle }.detach(); } void LedsPlayActionServer::execute( - const std::shared_ptr> goal_handle) { - - RCLCPP_INFO(this->get_logger(), "Executing LED goal"); - - auto goal = goal_handle->get_goal(); - bool canceled = false; - - std::array leds = goal->leds; - uint8_t mode = goal->mode; - float frequency = goal->frequency; - std::array colors = goal->colors; - std::array intensities = goal->intensities; - float duration = goal->duration; - - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - - if (mode == nao_led_interfaces::msg::LedModes::STEADY) { - steadyMode(goal_handle, leds, frequency, colors, intensities); - - } else if (mode == nao_led_interfaces::msg::LedModes::BLINKING) { - - canceled = blinkingMode(goal_handle, leds, frequency, colors ); - if (canceled) { - result->success = true; - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "LED Goal canceled"); - return; - } - // - - } else if (mode == nao_led_interfaces::msg::LedModes::LOOP) { - canceled = loopMode(goal_handle, leds, frequency, colors); - if (canceled) { - result->success = true; - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "LED Goal canceled"); - return; - } + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Executing LED goal"); + + auto goal = goal_handle->get_goal(); + bool canceled = false; + + std::array leds = goal->leds; + uint8_t mode = goal->mode; + float frequency = goal->frequency; + std::array colors = goal->colors; + std::array intensities = goal->intensities; + float duration = goal->duration; + + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + if (mode == nao_led_interfaces::msg::LedModes::STEADY) + { + steadyMode(goal_handle, leds, frequency, colors, intensities); + } + else if (mode == nao_led_interfaces::msg::LedModes::BLINKING) + { + canceled = blinkingMode(goal_handle, leds, frequency, colors); + if (canceled) + { + result->success = true; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "LED Goal canceled"); + return; } - - if (rclcpp::ok()) { - result->success = true; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "LED Goal succeeded"); + // + } + else if (mode == nao_led_interfaces::msg::LedModes::LOOP) + { + canceled = loopMode(goal_handle, leds, frequency, colors); + if (canceled) + { + result->success = true; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "LED Goal canceled"); + return; } - + } + + if (rclcpp::ok()) + { + result->success = true; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "LED Goal succeeded"); + } } - void LedsPlayActionServer::steadyMode( const std::shared_ptr> goal_handle, - std::array & leds, float frequency, - std::array & colors, - std::array & intensities) { - - auto result = std::make_shared(); - - if (leds[0] == nao_led_interfaces::msg::LedIndexes::HEAD) { - nao_lola_command_msgs::msg::HeadLeds head_leds_msg; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { - head_leds_msg.intensities[i] = intensities[i]; - } - head_pub_->publish(head_leds_msg); + std::array& leds, float frequency, std::array& colors, + std::array& intensities) +{ + auto result = std::make_shared(); + + if (leds[0] == nao_led_interfaces::msg::LedIndexes::HEAD) + { + nao_lola_command_msgs::msg::HeadLeds head_leds_msg; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { + head_leds_msg.intensities[i] = intensities[i]; } - - if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REYE && leds[1] == nao_led_interfaces::msg::LedIndexes::LEYE) || - (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE && leds[1] == nao_led_interfaces::msg::LedIndexes::REYE)) { - nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_msg; - nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_msg; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { - right_eye_leds_msg.colors[i] = colors[i]; - left_eye_leds_msg.colors[i] = colors[i]; - } - - right_eye_pub_->publish(right_eye_leds_msg); - left_eye_pub_->publish(left_eye_leds_msg); - + head_pub_->publish(head_leds_msg); + } + + if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REYE && leds[1] == nao_led_interfaces::msg::LedIndexes::LEYE) || + (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE && leds[1] == nao_led_interfaces::msg::LedIndexes::REYE)) + { + nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_msg; + nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_msg; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { + right_eye_leds_msg.colors[i] = colors[i]; + left_eye_leds_msg.colors[i] = colors[i]; } - if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REAR && leds[1] == nao_led_interfaces::msg::LedIndexes::LEAR) || - (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR && leds[1] == nao_led_interfaces::msg::LedIndexes::REAR)) { - nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_msg; - nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_msg; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - right_ear_leds_msg.intensities[i] = intensities[i]; - left_ear_leds_msg.intensities[i] = intensities[i]; - } - - right_ear_pub_->publish(right_ear_leds_msg); - left_ear_pub_->publish(left_ear_leds_msg); + right_eye_pub_->publish(right_eye_leds_msg); + left_eye_pub_->publish(left_eye_leds_msg); + } + + if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REAR && leds[1] == nao_led_interfaces::msg::LedIndexes::LEAR) || + (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR && leds[1] == nao_led_interfaces::msg::LedIndexes::REAR)) + { + nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_msg; + nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_msg; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + right_ear_leds_msg.intensities[i] = intensities[i]; + left_ear_leds_msg.intensities[i] = intensities[i]; } - if (leds[0] == nao_led_interfaces::msg::LedIndexes::CHEST) { - nao_lola_command_msgs::msg::ChestLed chest_led_msg; - chest_led_msg.color = colors[0]; - chest_pub_->publish(chest_led_msg); - } + right_ear_pub_->publish(right_ear_leds_msg); + left_ear_pub_->publish(left_ear_leds_msg); + } + if (leds[0] == nao_led_interfaces::msg::LedIndexes::CHEST) + { + nao_lola_command_msgs::msg::ChestLed chest_led_msg; + chest_led_msg.color = colors[0]; + chest_pub_->publish(chest_led_msg); + } } - bool LedsPlayActionServer::blinkingMode( const std::shared_ptr> goal_handle, - std::array & leds, float frequency, - std::array & colors) { - - rclcpp::Rate loop_rate(frequency); - bool canceled = false; - - if (leds[0] == nao_led_interfaces::msg::LedIndexes::HEAD) { - - nao_lola_command_msgs::msg::HeadLeds head_leds_on; - nao_lola_command_msgs::msg::HeadLeds head_leds_off; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { - head_leds_on.intensities[i] = 1.0; - head_leds_off.intensities[i] = 0.0; - } - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % 2; - - if (c == 0) { - head_pub_->publish(head_leds_on); - } else { - head_pub_->publish(head_leds_off); - } - loop_rate.sleep(); - } + std::array& leds, float frequency, std::array& colors) +{ + rclcpp::Rate loop_rate(frequency); + bool canceled = false; + + if (leds[0] == nao_led_interfaces::msg::LedIndexes::HEAD) + { + nao_lola_command_msgs::msg::HeadLeds head_leds_on; + nao_lola_command_msgs::msg::HeadLeds head_leds_off; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { + head_leds_on.intensities[i] = 1.0; + head_leds_off.intensities[i] = 0.0; } - - - if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REYE && leds[1] == nao_led_interfaces::msg::LedIndexes::LEYE)|| - (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE && leds[1] == nao_led_interfaces::msg::LedIndexes::REYE)) { - nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_off; - nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_off; - nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_on; - nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_on; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { - right_eye_leds_off.colors[i] = color_off_; - left_eye_leds_off.colors[i] = color_off_; - right_eye_leds_on.colors[i] = colors[i]; - left_eye_leds_on.colors[i] = colors[i]; - } - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % 2; - - if (c == 0) { - right_eye_pub_->publish(right_eye_leds_off); - left_eye_pub_->publish(left_eye_leds_off); - - } else { - right_eye_pub_->publish(right_eye_leds_on); - left_eye_pub_->publish(left_eye_leds_on); - } - loop_rate.sleep(); - } + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % 2; + + if (c == 0) + { + head_pub_->publish(head_leds_on); + } + else + { + head_pub_->publish(head_leds_off); + } + loop_rate.sleep(); } - - if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REAR && leds[1] == nao_led_interfaces::msg::LedIndexes::LEAR) || - (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR && leds[1] == nao_led_interfaces::msg::LedIndexes::REAR)) { - nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_off; - nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_off; - nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_on; - nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_on; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - right_ear_leds_off.intensities[i] = 0.0; - left_ear_leds_off.intensities[i] = 0.0; - right_ear_leds_on.intensities[i] = 1.0; - left_ear_leds_on.intensities[i] = 1.0; - } - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % 2; - - if (c == 0) { - right_ear_pub_->publish(right_ear_leds_off); - left_ear_pub_->publish(left_ear_leds_off); - - } else { - right_ear_pub_->publish(right_ear_leds_on); - left_ear_pub_->publish(left_ear_leds_on); - } - loop_rate.sleep(); - } + } + + if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REYE && leds[1] == nao_led_interfaces::msg::LedIndexes::LEYE) || + (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE && leds[1] == nao_led_interfaces::msg::LedIndexes::REYE)) + { + nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_off; + nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_off; + nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_on; + nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_on; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { + right_eye_leds_off.colors[i] = color_off_; + left_eye_leds_off.colors[i] = color_off_; + right_eye_leds_on.colors[i] = colors[i]; + left_eye_leds_on.colors[i] = colors[i]; } - - if (leds[0] == nao_led_interfaces::msg::LedIndexes::CHEST) { - - nao_lola_command_msgs::msg::ChestLed chest_led_on; - nao_lola_command_msgs::msg::ChestLed chest_led_off; - chest_led_on.color = colors[0]; - chest_led_off.color = color_off_; - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % 2; - - if (c == 0) { - chest_pub_->publish(chest_led_on); - } else { - chest_pub_->publish(chest_led_off); - } - loop_rate.sleep(); - } + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % 2; + + if (c == 0) + { + right_eye_pub_->publish(right_eye_leds_off); + left_eye_pub_->publish(left_eye_leds_off); + } + else + { + right_eye_pub_->publish(right_eye_leds_on); + left_eye_pub_->publish(left_eye_leds_on); + } + loop_rate.sleep(); } - RCLCPP_ERROR(this->get_logger(), "LED blinking mode not executed"); - return canceled = false; + } + + if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REAR && leds[1] == nao_led_interfaces::msg::LedIndexes::LEAR) || + (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR && leds[1] == nao_led_interfaces::msg::LedIndexes::REAR)) + { + nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_off; + nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_off; + nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_on; + nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_on; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + right_ear_leds_off.intensities[i] = 0.0; + left_ear_leds_off.intensities[i] = 0.0; + right_ear_leds_on.intensities[i] = 1.0; + left_ear_leds_on.intensities[i] = 1.0; + } + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % 2; + + if (c == 0) + { + right_ear_pub_->publish(right_ear_leds_off); + left_ear_pub_->publish(left_ear_leds_off); + } + else + { + right_ear_pub_->publish(right_ear_leds_on); + left_ear_pub_->publish(left_ear_leds_on); + } + loop_rate.sleep(); + } + } + + if (leds[0] == nao_led_interfaces::msg::LedIndexes::CHEST) + { + nao_lola_command_msgs::msg::ChestLed chest_led_on; + nao_lola_command_msgs::msg::ChestLed chest_led_off; + chest_led_on.color = colors[0]; + chest_led_off.color = color_off_; + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % 2; + + if (c == 0) + { + chest_pub_->publish(chest_led_on); + } + else + { + chest_pub_->publish(chest_led_off); + } + loop_rate.sleep(); + } + } + RCLCPP_ERROR(this->get_logger(), "LED blinking mode not executed"); + return canceled = false; } - bool LedsPlayActionServer::loopMode( const std::shared_ptr> goal_handle, - std::array & leds, - float frequency, - std::array & colors) { - - rclcpp::Rate loop_rate(frequency); - bool canceled = false; - - //HEAD - if (leds[0] == nao_led_interfaces::msg::LedIndexes::HEAD) { - - nao_lola_command_msgs::msg::HeadLeds head_leds_msg; - - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; - - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { - head_leds_msg.intensities[i] = 1.0; - } - head_leds_msg.intensities[c] = 0.0; - - head_pub_->publish(head_leds_msg); - - loop_rate.sleep(); - } + std::array& leds, float frequency, std::array& colors) +{ + rclcpp::Rate loop_rate(frequency); + bool canceled = false; + + // HEAD + if (leds[0] == nao_led_interfaces::msg::LedIndexes::HEAD) + { + nao_lola_command_msgs::msg::HeadLeds head_leds_msg; + + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; + + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { + head_leds_msg.intensities[i] = 1.0; + } + head_leds_msg.intensities[c] = 0.0; + + head_pub_->publish(head_leds_msg); + + loop_rate.sleep(); } - - // EYES - if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REYE && leds[1] == nao_led_interfaces::msg::LedIndexes::LEYE) || - (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE && leds[1] == nao_led_interfaces::msg::LedIndexes::REYE)) { - nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_msg; - nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_msg; - short int cw = 0, cw_succ = 0; - short int ccw = 0, ccw_succ = 0; - const short int num_eye_leds = nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - - for (unsigned i = 0; i < num_eye_leds; ++i) { - right_eye_leds_msg.colors[i] = colors[i]; - left_eye_leds_msg.colors[i] = colors[i]; - } - - cw = (cw + 1) % num_eye_leds; - cw_succ = (cw+1) % num_eye_leds; - - ccw = (cw != 0) ? num_eye_leds-cw : 0; - ccw_succ = (ccw != 0) ? ccw-1 : 7; - - right_eye_leds_msg.colors[cw] = color_off_; - right_eye_leds_msg.colors[cw_succ] = color_off_; - left_eye_leds_msg.colors[ccw] = color_off_; - left_eye_leds_msg.colors[ccw_succ] = color_off_; - right_eye_pub_->publish(right_eye_leds_msg); - left_eye_pub_->publish(left_eye_leds_msg); - - loop_rate.sleep(); - } + } + + // EYES + if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REYE && leds[1] == nao_led_interfaces::msg::LedIndexes::LEYE) || + (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE && leds[1] == nao_led_interfaces::msg::LedIndexes::REYE)) + { + nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_msg; + nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_msg; + short int cw = 0, cw_succ = 0; + short int ccw = 0, ccw_succ = 0; + const short int num_eye_leds = nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + + for (unsigned i = 0; i < num_eye_leds; ++i) + { + right_eye_leds_msg.colors[i] = colors[i]; + left_eye_leds_msg.colors[i] = colors[i]; + } + + cw = (cw + 1) % num_eye_leds; + cw_succ = (cw + 1) % num_eye_leds; + + ccw = (cw != 0) ? num_eye_leds - cw : 0; + ccw_succ = (ccw != 0) ? ccw - 1 : 7; + + right_eye_leds_msg.colors[cw] = color_off_; + right_eye_leds_msg.colors[cw_succ] = color_off_; + left_eye_leds_msg.colors[ccw] = color_off_; + left_eye_leds_msg.colors[ccw_succ] = color_off_; + right_eye_pub_->publish(right_eye_leds_msg); + left_eye_pub_->publish(left_eye_leds_msg); + + loop_rate.sleep(); } - if (leds[0] == nao_led_interfaces::msg::LedIndexes::REYE ) { - nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_msg; - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; - - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { - right_eye_leds_msg.colors[i] = colors[i]; - } - right_eye_leds_msg.colors[c] = color_off_; - right_eye_pub_->publish(right_eye_leds_msg); - - loop_rate.sleep(); - } + } + if (leds[0] == nao_led_interfaces::msg::LedIndexes::REYE) + { + nao_lola_command_msgs::msg::RightEyeLeds right_eye_leds_msg; + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; + + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { + right_eye_leds_msg.colors[i] = colors[i]; + } + right_eye_leds_msg.colors[c] = color_off_; + right_eye_pub_->publish(right_eye_leds_msg); + + loop_rate.sleep(); } - if (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE) { - nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_msg; - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; - - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { - left_eye_leds_msg.colors[i] = colors[i]; - } - left_eye_leds_msg.colors[c] = color_off_; - left_eye_pub_->publish(left_eye_leds_msg); - - loop_rate.sleep(); - } + } + if (leds[0] == nao_led_interfaces::msg::LedIndexes::LEYE) + { + nao_lola_command_msgs::msg::LeftEyeLeds left_eye_leds_msg; + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; + + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { + left_eye_leds_msg.colors[i] = colors[i]; + } + left_eye_leds_msg.colors[c] = color_off_; + left_eye_pub_->publish(left_eye_leds_msg); + + loop_rate.sleep(); } - - //EARS - if ( (leds[0] == nao_led_interfaces::msg::LedIndexes::REAR && leds[1] == nao_led_interfaces::msg::LedIndexes::LEAR) || - (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR && leds[1] == nao_led_interfaces::msg::LedIndexes::REAR)) { - - nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_msg; - nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_msg; - - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - right_ear_leds_msg.intensities[i] = 1.0; - left_ear_leds_msg.intensities[i] = 1.0; - } - right_ear_leds_msg.intensities[c] = 0.0; - left_ear_leds_msg.intensities[c] = 0.0; - right_ear_pub_->publish(right_ear_leds_msg); - left_ear_pub_->publish(left_ear_leds_msg); - - loop_rate.sleep(); - } + } + + // EARS + if ((leds[0] == nao_led_interfaces::msg::LedIndexes::REAR && leds[1] == nao_led_interfaces::msg::LedIndexes::LEAR) || + (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR && leds[1] == nao_led_interfaces::msg::LedIndexes::REAR)) + { + nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_msg; + nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_msg; + + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + right_ear_leds_msg.intensities[i] = 1.0; + left_ear_leds_msg.intensities[i] = 1.0; + } + right_ear_leds_msg.intensities[c] = 0.0; + left_ear_leds_msg.intensities[c] = 0.0; + right_ear_pub_->publish(right_ear_leds_msg); + left_ear_pub_->publish(left_ear_leds_msg); + + loop_rate.sleep(); } - if (leds[0] == nao_led_interfaces::msg::LedIndexes::REAR) { - - nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_msg; - - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - right_ear_leds_msg.intensities[i] = 1.0; - } - right_ear_leds_msg.intensities[c] = 0.0; - right_ear_pub_->publish(right_ear_leds_msg); - - loop_rate.sleep(); - } + } + if (leds[0] == nao_led_interfaces::msg::LedIndexes::REAR) + { + nao_lola_command_msgs::msg::RightEarLeds right_ear_leds_msg; + + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + right_ear_leds_msg.intensities[i] = 1.0; + } + right_ear_leds_msg.intensities[c] = 0.0; + right_ear_pub_->publish(right_ear_leds_msg); + + loop_rate.sleep(); } - if (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR) { - - nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_msg; - - uint8_t c = 0; - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - return canceled = true; - } - c = (c + 1) % nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - left_ear_leds_msg.intensities[i] = 1.0; - } - left_ear_leds_msg.intensities[c] = 0.0; - left_ear_pub_->publish(left_ear_leds_msg); - - loop_rate.sleep(); - } + } + if (leds[0] == nao_led_interfaces::msg::LedIndexes::LEAR) + { + nao_lola_command_msgs::msg::LeftEarLeds left_ear_leds_msg; + + uint8_t c = 0; + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + return canceled = true; + } + c = (c + 1) % nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + left_ear_leds_msg.intensities[i] = 1.0; + } + left_ear_leds_msg.intensities[c] = 0.0; + left_ear_pub_->publish(left_ear_leds_msg); + + loop_rate.sleep(); } + } - RCLCPP_ERROR(this->get_logger(), "LED loop mode not executed"); - return canceled = false; + RCLCPP_ERROR(this->get_logger(), "LED loop mode not executed"); + return canceled = false; } - } // namespace nao_led_action_server RCLCPP_COMPONENTS_REGISTER_NODE(nao_led_action_server::LedsPlayActionServer) \ No newline at end of file