diff --git a/.github/workflows/build_and_test_humble.yaml b/.github/workflows/build_and_test_humble.yaml index 18ba436..2e3b7ea 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 - - uses: ros-tooling/action-ros-ci@v0.3 with: - skip-tests: true + use-ros2-testing: true + - uses: ros-tooling/action-ros-ci@v0.3.7 + with: + #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..8d97917 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 - - uses: ros-tooling/action-ros-ci@v0.3 with: - skip-tests: true + use-ros2-testing: true + - uses: ros-tooling/action-ros-ci@v0.3.7 + with: + #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_interfaces/CMakeLists.txt b/nao_led_interfaces/CMakeLists.txt index 7b6d606..bf503eb 100644 --- a/nao_led_interfaces/CMakeLists.txt +++ b/nao_led_interfaces/CMakeLists.txt @@ -15,17 +15,17 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/LedModes.msg" "action/LedsPlay.action" DEPENDENCIES geometry_msgs - ) +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) + #set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/nao_led_interfaces/package.xml b/nao_led_interfaces/package.xml index e306ce3..28350a0 100644 --- a/nao_led_interfaces/package.xml +++ b/nao_led_interfaces/package.xml @@ -10,14 +10,13 @@ ament_cmake geometry_msgs + ament_lint_auto + ament_lint_common + ament_cmake_clang_format rosidl_default_generators rosidl_default_runtime rosidl_interface_packages - - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/nao_led_server/CMakeLists.txt b/nao_led_server/CMakeLists.txt index f58e2ef..c8130c4 100644 --- a/nao_led_server/CMakeLists.txt +++ b/nao_led_server/CMakeLists.txt @@ -114,11 +114,11 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files -set(ament_cmake_copyright_FOUND TRUE) +#set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() 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..6f1252c 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 @@ -18,68 +18,68 @@ #include #include #include -#include #include +#include #include "ament_index_cpp/get_package_share_directory.hpp" #include "boost/filesystem.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" - +#include "nao_led_interfaces/action/leds_play.hpp" +#include "nao_led_interfaces/msg/led_indexes.hpp" +#include "nao_led_interfaces/msg/led_modes.hpp" +#include "nao_lola_command_msgs/msg/chest_led.hpp" #include "nao_lola_command_msgs/msg/head_leds.hpp" -#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" #include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_foot_led.hpp" #include "nao_lola_command_msgs/msg/right_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/chest_led.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" #include "nao_lola_command_msgs/msg/right_foot_led.hpp" -#include "nao_lola_command_msgs/msg/left_foot_led.hpp" - -#include "nao_led_interfaces/action/leds_play.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 "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" #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_ 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..18e1e6d 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,32 +21,27 @@ //#include #include //#include -#include // std::string, std::stof #include -#include // std::cout - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" +#include // std::cout +#include // std::string, std::stof +#include "nao_led_interfaces/action/leds_play.hpp" +#include "nao_led_interfaces/msg/led_indexes.hpp" +#include "nao_led_interfaces/msg/led_modes.hpp" +#include "nao_lola_command_msgs/msg/chest_led.hpp" #include "nao_lola_command_msgs/msg/head_leds.hpp" -#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" #include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_foot_led.hpp" #include "nao_lola_command_msgs/msg/right_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/chest_led.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" #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 "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.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 +49,66 @@ 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 +#endif // NAO_LED_SERVER__LED_ACTION_SERVER_HPP_ diff --git a/nao_led_server/package.xml b/nao_led_server/package.xml index c96399f..cc0dff7 100644 --- a/nao_led_server/package.xml +++ b/nao_led_server/package.xml @@ -22,7 +22,8 @@ ament_lint_auto ament_lint_common - + ament_cmake_clang_format + ament_cmake diff --git a/nao_led_server/src/led_action_client.cpp b/nao_led_server/src/led_action_client.cpp index 54cf3b1..a02c717 100644 --- a/nao_led_server/src/led_action_client.cpp +++ b/nao_led_server/src/led_action_client.cpp @@ -12,38 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "nao_led_server/led_action_client.hpp" + #include #include #include -#include #include +#include #include "ament_index_cpp/get_package_share_directory.hpp" #include "boost/filesystem.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" - +#include "nao_led_interfaces/action/leds_play.hpp" +#include "nao_led_interfaces/msg/led_indexes.hpp" +#include "nao_led_interfaces/msg/led_modes.hpp" +#include "nao_lola_command_msgs/msg/chest_led.hpp" #include "nao_lola_command_msgs/msg/head_leds.hpp" -#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" #include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_foot_led.hpp" #include "nao_lola_command_msgs/msg/right_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/chest_led.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" #include "nao_lola_command_msgs/msg/right_foot_led.hpp" -#include "nao_lola_command_msgs/msg/left_foot_led.hpp" - -#include "nao_led_interfaces/action/leds_play.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 "nao_led_server/led_action_client.hpp" - +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" #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,23 +47,18 @@ 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"); +: 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 ) { - +void LedsPlayActionClient::eyesStatic(bool flag) +{ using namespace std::placeholders; if (!this->client_ptr_->wait_for_action_server()) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); @@ -80,9 +71,13 @@ void LedsPlayActionClient::eyesStatic( bool flag ) { goal_msg.mode = LedModes::STEADY; std_msgs::msg::ColorRGBA color; if (flag) { - color.r = 1.0; color.g = 1.0; color.b = 1.0; + 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; + 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) { @@ -97,17 +92,15 @@ void LedsPlayActionClient::eyesStatic( bool flag ) { 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()) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); @@ -120,9 +113,13 @@ void LedsPlayActionClient::chestStatic( bool flag ) { goal_msg.mode = LedModes::STEADY; std_msgs::msg::ColorRGBA color; if (flag) { - color.r = 1.0; color.g = 1.0; color.b = 1.0; + 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; + color.r = 0.0; + color.g = 0.0; + color.b = 0.0; } goal_msg.colors[0] = color; @@ -135,16 +132,15 @@ void LedsPlayActionClient::chestStatic( bool flag ) { 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()) { @@ -159,7 +155,9 @@ void LedsPlayActionClient::headStatic(bool 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(); @@ -170,16 +168,15 @@ void LedsPlayActionClient::headStatic(bool flag) { 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()) { @@ -194,7 +191,9 @@ void LedsPlayActionClient::earsStatic(bool 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(); @@ -205,16 +204,15 @@ void LedsPlayActionClient::earsStatic(bool flag) { 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()) { @@ -240,22 +238,24 @@ void LedsPlayActionClient::headLoop( bool flag ) { 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); + // 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); + // 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) { @@ -281,10 +281,9 @@ void LedsPlayActionClient::earsLoop(bool flag) { 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(); @@ -293,8 +292,8 @@ void LedsPlayActionClient::earsLoop(bool flag) { } } - -void LedsPlayActionClient::eyesLoop(bool flag) { +void LedsPlayActionClient::eyesLoop(bool flag) +{ using namespace std::placeholders; if (flag) { @@ -307,9 +306,11 @@ void LedsPlayActionClient::eyesLoop(bool flag) { 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) { goal_msg.colors[i] = color; @@ -322,13 +323,12 @@ void LedsPlayActionClient::eyesLoop(bool flag) { 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(); @@ -337,9 +337,8 @@ void LedsPlayActionClient::eyesLoop(bool flag) { } } - - -void LedsPlayActionClient::goalResponseCallback(const GoalHandleLedsPlay::SharedPtr & goal_handle) { +void LedsPlayActionClient::goalResponseCallback(const GoalHandleLedsPlay::SharedPtr & goal_handle) +{ if (!goal_handle) { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); } else { @@ -348,35 +347,32 @@ void LedsPlayActionClient::goalResponseCallback(const GoalHandleLedsPlay::Shared } void LedsPlayActionClient::feedbackCallback( - GoalHandleLedsPlay::SharedPtr, - const std::shared_ptr feedback) { - - //TODO - + GoalHandleLedsPlay::SharedPtr, const std::shared_ptr feedback) +{ + // TODO } -void LedsPlayActionClient::resultCallback(const GoalHandleLedsPlay::WrappedResult & result) { +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; + 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) - RCLCPP_INFO(this->get_logger(), "Leds regulary played."); + if (result.result->success) {RCLCPP_INFO(this->get_logger(), "Leds regulary played.");} rclcpp::shutdown(); } - } // namespace nao_led_action_client -RCLCPP_COMPONENTS_REGISTER_NODE(nao_led_action_client::LedsPlayActionClient) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(nao_led_action_client::LedsPlayActionClient) diff --git a/nao_led_server/src/led_action_server.cpp b/nao_led_server/src/led_action_server.cpp index 2345a58..309f289 100644 --- a/nao_led_server/src/led_action_server.cpp +++ b/nao_led_server/src/led_action_server.cpp @@ -18,500 +18,498 @@ //#include #include //#include -#include // std::string, std::stof #include -#include // std::cout - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" - -#include "std_msgs/msg/color_rgba.hpp" +#include // std::cout +#include // std::string, std::stof +#include "nao_led_interfaces/action/leds_play.hpp" +#include "nao_led_interfaces/msg/led_indexes.hpp" +#include "nao_led_interfaces/msg/led_modes.hpp" +#include "nao_led_server/led_action_server.hpp" +#include "nao_lola_command_msgs/msg/chest_led.hpp" #include "nao_lola_command_msgs/msg/head_leds.hpp" -#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" #include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_foot_led.hpp" #include "nao_lola_command_msgs/msg/right_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" -#include "nao_lola_command_msgs/msg/chest_led.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" #include "nao_lola_command_msgs/msg/right_foot_led.hpp" -#include "nao_lola_command_msgs/msg/left_foot_led.hpp" - -#include "nao_led_server/led_action_server.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 "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "std_msgs/msg/color_rgba.hpp" -namespace nao_led_action_server { +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"); - +: 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() {} - 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; + 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 - - 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); + 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]; } - - 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(); - } + 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; } - - - 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(); + } + } + + 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; + } + 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(); - } + 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(); } - - // 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 +RCLCPP_COMPONENTS_REGISTER_NODE(nao_led_action_server::LedsPlayActionServer)