Skip to content

Commit

Permalink
Publishing is slow in Docker with MutliThreadedExecutor
Browse files Browse the repository at this point in the history
  ros2/rclcpp#1487

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>
  • Loading branch information
fujitatomoya committed Dec 9, 2020
1 parent c7e4f34 commit ce0d33d
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 0 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,7 @@ endfunction()
custom_executable(rclcpp_1431)
custom_executable(rclcpp_1454)
custom_executable(rclcpp_1455)
custom_executable(rclcpp_1487)

if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" AND
"${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS 6.0)
Expand Down
53 changes: 53 additions & 0 deletions src/rclcpp_1487.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
#include <chrono>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <thread>

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

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. */

class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher()
: Node("minimal_publisher"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
timer_ = this->create_wall_timer(
1ms, std::bind(&MinimalPublisher::timer_callback, this)); // 1kHz
}

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<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);

rclcpp::executors::MultiThreadedExecutor exe;
// rclcpp::executors::SingleThreadedExecutor exe;

std::shared_ptr<MinimalPublisher> pub = std::make_shared<MinimalPublisher>();
exe.add_node(pub->get_node_base_interface());

exe.spin();
rclcpp::shutdown();
return 0;
}

0 comments on commit ce0d33d

Please sign in to comment.