Skip to content

Commit

Permalink
Migrate std::bind calls to lambda expressions (#4041)
Browse files Browse the repository at this point in the history
* Simple cpp pub/sub page lambda refactor

* Callback groups page lambda refactor

* Fast dds config page lambda refactor

* Custom interfaces lambda refactor

* Params in class lambda refactor

* tf2 add frame lambda refactor

* Action server lambda refactor

* Tf2 broadcaster lambda refactor

* Action server/client lambda refactor

* Read/record bagfile cpp lambda refactor

* tf2 listener lambda refactor

* Custom ROS 2 interfaces lambda refactor

* Cpp parameters class lambda refactor

* Webots basic tutorial lambda refactor

* Webots advanced tutorial lambda refactor

Signed-off-by: Felipe Gomes de Melo <[email protected]>
Co-authored-by: Tyler Weaver <[email protected]>
Co-authored-by: Chris Lalancette <[email protected]>
  • Loading branch information
3 people authored Jan 12, 2024
1 parent d412eee commit 913f2b6
Show file tree
Hide file tree
Showing 18 changed files with 382 additions and 398 deletions.
46 changes: 23 additions & 23 deletions source/How-To-Guides/Using-callback-groups.rst
Original file line number Diff line number Diff line change
Expand Up @@ -210,25 +210,25 @@ We have two nodes - one providing a simple service:
public:
ServiceNode() : Node("service_node")
{
auto service_callback = [this](
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> response)
{
(void)request_header;
(void)request;
(void)response;
RCLCPP_INFO(this->get_logger(), "Received request, responding...");
};
service_ptr_ = this->create_service<std_srvs::srv::Empty>(
"test_service",
std::bind(&ServiceNode::service_callback, this, _1, _2, _3)
service_callback
);
}
private:
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_ptr_;
void service_callback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> response)
{
(void)request_header;
(void)request;
(void)response;
RCLCPP_INFO(this->get_logger(), "Received request, responding...");
}
}; // class ServiceNode
} // namespace cb_group_demo
Expand Down Expand Up @@ -306,8 +306,18 @@ service calls:
timer_cb_group_ = nullptr;
client_ptr_ = this->create_client<std_srvs::srv::Empty>("test_service", rmw_qos_profile_services_default,
client_cb_group_);
timer_ptr_ = this->create_wall_timer(1s, std::bind(&DemoNode::timer_callback, this),
timer_cb_group_);
auto timer_callback = [this](){
RCLCPP_INFO(this->get_logger(), "Sending request");
auto request = std::make_shared<std_srvs::srv::Empty::Request>();
auto result_future = client_ptr_->async_send_request(request);
std::future_status status = result_future.wait_for(10s); // timeout to guarantee a graceful finish
if (status == std::future_status::ready) {
RCLCPP_INFO(this->get_logger(), "Received response");
}
};
timer_ptr_ = this->create_wall_timer(1s, timer_callback, timer_cb_group_);
}
private:
Expand All @@ -316,16 +326,6 @@ service calls:
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_ptr_;
void timer_callback()
{
RCLCPP_INFO(this->get_logger(), "Sending request");
auto request = std::make_shared<std_srvs::srv::Empty::Request>();
auto result_future = client_ptr_->async_send_request(request);
std::future_status status = result_future.wait_for(10s); // timeout to guarantee a graceful finish
if (status == std::future_status::ready) {
RCLCPP_INFO(this->get_logger(), "Received response");
}
}
}; // class DemoNode
} // namespace cb_group_demo
Expand Down
69 changes: 30 additions & 39 deletions source/Tutorials/Advanced/FastDDS-Configuration.rst
Original file line number Diff line number Diff line change
Expand Up @@ -112,41 +112,38 @@ Note that the synchronous publisher will be publishing on topic ``sync_topic``,
// Create the asynchronous publisher on topic 'async_topic'
async_publisher_ = this->create_publisher<std_msgs::msg::String>("async_topic", 10);

