diff --git a/common/autoware_control_center/src/autoware_control_center.cpp b/common/autoware_control_center/src/autoware_control_center.cpp index 6b0356f8..d9b10e0c 100644 --- a/common/autoware_control_center/src/autoware_control_center.cpp +++ b/common/autoware_control_center/src/autoware_control_center.cpp @@ -48,7 +48,7 @@ AutowareControlCenter::AutowareControlCenter(const rclcpp::NodeOptions & options std::chrono::milliseconds lease_duration_(get_parameter("lease_duration").as_int()); callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - + using std::placeholders::_1; using std::placeholders::_2; srv_register_ = create_service( @@ -126,7 +126,7 @@ void AutowareControlCenter::startup_callback() // check if some node has been registered if (node_registry_.is_empty()) { RCLCPP_INFO(get_logger(), "Node register map is empty. Countdown is %d", countdown); - } + } if (countdown < 1 && node_registry_.is_empty() && startup) { RCLCPP_INFO( get_logger(), "Startup timeout is over. Map is empty. Start re-registering procedure."); @@ -149,7 +149,7 @@ void AutowareControlCenter::startup_callback() rclcpp::Client::SharedPtr dereg_client_ = create_client( - pair.first); + pair.first); autoware_control_center_msgs::srv::AutowareControlCenterDeregister::Request::SharedPtr req = std::make_shared< autoware_control_center_msgs::srv::AutowareControlCenterDeregister::Request>(); @@ -162,7 +162,8 @@ void AutowareControlCenter::startup_callback() auto response_received_callback = [this](ServiceResponseFuture future) { auto response = future.get(); RCLCPP_INFO( - get_logger(), "Deregister response: %d, %s", response->status.status, response->name_node.c_str()); + get_logger(), "Deregister response: %d, %s", response->status.status, + response->name_node.c_str()); if (response->status.status == 1) { RCLCPP_INFO(get_logger(), "Node was deregistered"); diff --git a/common/autoware_node/include/autoware_node/autoware_node.hpp b/common/autoware_node/include/autoware_node/autoware_node.hpp index 99ee9b5b..af2ab271 100644 --- a/common/autoware_node/include/autoware_node/autoware_node.hpp +++ b/common/autoware_node/include/autoware_node/autoware_node.hpp @@ -16,6 +16,10 @@ #define AUTOWARE_NODE__AUTOWARE_NODE_HPP_ #include "autoware_node/visibility_control.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "autoware_control_center_msgs/msg/autoware_node_state.hpp" @@ -24,11 +28,6 @@ #include "autoware_control_center_msgs/srv/autoware_node_error.hpp" #include "autoware_control_center_msgs/srv/autoware_node_register.hpp" -#include "rclcpp/subscription_options.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/subscription_traits.hpp" -#include "rclcpp/message_memory_strategy.hpp" - #include namespace autoware_node @@ -42,69 +41,59 @@ class AutowareNode : public rclcpp_lifecycle::LifecycleNode const std::string & node_name, const std::string & ns = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - template< - typename MessageT, - typename CallbackT, - typename AllocatorT = std::allocator, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> + template < + typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> std::shared_ptr create_monitored_subscription( - const std::string & topic_name, - const float hz, - const rclcpp::QoS & qos, - CallbackT && callback, - const rclcpp::SubscriptionOptions & options = - rclcpp::SubscriptionOptions(), - typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( - MessageMemoryStrategyT::create_default() - ) - ) + const std::string & topic_name, const float hz, const rclcpp::QoS & qos, CallbackT && callback, + const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = + (MessageMemoryStrategyT::create_default())) { - // create proper qos based on input parameter - // update lease duration and deadline in qos + // create proper qos based on input parameter + // update lease duration and deadline in qos RCLCPP_INFO(get_logger(), "Create monitored subscription to topic %s", topic_name.c_str()); - std::chrono::milliseconds lease_duration{static_cast(1.0 / hz * 1000 * 1.1)}; // add 10 % gap to lease duration (buffer) + std::chrono::milliseconds lease_duration{ + static_cast(1.0 / hz * 1000 * 1.1)}; // add 10 % gap to lease duration (buffer) rclcpp::QoS qos_profile = qos; qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) .liveliness_lease_duration(lease_duration) .deadline(lease_duration); rclcpp::SubscriptionOptions sub_options = options; - sub_options.event_callbacks.deadline_callback = - [=](rclcpp::QOSDeadlineRequestedInfo& event) -> void { - RCLCPP_ERROR(get_logger(), "Requested deadline missed - total %d delta %d", - event.total_count, event.total_count_change); - // NodeError service call - std::string msg = "Deadline for topic " + topic_name + " was missed."; - autoware_control_center_msgs::msg::AutowareNodeState node_state; - node_state.status = autoware_control_center_msgs::msg::AutowareNodeState::ERROR; - send_state(node_state, msg); - }; - - sub_options.event_callbacks.liveliness_callback = + sub_options.event_callbacks.deadline_callback = + [=](rclcpp::QOSDeadlineRequestedInfo & event) -> void { + RCLCPP_ERROR( + get_logger(), "Requested deadline missed - total %d delta %d", event.total_count, + event.total_count_change); + // NodeError service call + std::string msg = "Deadline for topic " + topic_name + " was missed."; + autoware_control_center_msgs::msg::AutowareNodeState node_state; + node_state.status = autoware_control_center_msgs::msg::AutowareNodeState::ERROR; + send_state(node_state, msg); + }; + + sub_options.event_callbacks.liveliness_callback = [=](rclcpp::QOSLivelinessChangedInfo & event) -> void { - RCLCPP_INFO(get_logger(), "%s topic liveliness info changed", topic_name.c_str()); - printf("Reader Liveliness changed event: \n"); - printf(" alive_count: %d\n", event.alive_count); - printf(" not_alive_count: %d\n", event.not_alive_count); - printf(" alive_count_change: %d\n", event.alive_count_change); - printf(" not_alive_count_change: %d\n", event.not_alive_count_change); - if (event.alive_count == 0) { - RCLCPP_ERROR(get_logger(), "%s topic publisher is not alive.", topic_name.c_str()); - // NodeError service call - std::string msg = topic_name + " topic publisher is not alive."; - autoware_control_center_msgs::msg::AutowareNodeState node_state; - node_state.status = autoware_control_center_msgs::msg::AutowareNodeState::ERROR; - send_state(node_state, msg); - } - }; - - return create_subscription ( - topic_name, - qos_profile, - std::forward(callback), - sub_options, - msg_mem_strat); + RCLCPP_INFO(get_logger(), "%s topic liveliness info changed", topic_name.c_str()); + printf("Reader Liveliness changed event: \n"); + printf(" alive_count: %d\n", event.alive_count); + printf(" not_alive_count: %d\n", event.not_alive_count); + printf(" alive_count_change: %d\n", event.alive_count_change); + printf(" not_alive_count_change: %d\n", event.not_alive_count_change); + if (event.alive_count == 0) { + RCLCPP_ERROR(get_logger(), "%s topic publisher is not alive.", topic_name.c_str()); + // NodeError service call + std::string msg = topic_name + " topic publisher is not alive."; + autoware_control_center_msgs::msg::AutowareNodeState node_state; + node_state.status = autoware_control_center_msgs::msg::AutowareNodeState::ERROR; + send_state(node_state, msg); + } + }; + + return create_subscription( + topic_name, qos_profile, std::forward(callback), sub_options, msg_mem_strat); } rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_; diff --git a/common/test_node/include/test_node/test_node.hpp b/common/test_node/include/test_node/test_node.hpp index 027fe906..7b1730d7 100644 --- a/common/test_node/include/test_node/test_node.hpp +++ b/common/test_node/include/test_node/test_node.hpp @@ -19,7 +19,6 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "test_node/visibility_control.hpp" - #include "std_msgs/msg/string.hpp" namespace test_node { diff --git a/common/test_node/src/test_node.cpp b/common/test_node/src/test_node.cpp index d90dde85..3c5e4c76 100644 --- a/common/test_node/src/test_node.cpp +++ b/common/test_node/src/test_node.cpp @@ -21,15 +21,15 @@ namespace test_node using std::placeholders::_1; - TestNode::TestNode(const rclcpp::NodeOptions & options) : autoware_node::AutowareNode("test_node", "", options) { RCLCPP_INFO( get_logger(), "TestNode constructor with name %s", autoware_node::AutowareNode::self_name.c_str()); - monitored_subscription_ = autoware_node::AutowareNode::create_monitored_subscription( - "topic", 10, 10, std::bind(&TestNode::topic_callback, this, _1)); + monitored_subscription_ = + autoware_node::AutowareNode::create_monitored_subscription( + "topic", 10, 10, std::bind(&TestNode::topic_callback, this, _1)); } void TestNode::topic_callback(const std_msgs::msg::String::SharedPtr msg) diff --git a/common/test_node/src/test_pub.cpp b/common/test_node/src/test_pub.cpp index 2c5fa94d..5caca450 100644 --- a/common/test_node/src/test_pub.cpp +++ b/common/test_node/src/test_pub.cpp @@ -1,45 +1,46 @@ +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/string.hpp" + #include #include #include #include -#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. */ + * member function as a callback from the timer. */ class MinimalPublisher : public rclcpp::Node { - public: - MinimalPublisher() - : Node("minimal_publisher"), count_(0) - { - rclcpp::QoS qos_profile(10); - declare_parameter("hz", 10); - int hz = get_parameter("hz").as_int(); - std::chrono::milliseconds timer_period{static_cast(1.0 / hz * 1000)}; - std::chrono::milliseconds lease_duration{static_cast(1.0 / hz * 1000 * 1.1)}; - qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) - .liveliness_lease_duration(lease_duration) - .deadline(lease_duration); - publisher_ = this->create_publisher("topic", qos_profile); - timer_ = this->create_wall_timer(timer_period, std::bind(&MinimalPublisher::timer_callback, this)); - } +public: + MinimalPublisher() : Node("minimal_publisher"), count_(0) + { + rclcpp::QoS qos_profile(10); + declare_parameter("hz", 10); + int hz = get_parameter("hz").as_int(); + std::chrono::milliseconds timer_period{static_cast(1.0 / hz * 1000)}; + std::chrono::milliseconds lease_duration{static_cast(1.0 / hz * 1000 * 1.1)}; + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(lease_duration) + .deadline(lease_duration); + publisher_ = this->create_publisher("topic", qos_profile); + timer_ = + this->create_wall_timer(timer_period, 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_; +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_; }; int main(int argc, char * argv[]) @@ -48,4 +49,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +}