diff --git a/prover_rclcpp/CMakeLists.txt b/prover_rclcpp/CMakeLists.txt index 889c129..29bbb39 100644 --- a/prover_rclcpp/CMakeLists.txt +++ b/prover_rclcpp/CMakeLists.txt @@ -65,6 +65,9 @@ custom_executable(rclcpp_1566) #custom_executable(rclcpp_1580) custom_executable(rclcpp_1585) #custom_executable(rclcpp_1597) +custom_executable(rclcpp_1611_client) +custom_executable(rclcpp_1611_publisher) +custom_executable(rclcpp_1611_server) #custom_executable(ros2_644) #custom_executable(ros2_946_pub) diff --git a/prover_rclcpp/src/rclcpp_1611_client.cpp b/prover_rclcpp/src/rclcpp_1611_client.cpp new file mode 100644 index 0000000..1aa566f --- /dev/null +++ b/prover_rclcpp/src/rclcpp_1611_client.cpp @@ -0,0 +1,94 @@ +#include +#include +#include +#include + +#include +#include +#include + +class CallbackGroupSrvClient: public rclcpp::Node { +public: + // Constructor + CallbackGroupSrvClient() + : Node("callback_group_srv_client"){ + + // Init callback group + callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor_thread_ = std::thread([this]() { + callback_group_executor_.add_callback_group(callback_group_, this->get_node_base_interface()); + callback_group_executor_.spin(); + }); + + // Init subscription + subscription_ = this->create_subscription( + "topic_bool", + rclcpp::QoS(1), + std::bind( + &CallbackGroupSrvClient::sub_callback, + this, + std::placeholders::_1 + ) + ); + + // Create service client and link it to callback group + srv_client_ = this->create_client( + "trigger_srv", + rmw_qos_profile_services_default, + callback_group_ + ); + } + + // Destructor + ~CallbackGroupSrvClient(){ + callback_group_executor_.cancel(); + callback_group_executor_thread_.join(); + } + +private: + // Methods + void sub_callback(const std_msgs::msg::Bool::SharedPtr ){ + RCLCPP_INFO_STREAM(this->get_logger(), "Received new msg! "); + + // Send request and wait for future + auto req = std::make_shared(); + auto future = srv_client_->async_send_request(req); + RCLCPP_INFO_STREAM(this->get_logger(), "Sending request"); + auto future_status = future.wait_for(std::chrono::seconds(10)); + + // Process result + if (future_status == std::future_status::ready) { + if (future.get()->success) { + RCLCPP_INFO_STREAM(this->get_logger(), "Srv suceeded"); + } + else { + RCLCPP_INFO_STREAM(this->get_logger(), "Srv failed"); + } + } + else if (future_status == std::future_status::timeout) { + RCLCPP_INFO_STREAM(this->get_logger(), "Response not received within timeout"); + } + else if (future_status == std::future_status::deferred) { + RCLCPP_INFO_STREAM(this->get_logger(), "Srv deferred"); + } + + return; + } + + // Attributes + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + std::thread callback_group_executor_thread_; + rclcpp::Client::SharedPtr srv_client_; +}; + + + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + auto client = std::make_shared(); + rclcpp::spin(client); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/prover_rclcpp/src/rclcpp_1611_publisher.cpp b/prover_rclcpp/src/rclcpp_1611_publisher.cpp new file mode 100644 index 0000000..e046db9 --- /dev/null +++ b/prover_rclcpp/src/rclcpp_1611_publisher.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include + +#include +#include + + +class MinimalPublisher: public rclcpp::Node { +public: + // constructor + MinimalPublisher() + : Node("minimal_publisher"){ + publisher_ = this->create_publisher("topic_bool", rclcpp::QoS(1)); + timer_ = this->create_wall_timer( + std::chrono::seconds(5), + std::bind( + &MinimalPublisher::timer_callback, + this + ) + ); + } + +private: + // method + void timer_callback(){ + std_msgs::msg::Bool msg; + msg.data = false; + publisher_->publish(msg); + RCLCPP_INFO_STREAM(this->get_logger(), "Publishing data! "); + return; + } + + // attributes + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + auto pub = std::make_shared(); + rclcpp::spin(pub); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/prover_rclcpp/src/rclcpp_1611_server.cpp b/prover_rclcpp/src/rclcpp_1611_server.cpp new file mode 100644 index 0000000..51a4864 --- /dev/null +++ b/prover_rclcpp/src/rclcpp_1611_server.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +#include + +#include +#include +#include + + +class CallbackGroupSrvServer: public rclcpp::Node { +public: + // Constructor + CallbackGroupSrvServer() + : Node("callback_group_srv_server"){ + // Init callback group and spin it + callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + //callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); + callback_group_executor_thread_ = std::thread([this]() { + callback_group_executor_.add_callback_group(callback_group_, this->get_node_base_interface()); + callback_group_executor_.spin(); + }); + + // Init service server and link it to callback group + service_ = this->create_service( + "trigger_srv", + std::bind( + &CallbackGroupSrvServer::srv_callback, + this, + std::placeholders::_1, + std::placeholders::_2) + ); + } + + // Destructor + ~CallbackGroupSrvServer(){ + callback_group_executor_.cancel(); + callback_group_executor_thread_.join(); + } + +private: + // Methods + void sub_callback(const std_msgs::msg::Bool::SharedPtr ){ + new_msg_ = true; + RCLCPP_INFO_STREAM(this->get_logger(), "Received new msg!"); + return; + } + + void srv_callback(const std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr res){ + RCLCPP_INFO_STREAM(this->get_logger(), "Received request"); + new_msg_ = false; + + // Create subscription + auto sub_options_ = rclcpp::SubscriptionOptions(); + sub_options_.callback_group = callback_group_; + auto subscription = this->create_subscription( + "topic_bool", + rclcpp::QoS(1), + std::bind( + &CallbackGroupSrvServer::sub_callback, + this, + std::placeholders::_1), + sub_options_ + ); + + // Wait for new_msg + while (rclcpp::ok() + && new_msg_ == false) { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO_STREAM(this->get_logger(), "Sleep..."); + } + + res->message = ""; + res->success = true; + return; + } + + // Attributes + rclcpp::Service::SharedPtr service_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + std::thread callback_group_executor_thread_; + bool new_msg_; +}; + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + auto srv_server = std::make_shared(); + rclcpp::spin(srv_server); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file