// This timer will trigger the publication of new data every half a second
timer_ = this->create_wall_timer(
500ms, std::bind(&SyncAsyncPublisher::timer_callback, this));
}
// Actions to run every time the timer expires
auto timer_callback = [this](){

private:
/**
* Actions to run every time the timer expires
*/
void timer_callback()
{
// Create a new message to be sent
auto sync_message = std_msgs::msg::String();
sync_message.data = "SYNC: Hello, world! " + std::to_string(count_);
// Create a new message to be sent
auto sync_message = std_msgs::msg::String();
sync_message.data = "SYNC: Hello, world! " + std::to_string(count_);

// Log the message to the console to show progress
RCLCPP_INFO(this->get_logger(), "Synchronously publishing: '%s'", sync_message.data.c_str());
// Log the message to the console to show progress
RCLCPP_INFO(this->get_logger(), "Synchronously publishing: '%s'", sync_message.data.c_str());

// Publish the message using the synchronous publisher
sync_publisher_->publish(sync_message);
// Publish the message using the synchronous publisher
sync_publisher_->publish(sync_message);

// Create a new message to be sent
auto async_message = std_msgs::msg::String();
async_message.data = "ASYNC: Hello, world! " + std::to_string(count_);
// Create a new message to be sent
auto async_message = std_msgs::msg::String();
async_message.data = "ASYNC: Hello, world! " + std::to_string(count_);

// Log the message to the console to show progress
RCLCPP_INFO(this->get_logger(), "Asynchronously publishing: '%s'", async_message.data.c_str());
// Log the message to the console to show progress
RCLCPP_INFO(this->get_logger(), "Asynchronously publishing: '%s'", async_message.data.c_str());

// Publish the message using the asynchronous publisher
async_publisher_->publish(async_message);
// Publish the message using the asynchronous publisher
async_publisher_->publish(async_message);

// Prepare the count for the next message
count_++;
// Prepare the count for the next message
count_++;
};

// This timer will trigger the publication of new data every half a second
timer_ = this->create_wall_timer(500ms, timer_callback);
}

private:
// This timer will trigger the publication of new data every half a second
rclcpp::TimerBase::SharedPtr timer_;

Expand Down Expand Up @@ -322,42 +319,36 @@ In a new source file named ``src/sync_async_reader.cpp`` write the following con

.. code-block:: C++

#include <functional>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

class SyncAsyncSubscriber : public rclcpp::Node
{
public:

SyncAsyncSubscriber()
: Node("sync_async_subscriber")
{
// Lambda function to run every time a new message is received
auto topic_callback = [this](const std_msgs::msg::String & msg){
RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());
};

// Create the synchronous subscriber on topic 'sync_topic'
// and tie it to the topic_callback
sync_subscription_ = this->create_subscription<std_msgs::msg::String>(
"sync_topic", 10, std::bind(&SyncAsyncSubscriber::topic_callback, this, _1));
"sync_topic", 10, topic_callback);

// Create the asynchronous subscriber on topic 'async_topic'
// and tie it to the topic_callback
async_subscription_ = this->create_subscription<std_msgs::msg::String>(
"async_topic", 10, std::bind(&SyncAsyncSubscriber::topic_callback, this, _1));
"async_topic", 10, topic_callback);
}

private:

/**
* Actions to run every time a new message is received
*/
void topic_callback(const std_msgs::msg::String & msg) const
{
RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());
}
// A subscriber that listens to topic 'sync_topic'
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sync_subscription_;

Expand Down
12 changes: 8 additions & 4 deletions source/Tutorials/Advanced/Reading-From-A-Bag-File-CPP.rst
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,10 @@ Inside your package's ``src`` directory, create a new file called ``simple_bag_r
: Node("playback_node")
{
publisher_ = this->create_publisher<turtlesim::msg::Pose>("/turtle1/pose", 10);
timer_ = this->create_wall_timer(
100ms, std::bind(&PlaybackNode::timer_callback, this));

timer_ = this->create_wall_timer(100ms,
[this](){return this->timer_callback();}
);

reader_.open(bag_filename);
}
Expand Down Expand Up @@ -158,8 +160,10 @@ Note the constructor takes a path to the bag file as a parameter.
: Node("playback_node")
{
publisher_ = this->create_publisher<turtlesim::msg::Pose>("/turtle1/pose", 10);
timer_ = this->create_wall_timer(
100ms, std::bind(&PlaybackNode::timer_callback, this));

timer_ = this->create_wall_timer(100ms,
[this](){return this->timer_callback();}
);

We also open the bag in the constructor.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,6 @@ Inside the ``ros2_ws/src/bag_recorder_nodes/src`` directory, create a new file c

#include <rosbag2_cpp/writer.hpp>

using std::placeholders::_1;

class SimpleBagRecorder : public rclcpp::Node
{
public:
Expand All @@ -96,17 +94,17 @@ Inside the ``ros2_ws/src/bag_recorder_nodes/src`` directory, create a new file c

writer_->open("my_bag");

auto subscription_callback_lambda = [this](std::shared_ptr<rclcpp::SerializedMessage> msg){
rclcpp::Time time_stamp = this->now();

writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);
};

subscription_ = create_subscription<std_msgs::msg::String>(
"chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));
"chatter", 10, subscription_callback_lambda);
}

private:
void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
{
rclcpp::Time time_stamp = this->now();

writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);
}

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
std::unique_ptr<rosbag2_cpp::Writer> writer_;
Expand Down Expand Up @@ -146,8 +144,14 @@ We will write data to the bag in the callback.

.. code-block:: C++

auto subscription_callback_lambda = [this](std::shared_ptr<rclcpp::SerializedMessage> msg){
rclcpp::Time time_stamp = this->now();

writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);
};

subscription_ = create_subscription<std_msgs::msg::String>(
"chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));
"chatter", 10, subscription_callback_lambda);

The callback itself is different from a typical callback.
Rather than receiving an instance of the data type of the topic, we instead receive a ``rclcpp::SerializedMessage``.
Expand All @@ -158,8 +162,7 @@ We do this for two reasons.

.. code-block:: C++

void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
{
auto subscription_callback_lambda = [this](std::shared_ptr<rclcpp::SerializedMessage> msg){

Within the subscription callback, the first thing to do is determine the time stamp to use for the stored message.
This can be anything appropriate to your data, but two common values are the time at which the data was produced, if known, and the time it is received.
Expand Down Expand Up @@ -349,7 +352,8 @@ Inside the ``ros2_ws/src/bag_recorder_nodes/src`` directory, create a new file c
rmw_get_serialization_format(),
""});

timer_ = create_wall_timer(1s, std::bind(&DataGenerator::timer_callback, this));
auto timer_callback_lambda = [this](){return this->timer_callback();};
timer_ = create_wall_timer(1s, timer_callback_lambda);
}

private:
Expand Down Expand Up @@ -401,7 +405,8 @@ The timer fires with a one-second period, and calls the given member function wh

.. code-block:: C++

timer_ = create_wall_timer(1s, std::bind(&DataGenerator::timer_callback, this));
auto timer_callback_lambda = [this](){return this->timer_callback();};
timer_ = create_wall_timer(1s, timer_callback_lambda);

Within the timer callback, we generate (or otherwise obtain, e.g. read from a serial port connected to some hardware) the data we wish to store in the bag.
The important difference between this and the previous sample is that the data is not yet serialised.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,11 @@ void MyRobotDriver::init(

cmd_vel_subscription_ = node->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", rclcpp::SensorDataQoS().reliable(),
std::bind(&MyRobotDriver::cmdVelCallback, this, std::placeholders::_1));
}

void MyRobotDriver::cmdVelCallback(
const geometry_msgs::msg::Twist::SharedPtr msg) {
cmd_vel_msg.linear = msg->linear;
cmd_vel_msg.angular = msg->angular;
[this](const geometry_msgs::msg::Twist::SharedPtr msg){
this->cmd_vel_msg.linear = msg->linear;
this->cmd_vel_msg.angular = msg->angular;
}
);
}

void MyRobotDriver::step() {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@ class MyRobotDriver : public webots_ros2_driver::PluginInterface {
std::unordered_map<std::string, std::string> &parameters) override;

private:
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);

rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
cmd_vel_subscription_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,17 @@ ObstacleAvoider::ObstacleAvoider() : Node("obstacle_avoider") {

left_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
"/left_sensor", 1,
std::bind(&ObstacleAvoider::leftSensorCallback, this,
std::placeholders::_1));
[this](const sensor_msgs::msg::Range::SharedPtr msg){
return this->leftSensorCallback(msg);
}
);

right_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
"/right_sensor", 1,
std::bind(&ObstacleAvoider::rightSensorCallback, this,
std::placeholders::_1));
[this](const sensor_msgs::msg::Range::SharedPtr msg){
return this->rightSensorCallback(msg);
}
);
}

void ObstacleAvoider::leftSensorCallback(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,21 +108,21 @@ In addition to your custom plugin, the ``webots_ros2_driver`` will parse the ``<

.. literalinclude:: Code/ObstacleAvoider.cpp
:language: cpp
:lines: 6-16
:lines: 6-20

When a measurement is received from the left sensor it will be copied to a member field:

.. literalinclude:: Code/ObstacleAvoider.cpp
:language: cpp
:lines: 19-22
:lines: 23-26

Finally, a message will be sent to the ``/cmd_vel`` topic when a measurement from the right sensor is received.
The ``command_message`` will register at least a forward speed in ``linear.x`` in order to make the robot move when no obstacle is detected.
If any of the two sensors detect an obstacle, ``command_message`` will also register a rotational speed in ``angular.z`` in order to make the robot turn right.

.. literalinclude:: Code/ObstacleAvoider.cpp
:language: cpp
:lines: 24-38
:lines: 28-42


3 Updating additional files
Expand Down
Loading

0 comments on commit 913f2b6

Please sign in to comment.