Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Nov 26, 2024
1 parent 72e4382 commit ce6afb8
Show file tree
Hide file tree
Showing 4 changed files with 126 additions and 122 deletions.
210 changes: 105 additions & 105 deletions common/autoware_node/include/autoware/node/node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,110 +36,110 @@

namespace autoware::node
{
class Node : public rclcpp_lifecycle::LifecycleNode
{
public:
AUTOWARE_NODE_PUBLIC
explicit Node(
const std::string& node_name, const std::string& ns = "",
const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

protected:
// /// @brief Create a subscription with a monitored deadline and liveliness
// template <
// typename MessageT, typename CallbackT, typename AllocatorT = std::allocator<void>,
// typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
// typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
// std::shared_ptr<SubscriptionT> 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_strategy =
// (MessageMemoryStrategyT::create_default()))
// {
// // create proper qos based on input parameter
// // update lease duration and deadline in qos
// RCLCPP_DEBUG(get_logger(), "Create monitored subscription to topic %s", topic_name.c_str());
// std::chrono::milliseconds lease_duration{
// static_cast<int>(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::NodeState node_state;
// node_state.status = autoware_control_center_msgs::msg::NodeState::ERROR;
// send_state(node_state, msg);
// };
//
// sub_options.event_callbacks.liveliness_callback =
// [=](rclcpp::QOSLivelinessChangedInfo & event) -> void {
// RCLCPP_DEBUG(get_logger(), "%s topic liveliness info changed", topic_name.c_str());
// RCLCPP_DEBUG(get_logger(), " alive_count: %d", event.alive_count);
// RCLCPP_DEBUG(get_logger(), " not_alive_count: %d", event.not_alive_count);
// RCLCPP_DEBUG(get_logger(), " alive_count_change: %d", event.alive_count_change);
// RCLCPP_DEBUG(get_logger(), " not_alive_count_change: %d", 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::NodeState node_state;
// node_state.status = autoware_control_center_msgs::msg::NodeState::ERROR;
// send_state(node_state, msg);
// }
// };
//
// return create_subscription<MessageT>(
// topic_name, qos_profile, std::forward<CallbackT>(callback), sub_options,
// msg_mem_strategy);
// }

private:
using Heartbeat = autoware_control_center_msgs::msg::Heartbeat;
using NodeStatusActivity = autoware_control_center_msgs::msg::NodeStatusActivity;
using NodeStatusOperational = autoware_control_center_msgs::msg::NodeStatusOperational;
using ResultDeregistration = autoware_control_center_msgs::msg::ResultDeregistration;
using ResultRegistration = autoware_control_center_msgs::msg::ResultRegistration;
using Deregister = autoware_control_center_msgs::srv::Deregister;
using Register = autoware_control_center_msgs::srv::Register;

using FutureRegister = rclcpp::Client<autoware_control_center_msgs::srv::Register>::SharedFuture;

rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_;

rclcpp::Client<autoware_control_center_msgs::srv::Register>::SharedPtr cli_register_;

rclcpp::Publisher<autoware_control_center_msgs::msg::Heartbeat>::SharedPtr pub_heartbeat_;

uint16_t sequence_number_;

bool is_registered_;
double period_timer_register_ms_;
double period_heartbeat_ms_;
double deadline_ms_;

rclcpp::TimerBase::SharedPtr timer_registration_;
rclcpp::TimerBase::SharedPtr timer_heartbeat_;

unique_identifier_msgs::msg::UUID uuid_node_;

void on_tick_registration();
void on_tick_heartbeat();
void on_register(FutureRegister future);

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_shutdown(const rclcpp_lifecycle::State& state) override;

protected:
void destroy_node();
};
} // namespace autoware::node
class Node : public rclcpp_lifecycle::LifecycleNode
{
public:
AUTOWARE_NODE_PUBLIC
explicit Node(
const std::string & node_name, const std::string & ns = "",
const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

protected:
// /// @brief Create a subscription with a monitored deadline and liveliness
// template <
// typename MessageT, typename CallbackT, typename AllocatorT = std::allocator<void>,
// typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
// typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
// std::shared_ptr<SubscriptionT> 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_strategy =
// (MessageMemoryStrategyT::create_default()))
// {
// // create proper qos based on input parameter
// // update lease duration and deadline in qos
// RCLCPP_DEBUG(get_logger(), "Create monitored subscription to topic %s", topic_name.c_str());
// std::chrono::milliseconds lease_duration{
// static_cast<int>(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::NodeState node_state;
// node_state.status = autoware_control_center_msgs::msg::NodeState::ERROR;
// send_state(node_state, msg);
// };
//
// sub_options.event_callbacks.liveliness_callback =
// [=](rclcpp::QOSLivelinessChangedInfo & event) -> void {
// RCLCPP_DEBUG(get_logger(), "%s topic liveliness info changed", topic_name.c_str());
// RCLCPP_DEBUG(get_logger(), " alive_count: %d", event.alive_count);
// RCLCPP_DEBUG(get_logger(), " not_alive_count: %d", event.not_alive_count);
// RCLCPP_DEBUG(get_logger(), " alive_count_change: %d", event.alive_count_change);
// RCLCPP_DEBUG(get_logger(), " not_alive_count_change: %d", 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::NodeState node_state;
// node_state.status = autoware_control_center_msgs::msg::NodeState::ERROR;
// send_state(node_state, msg);
// }
// };
//
// return create_subscription<MessageT>(
// topic_name, qos_profile, std::forward<CallbackT>(callback), sub_options,
// msg_mem_strategy);
// }

private:
using Heartbeat = autoware_control_center_msgs::msg::Heartbeat;
using NodeStatusActivity = autoware_control_center_msgs::msg::NodeStatusActivity;
using NodeStatusOperational = autoware_control_center_msgs::msg::NodeStatusOperational;
using ResultDeregistration = autoware_control_center_msgs::msg::ResultDeregistration;
using ResultRegistration = autoware_control_center_msgs::msg::ResultRegistration;
using Deregister = autoware_control_center_msgs::srv::Deregister;
using Register = autoware_control_center_msgs::srv::Register;

using FutureRegister = rclcpp::Client<autoware_control_center_msgs::srv::Register>::SharedFuture;

rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_;

rclcpp::Client<autoware_control_center_msgs::srv::Register>::SharedPtr cli_register_;

rclcpp::Publisher<autoware_control_center_msgs::msg::Heartbeat>::SharedPtr pub_heartbeat_;

uint16_t sequence_number_;

bool is_registered_;
double period_timer_register_ms_;
double period_heartbeat_ms_;
double deadline_ms_;

rclcpp::TimerBase::SharedPtr timer_registration_;
rclcpp::TimerBase::SharedPtr timer_heartbeat_;

unique_identifier_msgs::msg::UUID uuid_node_;

void on_tick_registration();
void on_tick_heartbeat();
void on_register(FutureRegister future);

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & state) override;

protected:
void destroy_node();
};
} // namespace autoware::node

