Skip to content

Commit

Permalink
Migrate std::bind calls to lambda expressions (#659)
Browse files Browse the repository at this point in the history
* ♻️ Actions tutorial lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>

* ♻️ Composition nodes lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>

* ♻️ Lifecycle nodes lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>

* ♻️ Logging demo lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>

* ♻️ Image tools lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>

* ♻️  Intra process lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>

* 🎨 Format code

Signed-off-by: Felipe Gomes de Melo <[email protected]>

---------

Signed-off-by: Felipe Gomes de Melo <[email protected]>
  • Loading branch information
FelipeGdM authored Dec 26, 2023
1 parent 2864e1d commit 9918a14
Show file tree
Hide file tree
Showing 12 changed files with 130 additions and 121 deletions.
29 changes: 18 additions & 11 deletions action_tutorials/action_tutorials_cpp/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,17 @@ In the constructor for `FibonacciActionServer`, an action server is created with

```cpp
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
...
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;
Expand All @@ -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.
Expand All @@ -51,12 +54,16 @@ The goal is sent asynchronously with callbacks registered for the goal response,

```cpp
auto send_goal_options = rclcpp_action::Client<Fibonacci>::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<const Fibonacci::Feedback> feedback)
{...};
send_goal_options.result_callback = [this](
const GoalHandleFibonacci::WrappedResult & result)
{...};
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
```
Expand Down
104 changes: 48 additions & 56 deletions action_tutorials/action_tutorials_cpp/src/fibonacci_action_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -67,67 +67,59 @@ class FibonacciActionClient : public rclcpp::Node
RCLCPP_INFO(this->get_logger(), "Sending goal");

auto send_goal_options = rclcpp_action::Client<Fibonacci>::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<const Fibonacci::Feedback> 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<Fibonacci>::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<const Fibonacci::Feedback> 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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<const Fibonacci::Goal> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> 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<Fibonacci>(
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<Fibonacci>::SharedPtr action_server_;

ACTION_TUTORIALS_CPP_LOCAL
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> goal_handle)
{
Expand Down
2 changes: 1 addition & 1 deletion composition/src/client_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
2 changes: 1 addition & 1 deletion composition/src/talker_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ Talker::Talker(const rclcpp::NodeOptions & options)
pub_ = create_publisher<std_msgs::msg::String>("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()
Expand Down
2 changes: 1 addition & 1 deletion image_tools/src/cam2image.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ class Cam2Image : public rclcpp::Node
// Start main timer loop
timer_ = this->create_wall_timer(
std::chrono::milliseconds(static_cast<int>(1000.0 / freq_)),
std::bind(&Cam2Image::timerCallback, this));
[this]() {return this->timerCallback();});
}

/// Publish camera, or burger, image.
Expand Down
2 changes: 1 addition & 1 deletion intra_process_demo/include/image_pipeline/camera_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class CameraNode final : public rclcpp::Node
// Create a publisher on the output topic.
pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, rclcpp::SensorDataQoS());
// Create the camera reading loop.
thread_ = std::thread(std::bind(&CameraNode::loop, this));
thread_ = std::thread([this]() {return this->loop();});
}

~CameraNode()
Expand Down
7 changes: 5 additions & 2 deletions lifecycle/src/lifecycle_listener.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,18 @@ class LifecycleListener : public rclcpp::Node
// Data topic from the lc_talker node
sub_data_ = this->create_subscription<std_msgs::msg::String>(
"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
// a start and goal state indicating the transition
sub_notification_ = this->create_subscription<lifecycle_msgs::msg::TransitionEvent>(
"/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)
Expand Down
10 changes: 8 additions & 2 deletions lifecycle/src/lifecycle_service_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -328,10 +328,16 @@ int main(int argc, char ** argv)

std::shared_future<void> 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);

Expand Down
2 changes: 1 addition & 1 deletion lifecycle/src/lifecycle_talker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode
// available.
pub_ = this->create_publisher<std_msgs::msg::String>("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.");

Expand Down
9 changes: 6 additions & 3 deletions logging_demo/src/logger_config_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,12 @@ LoggerConfig::LoggerConfig(rclcpp::NodeOptions options)
: Node("logger_config", options)
{
srv_ = create_service<logging_demo::srv::ConfigLogger>(
"config_logger", std::bind(
&LoggerConfig::handle_logger_config_request,
this, std::placeholders::_1, std::placeholders::_2));
"config_logger", [this](
const std::shared_ptr<logging_demo::srv::ConfigLogger::Request> request,
std::shared_ptr<logging_demo::srv::ConfigLogger::Response> response) {
return this->handle_logger_config_request(request, response);
}
);
}

void
Expand Down
6 changes: 4 additions & 2 deletions logging_demo/src/logger_usage_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,10 @@ LoggerUsage::LoggerUsage(rclcpp::NodeOptions options)
: Node("logger_usage_demo", options), count_(0)
{
pub_ = create_publisher<std_msgs::msg::String>("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 =
Expand Down

0 comments on commit 9918a14

Please sign in to comment.