diff --git a/source/How-To-Guides/Using-callback-groups.rst b/source/How-To-Guides/Using-callback-groups.rst index 460797c3abf..b41068eaa04 100644 --- a/source/How-To-Guides/Using-callback-groups.rst +++ b/source/How-To-Guides/Using-callback-groups.rst @@ -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 request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + (void)response; + RCLCPP_INFO(this->get_logger(), "Received request, responding..."); + }; service_ptr_ = this->create_service( "test_service", - std::bind(&ServiceNode::service_callback, this, _1, _2, _3) + service_callback ); } private: rclcpp::Service::SharedPtr service_ptr_; - void service_callback( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) - { - (void)request_header; - (void)request; - (void)response; - RCLCPP_INFO(this->get_logger(), "Received request, responding..."); - } }; // class ServiceNode } // namespace cb_group_demo @@ -306,8 +306,18 @@ service calls: timer_cb_group_ = nullptr; client_ptr_ = this->create_client("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(); + 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: @@ -316,16 +326,6 @@ service calls: rclcpp::Client::SharedPtr client_ptr_; rclcpp::TimerBase::SharedPtr timer_ptr_; - void timer_callback() - { - RCLCPP_INFO(this->get_logger(), "Sending request"); - auto request = std::make_shared(); - 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 diff --git a/source/Tutorials/Advanced/FastDDS-Configuration.rst b/source/Tutorials/Advanced/FastDDS-Configuration.rst index 19c3e6ffa51..fc9e021761d 100644 --- a/source/Tutorials/Advanced/FastDDS-Configuration.rst +++ b/source/Tutorials/Advanced/FastDDS-Configuration.rst @@ -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("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_; @@ -322,14 +319,11 @@ In a new source file named ``src/sync_async_reader.cpp`` write the following con .. code-block:: C++ - #include #include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" - using std::placeholders::_1; - class SyncAsyncSubscriber : public rclcpp::Node { public: @@ -337,27 +331,24 @@ In a new source file named ``src/sync_async_reader.cpp`` write the following con 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( - "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( - "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::SharedPtr sync_subscription_; diff --git a/source/Tutorials/Advanced/Reading-From-A-Bag-File-CPP.rst b/source/Tutorials/Advanced/Reading-From-A-Bag-File-CPP.rst index 1f66fedb474..088f612c354 100644 --- a/source/Tutorials/Advanced/Reading-From-A-Bag-File-CPP.rst +++ b/source/Tutorials/Advanced/Reading-From-A-Bag-File-CPP.rst @@ -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("/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); } @@ -158,8 +160,10 @@ Note the constructor takes a path to the bag file as a parameter. : Node("playback_node") { publisher_ = this->create_publisher("/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. diff --git a/source/Tutorials/Advanced/Recording-A-Bag-From-Your-Own-Node-CPP.rst b/source/Tutorials/Advanced/Recording-A-Bag-From-Your-Own-Node-CPP.rst index 6518beb8de9..572a4a5369e 100644 --- a/source/Tutorials/Advanced/Recording-A-Bag-From-Your-Own-Node-CPP.rst +++ b/source/Tutorials/Advanced/Recording-A-Bag-From-Your-Own-Node-CPP.rst @@ -84,8 +84,6 @@ Inside the ``ros2_ws/src/bag_recorder_nodes/src`` directory, create a new file c #include - using std::placeholders::_1; - class SimpleBagRecorder : public rclcpp::Node { public: @@ -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 msg){ + rclcpp::Time time_stamp = this->now(); + + writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp); + }; + subscription_ = create_subscription( - "chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1)); + "chatter", 10, subscription_callback_lambda); } private: - void topic_callback(std::shared_ptr msg) const - { - rclcpp::Time time_stamp = this->now(); - - writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp); - } rclcpp::Subscription::SharedPtr subscription_; std::unique_ptr writer_; @@ -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 msg){ + rclcpp::Time time_stamp = this->now(); + + writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp); + }; + subscription_ = create_subscription( - "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``. @@ -158,8 +162,7 @@ We do this for two reasons. .. code-block:: C++ - void topic_callback(std::shared_ptr msg) const - { + auto subscription_callback_lambda = [this](std::shared_ptr 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. @@ -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: @@ -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. diff --git a/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.cpp b/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.cpp index 56fdf7a64a5..dab1253033a 100644 --- a/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.cpp +++ b/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.cpp @@ -25,13 +25,11 @@ void MyRobotDriver::init( cmd_vel_subscription_ = node->create_subscription( "/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() { diff --git a/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.hpp b/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.hpp index 8868a145916..bf1d3c9d3eb 100644 --- a/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.hpp +++ b/source/Tutorials/Advanced/Simulators/Webots/Code/MyRobotDriver.hpp @@ -16,7 +16,6 @@ class MyRobotDriver : public webots_ros2_driver::PluginInterface { std::unordered_map ¶meters) override; private: - void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); rclcpp::Subscription::SharedPtr cmd_vel_subscription_; diff --git a/source/Tutorials/Advanced/Simulators/Webots/Code/ObstacleAvoider.cpp b/source/Tutorials/Advanced/Simulators/Webots/Code/ObstacleAvoider.cpp index b584975e63b..530f4baa192 100644 --- a/source/Tutorials/Advanced/Simulators/Webots/Code/ObstacleAvoider.cpp +++ b/source/Tutorials/Advanced/Simulators/Webots/Code/ObstacleAvoider.cpp @@ -7,13 +7,17 @@ ObstacleAvoider::ObstacleAvoider() : Node("obstacle_avoider") { left_sensor_sub_ = create_subscription( "/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( "/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( diff --git a/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Advanced.rst b/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Advanced.rst index cac545fa34b..1d3c64b3e7d 100644 --- a/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Advanced.rst +++ b/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Advanced.rst @@ -108,13 +108,13 @@ 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. @@ -122,7 +122,7 @@ In addition to your custom plugin, the ``webots_ros2_driver`` will parse the ``< .. literalinclude:: Code/ObstacleAvoider.cpp :language: cpp - :lines: 24-38 + :lines: 28-42 3 Updating additional files diff --git a/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Basic.rst b/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Basic.rst index 376f77b788f..a8645eb50c5 100644 --- a/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Basic.rst +++ b/source/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Basic.rst @@ -245,13 +245,13 @@ You can use it to access the `Webots robot API create_publisher("topic", 10); // CHANGE - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); + + auto timer_callback = [this](){ + auto message = tutorial_interfaces::msg::Num(); // CHANGE + message.num = this->count_++; // CHANGE + RCLCPP_INFO_STREAM(this->get_logger(), "Publishing: '" << message.num << "'"); // CHANGE + publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); } private: - void timer_callback() - { - auto message = tutorial_interfaces::msg::Num(); // CHANGE - message.num = this->count_++; // CHANGE - RCLCPP_INFO_STREAM(this->get_logger(), "Publishing: '" << message.num << "'"); // CHANGE - publisher_->publish(message); - } rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr publisher_; // CHANGE size_t count_; @@ -355,15 +354,14 @@ Since you'll be changing the standard string msg to a numerical one, the output MinimalSubscriber() : Node("minimal_subscriber") { + auto topic_callback = [this](const tutorial_interfaces::msg::Num & msg){ // CHANGE + RCLCPP_INFO_STREAM(this->get_logger(), "I heard: '" << msg.num << "'"); // CHANGE + }; subscription_ = this->create_subscription( // CHANGE - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + "topic", 10, topic_callback); } private: - void topic_callback(const tutorial_interfaces::msg::Num & msg) const // CHANGE - { - RCLCPP_INFO_STREAM(this->get_logger(), "I heard: '" << msg.num << "'"); // CHANGE - } rclcpp::Subscription::SharedPtr subscription_; // CHANGE }; diff --git a/source/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.rst b/source/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.rst index 3a1805783d3..128c6087b8a 100644 --- a/source/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.rst +++ b/source/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.rst @@ -87,18 +87,15 @@ Inside the ``ros2_ws/src/cpp_parameters/src`` directory, create a new file calle { this->declare_parameter("my_parameter", "world"); - timer_ = this->create_wall_timer( - 1000ms, std::bind(&MinimalParam::timer_callback, this)); - } - - void timer_callback() - { - std::string my_param = this->get_parameter("my_parameter").as_string(); + auto timer_callback = [this](){ + std::string my_param = this->get_parameter("my_parameter").as_string(); - RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str()); + RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str()); - std::vector all_new_parameters{rclcpp::Parameter("my_parameter", "world")}; - this->set_parameters(all_new_parameters); + std::vector all_new_parameters{rclcpp::Parameter("my_parameter", "world")}; + this->set_parameters(all_new_parameters); + }; + timer_ = this->create_wall_timer(1000ms, timer_callback); } private: @@ -120,7 +117,13 @@ The ``#include`` statements at the top are the package dependencies. The next piece of code creates the class and the constructor. The first line of this constructor creates a parameter with the name ``my_parameter`` and a default value of ``world``. The parameter type is inferred from the default value, so in this case it would be set to a string type. -Next the ``timer_`` is initialized with a period of 1000ms, which causes the ``timer_callback`` function to be executed once a second. +Next, a `lambda function `_ called ``timer_callback`` is declared. +It performs a by-reference capture of the current object ``this``, takes no input arguments and returns void. +The first line of our ``timer_callback`` function gets the parameter ``my_parameter`` from the node, and stores it in ``my_param``. +Then the ``RCLCPP_INFO`` function ensures the event is logged. +The ``set_parameters`` function sets the parameter ``my_parameter`` back to the default string value ``world``. +In the case that the user changed the parameter externally, this ensures it is always reset back to the original. +In the end, ``timer_`` is initialized with a period of 1000ms, which causes the ``timer_callback`` function to be executed once a second. .. code-block:: C++ @@ -132,26 +135,16 @@ Next the ``timer_`` is initialized with a period of 1000ms, which causes the ``t { this->declare_parameter("my_parameter", "world"); - timer_ = this->create_wall_timer( - 1000ms, std::bind(&MinimalParam::timer_callback, this)); - } - -The first line of our ``timer_callback`` function gets the parameter ``my_parameter`` from the node, and stores it in ``my_param``. -Next the ``RCLCPP_INFO`` function ensures the event is logged. -The ``set_parameters`` function then sets the parameter ``my_parameter`` back to the default string value ``world``. -In the case that the user changed the parameter externally, this ensures it is always reset back to the original. - -.. code-block:: C++ + auto timer_callback = [this](){ + std::string my_param = this->get_parameter("my_parameter").as_string(); - void timer_callback() - { - std::string my_param = this->get_parameter("my_parameter").as_string(); + RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str()); - RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str()); - - std::vector all_new_parameters{rclcpp::Parameter("my_parameter", "world")}; - this->set_parameters(all_new_parameters); - } + std::vector all_new_parameters{rclcpp::Parameter("my_parameter", "world")}; + this->set_parameters(all_new_parameters); + }; + timer_ = this->create_wall_timer(1000ms, timer_callback); + } Last is the declaration of ``timer_``. @@ -194,8 +187,16 @@ For that to work, the code in the constructor has to be changed to: this->declare_parameter("my_parameter", "world", param_desc); - timer_ = this->create_wall_timer( - 1000ms, std::bind(&MinimalParam::timer_callback, this)); + auto timer_callback = [this](){ + std::string my_param = this->get_parameter("my_parameter").as_string(); + + RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str()); + + std::vector all_new_parameters{rclcpp::Parameter("my_parameter", "world")}; + this->set_parameters(all_new_parameters); + }; + timer_ = this->create_wall_timer(1000ms, timer_callback); + } The rest of the code remains the same. diff --git a/source/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.rst b/source/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.rst index 99eabf64949..fddc850248b 100644 --- a/source/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.rst +++ b/source/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.rst @@ -65,13 +65,13 @@ Download the example talker code by entering the following command: .. code-block:: console - wget -O publisher_member_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/member_function.cpp + wget -O publisher_lambda_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/lambda.cpp .. group-tab:: macOS .. code-block:: console - wget -O publisher_member_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/member_function.cpp + wget -O publisher_lambda_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/lambda.cpp .. group-tab:: Windows @@ -79,21 +79,20 @@ Download the example talker code by entering the following command: .. code-block:: console - curl -sk https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/member_function.cpp -o publisher_member_function.cpp + curl -sk https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/lambda.cpp -o publisher_lambda_function.cpp Or in powershell: .. code-block:: console - curl https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/member_function.cpp -o publisher_member_function.cpp + curl https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_publisher/lambda.cpp -o publisher_lambda_function.cpp -Now there will be a new file named ``publisher_member_function.cpp``. +Now there will be a new file named ``publisher_lambda_function.cpp``. Open the file using your preferred text editor. .. code-block:: C++ #include - #include #include #include @@ -102,31 +101,31 @@ Open the file using your preferred text editor. using namespace std::chrono_literals; - /* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ + /* This example creates a subclass of Node and uses a fancy C++11 lambda + * function to shorten the callback syntax, at the expense of making the + * code somewhat more difficult to understand at first glance. */ class MinimalPublisher : public rclcpp::Node { - public: - MinimalPublisher() - : Node("minimal_publisher"), count_(0) - { - publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); - } - - private: - void timer_callback() - { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; + public: + MinimalPublisher() + : Node("minimal_publisher"), count_(0) + { + publisher_ = this->create_publisher("topic", 10); + auto timer_callback = + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(this->count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); + } + + private: + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; }; int main(int argc, char * argv[]) @@ -147,7 +146,6 @@ Last is ``std_msgs/msg/string.hpp``, which includes the built-in message type yo .. code-block:: C++ #include - #include #include #include @@ -168,7 +166,11 @@ Every ``this`` in the code is referring to the node. The public constructor names the node ``minimal_publisher`` and initializes ``count_`` to 0. Inside the constructor, the publisher is initialized with the ``String`` message type, the topic name ``topic``, and the required queue size to limit messages in the event of a backup. -Next, ``timer_`` is initialized, which causes the ``timer_callback`` function to be executed twice a second. +Next, a `lambda function `_ called ``timer_callback`` is declared. +It performs a by-reference capture of the current object ``this``, takes no input arguments and returns void. +The ``timer_callback`` function creates a new message of type ``String``, sets its data with the desired string and publishes it. +The ``RCLCPP_INFO`` macro ensures every published message is printed to the console. +At last, ``timer_`` is initialized, which causes the ``timer_callback`` function to be executed twice a second. .. code-block:: C++ @@ -177,31 +179,24 @@ Next, ``timer_`` is initialized, which causes the ``timer_callback`` function to : Node("minimal_publisher"), count_(0) { publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); + auto timer_callback = + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(this->count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); } -The ``timer_callback`` function is where the message data is set and the messages are actually published. -The ``RCLCPP_INFO`` macro ensures every published message is printed to the console. +In the bottom of the class is the declaration of the timer, publisher, and counter fields. .. code-block:: C++ private: - void timer_callback() - { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - -Last is the declaration of the timer, publisher, and counter fields. - -.. code-block:: C++ - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; Following the ``MinimalPublisher`` class is ``main``, where the node actually executes. ``rclcpp::init`` initializes ROS 2, and ``rclcpp::spin`` starts processing data from the node, including callbacks from the timer. @@ -257,7 +252,7 @@ After that, add the executable and name it ``talker`` so you can run your node u .. code-block:: console - add_executable(talker src/publisher_member_function.cpp) + add_executable(talker src/publisher_lambda_function.cpp) ament_target_dependencies(talker rclcpp std_msgs) Finally, add the ``install(TARGETS...)`` section so ``ros2 run`` can find your executable: @@ -288,7 +283,7 @@ You can clean up your ``CMakeLists.txt`` by removing some unnecessary sections a find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) - add_executable(talker src/publisher_member_function.cpp) + add_executable(talker src/publisher_lambda_function.cpp) ament_target_dependencies(talker rclcpp std_msgs) install(TARGETS @@ -311,13 +306,13 @@ Enter the following code in your terminal: .. code-block:: console - wget -O subscriber_member_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/member_function.cpp + wget -O subscriber_lambda_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/lambda.cpp .. group-tab:: macOS .. code-block:: console - wget -O subscriber_member_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/member_function.cpp + wget -O subscriber_lambda_function.cpp https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/lambda.cpp .. group-tab:: Windows @@ -325,21 +320,21 @@ Enter the following code in your terminal: .. code-block:: console - curl -sk https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/member_function.cpp -o subscriber_member_function.cpp + curl -sk https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/lambda.cpp -o subscriber_lambda_function.cpp Or in powershell: .. code-block:: console - curl https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/member_function.cpp -o subscriber_member_function.cpp + curl https://raw.githubusercontent.com/ros2/examples/{REPOS_FILE_BRANCH}/rclcpp/topics/minimal_subscriber/lambda.cpp -o subscriber_lambda_function.cpp Check to ensure that these files exist: .. code-block:: console - publisher_member_function.cpp subscriber_member_function.cpp + publisher_lambda_function.cpp subscriber_lambda_function.cpp -Open the ``subscriber_member_function.cpp`` with your text editor. +Open the ``subscriber_lambda_function.cpp`` with your text editor. .. code-block:: C++ @@ -347,24 +342,23 @@ Open the ``subscriber_member_function.cpp`` with your text editor. #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" - using std::placeholders::_1; class MinimalSubscriber : public rclcpp::Node { - public: - MinimalSubscriber() - : Node("minimal_subscriber") - { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); - } - - private: - void topic_callback(const std_msgs::msg::String & msg) const - { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); - } - rclcpp::Subscription::SharedPtr subscription_; + public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + auto topic_callback = + [this](std_msgs::msg::String::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + }; + subscription_ = + this->create_subscription("topic", 10, topic_callback); + } + + private: + rclcpp::Subscription::SharedPtr subscription_; }; int main(int argc, char * argv[]) @@ -379,33 +373,33 @@ Open the ``subscriber_member_function.cpp`` with your text editor. ~~~~~~~~~~~~~~~~~~~~ The subscriber node's code is nearly identical to the publisher's. -Now the node is named ``minimal_subscriber``, and the constructor uses the node's ``create_subscription`` class to execute the callback. +Now the node is named ``minimal_subscriber``, and the constructor uses the node's ``create_subscription`` function to execute the callback. There is no timer because the subscriber simply responds whenever data is published to the ``topic`` topic. +The ``topic_callback`` function receives the string message data published over the topic, and simply writes it to the console using the ``RCLCPP_INFO`` macro. + +Recall from the :doc:`topic tutorial <../Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics>` that the topic name and message type used by the publisher and subscriber must match to allow them to communicate. + .. code-block:: C++ public: MinimalSubscriber() : Node("minimal_subscriber") { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + auto topic_callback = + [this](std_msgs::msg::String::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + }; + subscription_ = + this->create_subscription("topic", 10, topic_callback); } -Recall from the :doc:`topic tutorial <../Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics>` that the topic name and message type used by the publisher and subscriber must match to allow them to communicate. - -The ``topic_callback`` function receives the string message data published over the topic, and simply writes it to the console using the ``RCLCPP_INFO`` macro. - The only field declaration in this class is the subscription. .. code-block:: C++ private: - void topic_callback(const std_msgs::msg::String & msg) const - { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); - } rclcpp::Subscription::SharedPtr subscription_; The ``main`` function is exactly the same, except now it spins the ``MinimalSubscriber`` node. @@ -420,7 +414,7 @@ Reopen ``CMakeLists.txt`` and add the executable and target for the subscriber n .. code-block:: cmake - add_executable(listener src/subscriber_member_function.cpp) + add_executable(listener src/subscriber_lambda_function.cpp) ament_target_dependencies(listener rclcpp std_msgs) install(TARGETS diff --git a/source/Tutorials/Intermediate/Tf2/Adding-A-Frame-Cpp.rst b/source/Tutorials/Intermediate/Tf2/Adding-A-Frame-Cpp.rst index af48725ba04..858908e317d 100644 --- a/source/Tutorials/Intermediate/Tf2/Adding-A-Frame-Cpp.rst +++ b/source/Tutorials/Intermediate/Tf2/Adding-A-Frame-Cpp.rst @@ -101,30 +101,28 @@ Now open the file called ``fixed_frame_tf2_broadcaster.cpp``. : Node("fixed_frame_tf2_broadcaster") { tf_broadcaster_ = std::make_shared(this); - timer_ = this->create_wall_timer( - 100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this)); - } - private: - void broadcast_timer_callback() - { - geometry_msgs::msg::TransformStamped t; - - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = "turtle1"; - t.child_frame_id = "carrot1"; - t.transform.translation.x = 0.0; - t.transform.translation.y = 2.0; - t.transform.translation.z = 0.0; - t.transform.rotation.x = 0.0; - t.transform.rotation.y = 0.0; - t.transform.rotation.z = 0.0; - t.transform.rotation.w = 1.0; - - tf_broadcaster_->sendTransform(t); + auto broadcast_timer_callback = [this](){ + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "turtle1"; + t.child_frame_id = "carrot1"; + t.transform.translation.x = 0.0; + t.transform.translation.y = 2.0; + t.transform.translation.z = 0.0; + t.transform.rotation.x = 0.0; + t.transform.rotation.y = 0.0; + t.transform.rotation.z = 0.0; + t.transform.rotation.w = 1.0; + + tf_broadcaster_->sendTransform(t); + }; + timer_ = this->create_wall_timer(100ms, broadcast_timer_callback); } - rclcpp::TimerBase::SharedPtr timer_; + private: + rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr tf_broadcaster_; }; @@ -396,31 +394,29 @@ Now open the file called ``dynamic_frame_tf2_broadcaster.cpp``: : Node("dynamic_frame_tf2_broadcaster") { tf_broadcaster_ = std::make_shared(this); - timer_ = this->create_wall_timer( - 100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this)); - } - private: - void broadcast_timer_callback() - { - rclcpp::Time now = this->get_clock()->now(); - double x = now.seconds() * PI; - - geometry_msgs::msg::TransformStamped t; - t.header.stamp = now; - t.header.frame_id = "turtle1"; - t.child_frame_id = "carrot1"; - t.transform.translation.x = 10 * sin(x); - t.transform.translation.y = 10 * cos(x); - t.transform.translation.z = 0.0; - t.transform.rotation.x = 0.0; - t.transform.rotation.y = 0.0; - t.transform.rotation.z = 0.0; - t.transform.rotation.w = 1.0; - - tf_broadcaster_->sendTransform(t); + auto broadcast_timer_callback = [this](){ + rclcpp::Time now = this->get_clock()->now(); + double x = now.seconds() * PI; + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = now; + t.header.frame_id = "turtle1"; + t.child_frame_id = "carrot1"; + t.transform.translation.x = 10 * sin(x); + t.transform.translation.y = 10 * cos(x); + t.transform.translation.z = 0.0; + t.transform.rotation.x = 0.0; + t.transform.rotation.y = 0.0; + t.transform.rotation.z = 0.0; + t.transform.rotation.w = 1.0; + + tf_broadcaster_->sendTransform(t); + }; + timer_ = this->create_wall_timer(100ms, broadcast_timer_callback); } + private: rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr tf_broadcaster_; }; diff --git a/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.rst b/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.rst index b0c60d292be..67898cf523b 100644 --- a/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.rst +++ b/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.rst @@ -102,42 +102,40 @@ Open the file using your preferred text editor. stream << "/" << turtlename_.c_str() << "/pose"; std::string topic_name = stream.str(); + auto handle_turtle_pose = [this](const std::shared_ptr msg){ + geometry_msgs::msg::TransformStamped t; + + // Read message content and assign it to + // corresponding tf variables + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = turtlename_.c_str(); + + // Turtle only exists in 2D, thus we get x and y translation + // coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = msg->x; + t.transform.translation.y = msg->y; + t.transform.translation.z = 0.0; + + // For the same reason, turtle can only rotate around one axis + // and this why we set rotation in x and y to 0 and obtain + // rotation in z axis from the message + tf2::Quaternion q; + q.setRPY(0, 0, msg->theta); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + // Send the transformation + tf_broadcaster_->sendTransform(t); + }; subscription_ = this->create_subscription( topic_name, 10, - std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1)); + handle_turtle_pose); } private: - void handle_turtle_pose(const std::shared_ptr msg) - { - geometry_msgs::msg::TransformStamped t; - - // Read message content and assign it to - // corresponding tf variables - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = "world"; - t.child_frame_id = turtlename_.c_str(); - - // Turtle only exists in 2D, thus we get x and y translation - // coordinates from the message and set the z coordinate to 0 - t.transform.translation.x = msg->x; - t.transform.translation.y = msg->y; - t.transform.translation.z = 0.0; - - // For the same reason, turtle can only rotate around one axis - // and this why we set rotation in x and y to 0 and obtain - // rotation in z axis from the message - tf2::Quaternion q; - q.setRPY(0, 0, msg->theta); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - - // Send the transformation - tf_broadcaster_->sendTransform(t); - } - rclcpp::Subscription::SharedPtr subscription_; std::unique_ptr tf_broadcaster_; std::string turtlename_; @@ -167,7 +165,7 @@ Afterward, the node subscribes to topic ``turtleX/pose`` and runs function ``han subscription_ = this->create_subscription( topic_name, 10, - std::bind(&FramePublisher::handle_turtle_pose, this, _1)); + handle_turtle_pose); Now, we create a ``TransformStamped`` object and give it the appropriate metadata. diff --git a/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.rst b/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.rst index 87f2db40339..2cf281a11a9 100644 --- a/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.rst +++ b/source/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.rst @@ -112,7 +112,7 @@ Open the file using your preferred text editor. // Call on_timer function every second timer_ = this->create_wall_timer( - 1s, std::bind(&FrameListener::on_timer, this)); + 1s, [this](){return this->on_timer();}); } private: diff --git a/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.rst b/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.rst index 3ada605427a..81d690570b5 100644 --- a/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.rst +++ b/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.rst @@ -153,7 +153,7 @@ The constructor also instantiates a new action server: .. literalinclude:: scripts/server.cpp :language: c++ - :lines: 26-31 + :lines: 86-91 An action server requires 6 things: @@ -164,14 +164,14 @@ An action server requires 6 things: 5. A callback function for handling cancellation: ``handle_cancel``. 6. A callback function for handling goal accept: ``handle_accept``. -The implementation of the various callbacks is next in the file. +The implementation of the various callbacks is done with [lambda expressions](https://en.cppreference.com/w/cpp/language/lambda) within the constructor. Note that all of the callbacks need to return quickly, otherwise we risk starving the executor. We start with the callback for handling new goals: .. literalinclude:: scripts/server.cpp :language: c++ - :lines: 37-44 + :lines: 26-33 This implementation just accepts all goals. @@ -179,7 +179,7 @@ Next up is the callback for dealing with cancellation: .. literalinclude:: scripts/server.cpp :language: c++ - :lines: 46-52 + :lines: 35-41 This implementation just tells the client that it accepted the cancellation. @@ -187,7 +187,7 @@ The last of the callbacks accepts a new goal and starts processing it: .. literalinclude:: scripts/server.cpp :language: c++ - :lines: 54-59 + :lines: 43-50 Since the execution is a long-running operation, we spawn off a thread to do the actual work and return from ``handle_accepted`` quickly. @@ -195,7 +195,7 @@ All further processing and updates are done in the ``execute`` method in the new .. literalinclude:: scripts/server.cpp :language: c++ - :lines: 61-95 + :lines: 63-96 This work thread processes one sequence number of the Fibonacci sequence every second, publishing a feedback update for each step. When it has finished processing, it marks the ``goal_handle`` as succeeded, and quits. @@ -291,13 +291,13 @@ We also instantiate a ROS timer that will kick off the one and only call to ``se .. literalinclude:: scripts/client.cpp :language: c++ - :lines: 27-30 + :lines: 28-31 When the timer expires, it will call ``send_goal``: .. literalinclude:: scripts/client.cpp :language: c++ - :lines: 32-57 + :lines: 34-96 This function does the following: @@ -312,21 +312,21 @@ That response is handled by ``goal_response_callback``: .. literalinclude:: scripts/client.cpp :language: c++ - :lines: 62-71 + :lines: 51-58 Assuming the goal was accepted by the server, it will start processing. Any feedback to the client will be handled by the ``feedback_callback``: .. literalinclude:: scripts/client.cpp :language: c++ - :lines: 72-83 + :lines: 60-70 When the server is finished processing, it will return a result to the client. The result is handled by the ``result_callback``: .. literalinclude:: scripts/client.cpp :language: c++ - :lines: 84-107 + :lines: 72-94 We now have a fully functioning action client. Let's get it built and running. diff --git a/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/client.cpp b/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/client.cpp index 4ec691ba82c..5ed229cacd6 100644 --- a/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/client.cpp +++ b/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/client.cpp @@ -25,9 +25,10 @@ class FibonacciActionClient : public rclcpp::Node this, "fibonacci"); + auto timer_callback_lambda = [this](){ return this->send_goal(); }; this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), - std::bind(&FibonacciActionClient::send_goal, this)); + timer_callback_lambda); } void send_goal() @@ -47,63 +48,56 @@ 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_; - - void goal_response_callback(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"); - } - } - - 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(), ss.str().c_str()); - } - - 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(), ss.str().c_str()); - rclcpp::shutdown(); - } }; // class FibonacciActionClient } // namespace custom_action_cpp diff --git a/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/server.cpp b/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/server.cpp index be8dcb8aa81..88e2fef369f 100644 --- a/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/server.cpp +++ b/source/Tutorials/Intermediate/Writing-an-Action-Server-Client/scripts/server.cpp @@ -23,43 +23,44 @@ class FibonacciActionServer : public rclcpp::Node { using namespace std::placeholders; + auto handle_goal = [this]( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); + (void)uuid; + 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, "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_; - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - 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; - } - - 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(); - } - - void execute(const std::shared_ptr goal_handle) - { + void execute(const std::shared_ptr goal_handle) { RCLCPP_INFO(this->get_logger(), "Executing goal"); rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); @@ -92,7 +93,8 @@ class FibonacciActionServer : public rclcpp::Node goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal succeeded"); } - } + }; + }; // class FibonacciActionServer } // namespace custom_action_cpp