#endif // AUTOWARE__NODE__NODE_HPP_
18 changes: 9 additions & 9 deletions common/autoware_node/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,20 +26,20 @@ namespace autoware::node
{
Node::Node(
const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options)
: LifecycleNode(node_name, ns, options),
sequence_number_{0},
is_registered_{false},
period_timer_register_ms_{declare_parameter<double>("period_timer_register_ms")},
period_heartbeat_ms_{declare_parameter<double>("period_heartbeat_ms")},
deadline_ms_{declare_parameter<double>("deadline_ms")}
: LifecycleNode(node_name, ns, options),
sequence_number_{0},
is_registered_{false},
period_timer_register_ms_{declare_parameter<double>("period_timer_register_ms")},
period_heartbeat_ms_{declare_parameter<double>("period_heartbeat_ms")},
deadline_ms_{declare_parameter<double>("deadline_ms")}
{
RCLCPP_DEBUG(
get_logger(), "Node %s constructor", get_node_base_interface()->get_fully_qualified_name());

rclcpp::QoS qos_profile(1);
qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC)
.liveliness_lease_duration(std::chrono::duration<double, std::milli>(deadline_ms_))
.deadline(std::chrono::duration<double, std::milli>(deadline_ms_));
.liveliness_lease_duration(std::chrono::duration<double, std::milli>(deadline_ms_))
.deadline(std::chrono::duration<double, std::milli>(deadline_ms_));

pub_heartbeat_ = this->create_publisher<autoware_control_center_msgs::msg::Heartbeat>(
"~/heartbeat", qos_profile);
Expand Down Expand Up @@ -123,4 +123,4 @@ void Node::destroy_node()
cli_register_.reset();
pub_heartbeat_.reset();
}
} // namespace autoware::node
} // namespace autoware::node
14 changes: 9 additions & 5 deletions common/autoware_test_node/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ You should see the reports coming from the nodes.
### Lifecycle control

Output a list of nodes with lifecycle:

```console
$ ros2 lifecycle nodes
/autoware/control_center
Expand All @@ -29,26 +30,29 @@ $ ros2 lifecycle nodes
```

Get the current state of a node:

```console
$ ros2 lifecycle get /test_node4
unconfigured [1]
```

List the available transitions for the node:

```console
$ ros2 lifecycle list /test_node4
- configure [1]
Start: unconfigured
Goal: configuring
Start: unconfigured
Goal: configuring
- shutdown [5]
Start: unconfigured
Goal: shuttingdown
Start: unconfigured
Goal: shuttingdown
```

Shutdown the node:

```console
$ ros2 lifecycle set /test_node4 shutdown
Transitioning successful
```

Check the `/autoware/control_center/node_reports` topic to see the node's `is_alive` field change to `false`.
Check the `/autoware/control_center/node_reports` topic to see the node's `is_alive` field change to `false`.
6 changes: 3 additions & 3 deletions common/autoware_test_node/src/test_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,13 +19,13 @@
namespace autoware::test_node
{
TestNode::TestNode(const rclcpp::NodeOptions & options)
: autoware::node::Node("test_node", "", options)
: autoware::node::Node("test_node", "", options)
{
RCLCPP_INFO(
get_logger(), "TestNode constructor with name %s",
this->get_node_base_interface()->get_fully_qualified_name());
}
} // namespace autoware::test_node
} // namespace autoware::test_node

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::test_node::TestNode)
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::test_node::TestNode)

0 comments on commit ce6afb8

Please sign in to comment.