From e12df8d293a2a83401e5570784a2298819ad6ee3 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 11:42:25 -0300 Subject: [PATCH 1/7] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Actions=20tutorial=20l?= =?UTF-8?q?ambda=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- .../action_tutorials_cpp/README.md | 29 +++-- .../src/fibonacci_action_client.cpp | 104 ++++++++---------- .../src/fibonacci_action_server.cpp | 76 ++++++------- 3 files changed, 102 insertions(+), 107 deletions(-) diff --git a/action_tutorials/action_tutorials_cpp/README.md b/action_tutorials/action_tutorials_cpp/README.md index 5a9d68b2e..0ec1d24fc 100644 --- a/action_tutorials/action_tutorials_cpp/README.md +++ b/action_tutorials/action_tutorials_cpp/README.md @@ -4,16 +4,17 @@ In the constructor for `FibonacciActionServer`, an action server is created with ```cpp this->action_server_ = rclcpp_action::create_server( - ... + this, "fibonacci", - std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), - std::bind(&FibonacciActionServer::handle_cancel, this, _1), - std::bind(&FibonacciActionServer::handle_accepted, this, _1)); + handle_goal, + handle_cancel, + handle_accepted); ``` The `handle_goal` callback is called whenever a goal is sent to the action server by an action client. In the example code, the goal is accepted as long as the order is less than or equal to 46, otherwise it is rejected. This is to prevent potential [integer overflow](https://en.wikipedia.org/wiki/Integer_overflow): + ```cpp if (goal->order > 46) { return rclcpp_action::GoalResponse::REJECT; @@ -25,8 +26,10 @@ The `handle_cancelled` callback is called whenever an action client requests to In this case, the goal cancel request is always accepted. The `handle_accepted` callback is called following the action server's acceptance of a goal. In this example, a thread is started to execute the goal: + ```cpp -std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach(); +auto execute_in_thread = [this, goal_handle](){return this->execute(goal_handle);}; +std::thread{execute_in_thread}.detach(); ``` The execution thread calculates the Fibonacci sequence up to *order* and publishes partial sequences as feedback as each item is added to the sequence. @@ -51,12 +54,16 @@ The goal is sent asynchronously with callbacks registered for the goal response, ```cpp auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); -send_goal_options.goal_response_callback = - std::bind(&FibonacciActionClient::goal_response_callback, this, _1); -send_goal_options.feedback_callback = - std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); -send_goal_options.result_callback = - std::bind(&FibonacciActionClient::result_callback, this, _1); +send_goal_options.goal_response_callback = [this]( + const GoalHandleFibonacci::SharedPtr & goal_handle) +{...}; +send_goal_options.feedback_callback = [this]( + GoalHandleFibonacci::SharedPtr, + const std::shared_ptr feedback) +{...}; +send_goal_options.result_callback = [this]( + const GoalHandleFibonacci::WrappedResult & result) +{...}; this->client_ptr_->async_send_goal(goal_msg, send_goal_options); ``` diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp index c4cf996d3..a67806b12 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp @@ -45,7 +45,7 @@ class FibonacciActionClient : public rclcpp::Node this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), - std::bind(&FibonacciActionClient::send_goal, this)); + [this](){ return this->send_goal(); }); } ACTION_TUTORIALS_CPP_PUBLIC @@ -67,67 +67,59 @@ class FibonacciActionClient : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Sending goal"); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&FibonacciActionClient::goal_response_callback, this, _1); - send_goal_options.feedback_callback = - std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&FibonacciActionClient::result_callback, this, _1); + send_goal_options.goal_response_callback = [this]( + const GoalHandleFibonacci::SharedPtr & goal_handle) + { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + + send_goal_options.feedback_callback = [this]( + GoalHandleFibonacci::SharedPtr, + const std::shared_ptr feedback) + { + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + }; + + send_goal_options.result_callback = [this]( + const GoalHandleFibonacci::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; + } + std::stringstream ss; + ss << "Result received: "; + for (auto number : result.result->sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + rclcpp::shutdown(); + }; + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } private: rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::TimerBase::SharedPtr timer_; - - ACTION_TUTORIALS_CPP_LOCAL - void goal_response_callback(GoalHandleFibonacci::SharedPtr goal_handle) - { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - rclcpp::shutdown(); - } else { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - } - - ACTION_TUTORIALS_CPP_LOCAL - void feedback_callback( - GoalHandleFibonacci::SharedPtr, - const std::shared_ptr feedback) - { - std::stringstream ss; - ss << "Next number in sequence received: "; - for (auto number : feedback->partial_sequence) { - ss << number << " "; - } - RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str()); - } - - ACTION_TUTORIALS_CPP_LOCAL - void result_callback(const GoalHandleFibonacci::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; - } - std::stringstream ss; - ss << "Result received: "; - for (auto number : result.result->sequence) { - ss << number << " "; - } - RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str()); - rclcpp::shutdown(); - } }; // class FibonacciActionClient } // namespace action_tutorials_cpp diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp index cb95bac2b..ad9fadd7f 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp @@ -36,53 +36,49 @@ class FibonacciActionServer : public rclcpp::Node { using namespace std::placeholders; + auto handle_goal = [this]( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + (void)uuid; + RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); + // The Fibonacci action uses int32 for the return of sequences, which means it can only + // hold 2^31-1 (2147483647) before wrapping negative in two's complement. Based on empirical + // tests, that means that an order of > 46 will cause wrapping, so we don't allow that here. + if (goal->order > 46) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handle_cancel = [this]( + const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this]( + const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, + // so we declare a lambda function to be called inside a new thread + auto execute_in_thread = [this, goal_handle](){return this->execute(goal_handle);}; + std::thread{execute_in_thread}.detach(); + }; + this->action_server_ = rclcpp_action::create_server( - this->get_node_base_interface(), - this->get_node_clock_interface(), - this->get_node_logging_interface(), - this->get_node_waitables_interface(), + this, "fibonacci", - std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), - std::bind(&FibonacciActionServer::handle_cancel, this, _1), - std::bind(&FibonacciActionServer::handle_accepted, this, _1)); + handle_goal, + handle_cancel, + handle_accepted); } private: rclcpp_action::Server::SharedPtr action_server_; - ACTION_TUTORIALS_CPP_LOCAL - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - (void)uuid; - RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); - // The Fibonacci action uses int32 for the return of sequences, which means it can only - // hold 2^31-1 (2147483647) before wrapping negative in two's complement. Based on empirical - // tests, that means that an order of > 46 will cause wrapping, so we don't allow that here. - if (goal->order > 46) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - ACTION_TUTORIALS_CPP_LOCAL - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - ACTION_TUTORIALS_CPP_LOCAL - void handle_accepted(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(&FibonacciActionServer::execute, this, _1), goal_handle}.detach(); - } - ACTION_TUTORIALS_CPP_LOCAL void execute(const std::shared_ptr goal_handle) { From 55cf0235571457bcd643f5c5236bc14efc9c8c65 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 11:42:52 -0300 Subject: [PATCH 2/7] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Composition=20nodes=20?= =?UTF-8?q?lambda=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- composition/src/client_component.cpp | 2 +- composition/src/talker_component.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/composition/src/client_component.cpp b/composition/src/client_component.cpp index 953d4adc3..a5033d786 100644 --- a/composition/src/client_component.cpp +++ b/composition/src/client_component.cpp @@ -33,7 +33,7 @@ Client::Client(const rclcpp::NodeOptions & options) // Note(dhood): The timer period must be greater than the duration of the timer callback. // Otherwise, the timer can starve a single-threaded executor. // See https://github.com/ros2/rclcpp/issues/392 for updates. - timer_ = create_wall_timer(2s, std::bind(&Client::on_timer, this)); + timer_ = create_wall_timer(2s, [this](){return this->on_timer();}); } void Client::on_timer() diff --git a/composition/src/talker_component.cpp b/composition/src/talker_component.cpp index 510584058..a05c4bbc5 100644 --- a/composition/src/talker_component.cpp +++ b/composition/src/talker_component.cpp @@ -37,7 +37,7 @@ Talker::Talker(const rclcpp::NodeOptions & options) pub_ = create_publisher("chatter", 10); // Use a timer to schedule periodic message publishing. - timer_ = create_wall_timer(1s, std::bind(&Talker::on_timer, this)); + timer_ = create_wall_timer(1s, [this](){ return this->on_timer(); }); } void Talker::on_timer() From 2e468481795a9c9f7de8817d759fc3596df4cd0e Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 11:43:22 -0300 Subject: [PATCH 3/7] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Lifecycle=20nodes=20la?= =?UTF-8?q?mbda=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- lifecycle/src/lifecycle_listener.cpp | 5 +++-- lifecycle/src/lifecycle_service_client.cpp | 10 ++++++++-- lifecycle/src/lifecycle_talker.cpp | 2 +- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/lifecycle/src/lifecycle_listener.cpp b/lifecycle/src/lifecycle_listener.cpp index 3f87a9083..fdd4d5b1d 100644 --- a/lifecycle/src/lifecycle_listener.cpp +++ b/lifecycle/src/lifecycle_listener.cpp @@ -39,7 +39,7 @@ class LifecycleListener : public rclcpp::Node // Data topic from the lc_talker node sub_data_ = this->create_subscription( "lifecycle_chatter", 10, - std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1)); + [this](std_msgs::msg::String::ConstSharedPtr msg){ return this->data_callback(msg); }); // Notification event topic. All state changes // are published here as TransitionEvents with @@ -47,7 +47,8 @@ class LifecycleListener : public rclcpp::Node sub_notification_ = this->create_subscription( "/lc_talker/transition_event", 10, - std::bind(&LifecycleListener::notification_callback, this, std::placeholders::_1)); + [this](lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg){ return this->notification_callback(msg); } + ); } void data_callback(std_msgs::msg::String::ConstSharedPtr msg) diff --git a/lifecycle/src/lifecycle_service_client.cpp b/lifecycle/src/lifecycle_service_client.cpp index 15fa1b5af..71e48d73c 100644 --- a/lifecycle/src/lifecycle_service_client.cpp +++ b/lifecycle/src/lifecycle_service_client.cpp @@ -328,10 +328,16 @@ int main(int argc, char ** argv) std::shared_future script = std::async( std::launch::async, - std::bind(callee_script, lc_client)); + callee_script, + lc_client + ); + auto wake_exec = std::async( std::launch::async, - std::bind(wake_executor, script, std::ref(exe))); + wake_executor, + script, + std::ref(exe) + ); exe.spin_until_future_complete(script); diff --git a/lifecycle/src/lifecycle_talker.cpp b/lifecycle/src/lifecycle_talker.cpp index e2677dd75..c6042f971 100644 --- a/lifecycle/src/lifecycle_talker.cpp +++ b/lifecycle/src/lifecycle_talker.cpp @@ -116,7 +116,7 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode // available. pub_ = this->create_publisher("lifecycle_chatter", 10); timer_ = this->create_wall_timer( - 1s, std::bind(&LifecycleTalker::publish, this)); + 1s, [this](){ return this->publish(); } ); RCLCPP_INFO(get_logger(), "on_configure() is called."); From 26c72a3e693c3fd180ece21f31a65063e11d7ce4 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 11:43:51 -0300 Subject: [PATCH 4/7] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Logging=20demo=20lambd?= =?UTF-8?q?a=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- logging_demo/src/logger_config_component.cpp | 9 ++++++--- logging_demo/src/logger_usage_component.cpp | 4 ++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/logging_demo/src/logger_config_component.cpp b/logging_demo/src/logger_config_component.cpp index b0c91ee1e..cc4c475bd 100644 --- a/logging_demo/src/logger_config_component.cpp +++ b/logging_demo/src/logger_config_component.cpp @@ -30,9 +30,12 @@ LoggerConfig::LoggerConfig(rclcpp::NodeOptions options) : Node("logger_config", options) { srv_ = create_service( - "config_logger", std::bind( - &LoggerConfig::handle_logger_config_request, - this, std::placeholders::_1, std::placeholders::_2)); + "config_logger", [this]( + const std::shared_ptr request, + std::shared_ptr response){ + return this->handle_logger_config_request(request, response); + } + ); } void diff --git a/logging_demo/src/logger_usage_component.cpp b/logging_demo/src/logger_usage_component.cpp index 721b83d11..2b08bfa28 100644 --- a/logging_demo/src/logger_usage_component.cpp +++ b/logging_demo/src/logger_usage_component.cpp @@ -33,8 +33,8 @@ LoggerUsage::LoggerUsage(rclcpp::NodeOptions options) : Node("logger_usage_demo", options), count_(0) { pub_ = create_publisher("logging_demo_count", 10); - timer_ = create_wall_timer(500ms, std::bind(&LoggerUsage::on_timer, this)); - debug_function_to_evaluate_ = std::bind(is_divisor_of_twelve, std::cref(count_), get_logger()); + timer_ = create_wall_timer(500ms, [this](){ return this->on_timer(); }); + debug_function_to_evaluate_ = [this](){ return is_divisor_of_twelve(std::cref(this->count_), this->get_logger()); }; // After 10 iterations the severity will be set to DEBUG. auto on_one_shot_timer = From 487a525476cd91b1aaa672b93878e94ffd36c150 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 11:44:20 -0300 Subject: [PATCH 5/7] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Image=20tools=20lambda?= =?UTF-8?q?=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- image_tools/src/cam2image.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_tools/src/cam2image.cpp b/image_tools/src/cam2image.cpp index 2e254a992..9d303f59f 100644 --- a/image_tools/src/cam2image.cpp +++ b/image_tools/src/cam2image.cpp @@ -110,7 +110,7 @@ class Cam2Image : public rclcpp::Node // Start main timer loop timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(1000.0 / freq_)), - std::bind(&Cam2Image::timerCallback, this)); + [this](){return this->timerCallback(); } ); } /// Publish camera, or burger, image. From daca209051c9fffe780e63e2c9cbc3efe64031be Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 11:44:40 -0300 Subject: [PATCH 6/7] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20=20Intra=20process=20l?= =?UTF-8?q?ambda=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- intra_process_demo/include/image_pipeline/camera_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/intra_process_demo/include/image_pipeline/camera_node.hpp b/intra_process_demo/include/image_pipeline/camera_node.hpp index 23c61cbd9..db5bae2f3 100644 --- a/intra_process_demo/include/image_pipeline/camera_node.hpp +++ b/intra_process_demo/include/image_pipeline/camera_node.hpp @@ -61,7 +61,7 @@ class CameraNode final : public rclcpp::Node // Create a publisher on the output topic. pub_ = this->create_publisher(output, rclcpp::SensorDataQoS()); // Create the camera reading loop. - thread_ = std::thread(std::bind(&CameraNode::loop, this)); + thread_ = std::thread([this](){return this->loop();}); } ~CameraNode() From e0ad510b4bb7d5ad4260ad0889ea84fa557935bf Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Thu, 14 Dec 2023 13:30:20 -0300 Subject: [PATCH 7/7] =?UTF-8?q?=F0=9F=8E=A8=20Format=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Felipe Gomes de Melo --- .../src/fibonacci_action_client.cpp | 76 +++++++++---------- .../src/fibonacci_action_server.cpp | 44 +++++------ composition/src/client_component.cpp | 2 +- composition/src/talker_component.cpp | 2 +- image_tools/src/cam2image.cpp | 2 +- .../include/image_pipeline/camera_node.hpp | 2 +- lifecycle/src/lifecycle_listener.cpp | 6 +- lifecycle/src/lifecycle_talker.cpp | 2 +- logging_demo/src/logger_config_component.cpp | 8 +- logging_demo/src/logger_usage_component.cpp | 6 +- 10 files changed, 77 insertions(+), 73 deletions(-) diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp index a67806b12..f86dd2280 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp @@ -45,7 +45,7 @@ class FibonacciActionClient : public rclcpp::Node this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), - [this](){ return this->send_goal(); }); + [this]() {return this->send_goal();}); } ACTION_TUTORIALS_CPP_PUBLIC @@ -69,50 +69,50 @@ class FibonacciActionClient : public rclcpp::Node auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = [this]( const GoalHandleFibonacci::SharedPtr & goal_handle) - { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; + { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + }; send_goal_options.feedback_callback = [this]( GoalHandleFibonacci::SharedPtr, const std::shared_ptr feedback) - { - std::stringstream ss; - ss << "Next number in sequence received: "; - for (auto number : feedback->partial_sequence) { - ss << number << " "; - } - RCLCPP_INFO(this->get_logger(), ss.str().c_str()); - }; + { + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + }; send_goal_options.result_callback = [this]( const GoalHandleFibonacci::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; - } - std::stringstream ss; - ss << "Result received: "; - for (auto number : result.result->sequence) { - ss << number << " "; - } - RCLCPP_INFO(this->get_logger(), ss.str().c_str()); - rclcpp::shutdown(); - }; + { + 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; + } + std::stringstream ss; + ss << "Result received: "; + for (auto number : result.result->sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + rclcpp::shutdown(); + }; this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } diff --git a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp index ad9fadd7f..d04c4c332 100644 --- a/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp +++ b/action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp @@ -39,34 +39,34 @@ class FibonacciActionServer : public rclcpp::Node auto handle_goal = [this]( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) - { - (void)uuid; - RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); - // The Fibonacci action uses int32 for the return of sequences, which means it can only - // hold 2^31-1 (2147483647) before wrapping negative in two's complement. Based on empirical - // tests, that means that an order of > 46 will cause wrapping, so we don't allow that here. - if (goal->order > 46) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + { + (void)uuid; + RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); + // The Fibonacci action uses int32 for the return of sequences, which means it can only hold + // 2^31-1 (2147483647) before wrapping negative in two's complement. Based on empirical + // tests, that means that an order of > 46 will cause wrapping, so we don't allow that here. + if (goal->order > 46) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; auto handle_cancel = [this]( const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - }; + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + }; auto handle_accepted = [this]( const std::shared_ptr goal_handle) - { - // this needs to return quickly to avoid blocking the executor, - // so we declare a lambda function to be called inside a new thread - auto execute_in_thread = [this, goal_handle](){return this->execute(goal_handle);}; - std::thread{execute_in_thread}.detach(); - }; + { + // this needs to return quickly to avoid blocking the executor, + // so we declare a lambda function to be called inside a new thread + auto execute_in_thread = [this, goal_handle]() {return this->execute(goal_handle);}; + std::thread{execute_in_thread}.detach(); + }; this->action_server_ = rclcpp_action::create_server( this, diff --git a/composition/src/client_component.cpp b/composition/src/client_component.cpp index a5033d786..08251d04e 100644 --- a/composition/src/client_component.cpp +++ b/composition/src/client_component.cpp @@ -33,7 +33,7 @@ Client::Client(const rclcpp::NodeOptions & options) // Note(dhood): The timer period must be greater than the duration of the timer callback. // Otherwise, the timer can starve a single-threaded executor. // See https://github.com/ros2/rclcpp/issues/392 for updates. - timer_ = create_wall_timer(2s, [this](){return this->on_timer();}); + timer_ = create_wall_timer(2s, [this]() {return this->on_timer();}); } void Client::on_timer() diff --git a/composition/src/talker_component.cpp b/composition/src/talker_component.cpp index a05c4bbc5..ae0fa4983 100644 --- a/composition/src/talker_component.cpp +++ b/composition/src/talker_component.cpp @@ -37,7 +37,7 @@ Talker::Talker(const rclcpp::NodeOptions & options) pub_ = create_publisher("chatter", 10); // Use a timer to schedule periodic message publishing. - timer_ = create_wall_timer(1s, [this](){ return this->on_timer(); }); + timer_ = create_wall_timer(1s, [this]() {return this->on_timer();}); } void Talker::on_timer() diff --git a/image_tools/src/cam2image.cpp b/image_tools/src/cam2image.cpp index 9d303f59f..0e4289f42 100644 --- a/image_tools/src/cam2image.cpp +++ b/image_tools/src/cam2image.cpp @@ -110,7 +110,7 @@ class Cam2Image : public rclcpp::Node // Start main timer loop timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(1000.0 / freq_)), - [this](){return this->timerCallback(); } ); + [this]() {return this->timerCallback();}); } /// Publish camera, or burger, image. diff --git a/intra_process_demo/include/image_pipeline/camera_node.hpp b/intra_process_demo/include/image_pipeline/camera_node.hpp index db5bae2f3..66d2b4e7f 100644 --- a/intra_process_demo/include/image_pipeline/camera_node.hpp +++ b/intra_process_demo/include/image_pipeline/camera_node.hpp @@ -61,7 +61,7 @@ class CameraNode final : public rclcpp::Node // Create a publisher on the output topic. pub_ = this->create_publisher(output, rclcpp::SensorDataQoS()); // Create the camera reading loop. - thread_ = std::thread([this](){return this->loop();}); + thread_ = std::thread([this]() {return this->loop();}); } ~CameraNode() diff --git a/lifecycle/src/lifecycle_listener.cpp b/lifecycle/src/lifecycle_listener.cpp index fdd4d5b1d..1dd4da440 100644 --- a/lifecycle/src/lifecycle_listener.cpp +++ b/lifecycle/src/lifecycle_listener.cpp @@ -39,7 +39,7 @@ class LifecycleListener : public rclcpp::Node // Data topic from the lc_talker node sub_data_ = this->create_subscription( "lifecycle_chatter", 10, - [this](std_msgs::msg::String::ConstSharedPtr msg){ return this->data_callback(msg); }); + [this](std_msgs::msg::String::ConstSharedPtr msg) {return this->data_callback(msg);}); // Notification event topic. All state changes // are published here as TransitionEvents with @@ -47,7 +47,9 @@ class LifecycleListener : public rclcpp::Node sub_notification_ = this->create_subscription( "/lc_talker/transition_event", 10, - [this](lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg){ return this->notification_callback(msg); } + [this](lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg) { + return this->notification_callback(msg); + } ); } diff --git a/lifecycle/src/lifecycle_talker.cpp b/lifecycle/src/lifecycle_talker.cpp index c6042f971..aeb2279bc 100644 --- a/lifecycle/src/lifecycle_talker.cpp +++ b/lifecycle/src/lifecycle_talker.cpp @@ -116,7 +116,7 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode // available. pub_ = this->create_publisher("lifecycle_chatter", 10); timer_ = this->create_wall_timer( - 1s, [this](){ return this->publish(); } ); + 1s, [this]() {return this->publish();}); RCLCPP_INFO(get_logger(), "on_configure() is called."); diff --git a/logging_demo/src/logger_config_component.cpp b/logging_demo/src/logger_config_component.cpp index cc4c475bd..fc442ab0c 100644 --- a/logging_demo/src/logger_config_component.cpp +++ b/logging_demo/src/logger_config_component.cpp @@ -32,10 +32,10 @@ LoggerConfig::LoggerConfig(rclcpp::NodeOptions options) srv_ = create_service( "config_logger", [this]( const std::shared_ptr request, - std::shared_ptr response){ - return this->handle_logger_config_request(request, response); - } - ); + std::shared_ptr response) { + return this->handle_logger_config_request(request, response); + } + ); } void diff --git a/logging_demo/src/logger_usage_component.cpp b/logging_demo/src/logger_usage_component.cpp index 2b08bfa28..d2b034266 100644 --- a/logging_demo/src/logger_usage_component.cpp +++ b/logging_demo/src/logger_usage_component.cpp @@ -33,8 +33,10 @@ LoggerUsage::LoggerUsage(rclcpp::NodeOptions options) : Node("logger_usage_demo", options), count_(0) { pub_ = create_publisher("logging_demo_count", 10); - timer_ = create_wall_timer(500ms, [this](){ return this->on_timer(); }); - debug_function_to_evaluate_ = [this](){ return is_divisor_of_twelve(std::cref(this->count_), this->get_logger()); }; + timer_ = create_wall_timer(500ms, [this]() {return this->on_timer();}); + debug_function_to_evaluate_ = [this]() { + return is_divisor_of_twelve(std::cref(this->count_), this->get_logger()); + }; // After 10 iterations the severity will be set to DEBUG. auto on_one_shot_timer =