Skip to content

Commit 4abddaa

Browse files
style(pre-commit): autofix
1 parent 80930c4 commit 4abddaa

13 files changed

+198
-175
lines changed

system/leader_election_converter/README.md

+21-23
Original file line numberDiff line numberDiff line change
@@ -8,45 +8,43 @@ The leader election converter node is responsible for relaying UDP packets and R
88

99
The availability converter subscribes `/system/operation_mode/availability` and `/vehicle/status/mrm_state`, adds them together into a structure called `Availability` and sends it as a udp packet.
1010

11-
1211
### Interface
1312

14-
| Interface Type | Interface Name | Data Type | Description |
15-
| -------------- | ------------------------------------- | ------------------------------------------------- | ---------------------------------- |
16-
| subscriber | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | Usable behavior of the ego. |
17-
| subscriber | `/vehicle/status/mrm_state` | `autoware_auto_vehicle_msgs/msg/ControlModeReport`| Ego control mode. |
18-
| udp sender | none | `struct Availability` | Combination of the above two. |
19-
13+
| Interface Type | Interface Name | Data Type | Description |
14+
| -------------- | ------------------------------------- | -------------------------------------------------- | ----------------------------- |
15+
| subscriber | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | Usable behavior of the ego. |
16+
| subscriber | `/vehicle/status/mrm_state` | `autoware_auto_vehicle_msgs/msg/ControlModeReport` | Ego control mode. |
17+
| udp sender | none | `struct Availability` | Combination of the above two. |
2018

2119
## mrm converte
20+
2221
The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet.
2322
In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`.
2423

2524
### Interface
2625

27-
| Interface Type | Interface Name | Data Type | Description |
28-
| -------------- | ------------------------------------- | ------------------------------------------------- | ---------------------------------- |
29-
| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_msgs/msg/MrmState` | MRM status of each ECU. |
30-
| udp sender | none | `struct MrmState` | Same as above. |
31-
| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. |
32-
| udp receiver | none | `struct MrmRequest` | Same as above. |
33-
26+
| Interface Type | Interface Name | Data Type | Description |
27+
| -------------- | ------------------------------ | ----------------------------------- | ------------------------ |
28+
| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_msgs/msg/MrmState` | MRM status of each ECU. |
29+
| udp sender | none | `struct MrmState` | Same as above. |
30+
| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. |
31+
| udp receiver | none | `struct MrmRequest` | Same as above. |
3432

3533
## log converter
36-
The log converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`,
34+
35+
The log converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`,
3736
`/system/election/status`, and `/system/fail_safe/over_all/mrm_state`.
3837

3938
### Interface
4039

41-
| Interface Type | Interface Name | Data Type | Description |
42-
| -------------- | ------------------------------------- | ------------------------------------------------- | ---------------------------------- |
43-
| udp receiver | none | `struct ElectionCommunication` | messages amoung election nodes. |
44-
| udp receiver | none | `struct ElectionStatus` | Leader Election status. |
45-
| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. |
46-
| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. |
47-
| publisher | `/system/fail_safe/over_all/mrm_state`| `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. |
40+
| Interface Type | Interface Name | Data Type | Description |
41+
| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------- |
42+
| udp receiver | none | `struct ElectionCommunication` | messages amoung election nodes. |
43+
| udp receiver | none | `struct ElectionStatus` | Leader Election status. |
44+
| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. |
45+
| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. |
46+
| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. |
4847

4948
## Parameters
5049

5150
{{ json_to_markdown("system/leader_election_converter/schema/leader_election_converter.schema.json") }}
52-

system/leader_election_converter/launch/leader_election_converter.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
<launch>
1+
<launch>
22
<arg name="param_file" default="$(find-pkg-share leader_election_converter)/config/leader_electioin_converter.param.yaml"/>
33
<arg name="input_control_mode" default="/vehicle/status/control_mode"/>
44
<arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>

system/leader_election_converter/package.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -10,11 +10,11 @@
1010
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1111
<buildtool_depend>autoware_cmake</buildtool_depend>
1212

13+
<depend>autoware_adapi_v1_msgs</depend>
14+
<depend>autoware_auto_vehicle_msgs</depend>
1315
<depend>rclcpp</depend>
1416
<depend>rclcpp_components</depend>
1517
<depend>tier4_system_msgs</depend>
16-
<depend>autoware_auto_vehicle_msgs</depend>
17-
<depend>autoware_adapi_v1_msgs</depend>
1818

1919
<test_depend>ament_lint_auto</test_depend>
2020
<test_depend>autoware_lint_common</test_depend>

system/leader_election_converter/src/common/converter/availability_converter.cpp

+19-13
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,16 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include "availability_converter.hpp"
1516

1617
#include "rclcpp/rclcpp.hpp"
17-
#include "availability_converter.hpp"
1818

1919
namespace leader_election_converter
2020
{
2121

22-
AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node)
23-
: node_(node) {}
22+
AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) : node_(node)
23+
{
24+
}
2425

2526
void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port)
2627
{
@@ -30,22 +31,27 @@ void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std:
3031
void AvailabilityConverter::setSubscriber()
3132
{
3233
const auto qos = rclcpp::QoS(1).transient_local();
33-
availability_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
34+
availability_callback_group_ =
35+
node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
3436
rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions();
3537
availability_options.callback_group = availability_callback_group_;
3638

37-
control_mode_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
39+
control_mode_callback_group_ =
40+
node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
3841
rclcpp::SubscriptionOptions control_mode_options = rclcpp::SubscriptionOptions();
39-
control_mode_options.callback_group = control_mode_callback_group_;
40-
auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) {};
42+
control_mode_options.callback_group = control_mode_callback_group_;
43+
auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::
44+
ControlModeReport::ConstSharedPtr msg) {};
4145

42-
sub_operation_mode_availability_ = node_->create_subscription<tier4_system_msgs::msg::OperationModeAvailability>(
43-
"~/input/operation_mode_availability", qos,
44-
std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1), availability_options);
46+
sub_operation_mode_availability_ =
47+
node_->create_subscription<tier4_system_msgs::msg::OperationModeAvailability>(
48+
"~/input/operation_mode_availability", qos,
49+
std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1),
50+
availability_options);
4551

46-
sub_control_mode_ = node_->create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
47-
"~/input/control_mode", qos, not_executed_callback, control_mode_options);
48-
52+
sub_control_mode_ =
53+
node_->create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
54+
"~/input/control_mode", qos, not_executed_callback, control_mode_options);
4955
}
5056

5157
void AvailabilityConverter::convertToUdp(

system/leader_election_converter/src/common/converter/availability_converter.hpp

+16-16
Original file line numberDiff line numberDiff line change
@@ -12,16 +12,15 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef COMMON__AVAILABILITY_CONVERTER_HPP_
16-
#define COMMON__AVAILABILITY_CONVERTER_HPP_
15+
#ifndef COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_
16+
#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_
1717

18-
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
19-
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
18+
#include "udp_sender.hpp"
2019

2120
#include <rclcpp/rclcpp.hpp>
2221

23-
#include "udp_sender.hpp"
24-
22+
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
23+
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
2524

2625
namespace leader_election_converter
2726
{
@@ -41,25 +40,26 @@ struct Availability
4140

4241
class AvailabilityConverter
4342
{
44-
4543
public:
4644
AvailabilityConverter(rclcpp::Node * node);
4745
~AvailabilityConverter() = default;
4846

4947
void setUdpSender(const std::string & dest_ip, const std::string & dest_port);
5048
void setSubscriber();
51-
void convertToUdp(const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg);
49+
void convertToUdp(
50+
const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg);
5251

5352
private:
54-
rclcpp::Node * node_;
55-
std::unique_ptr<UdpSender<Availability>> udp_availability_sender_;
56-
rclcpp::CallbackGroup::SharedPtr availability_callback_group_;
57-
rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_;
58-
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr sub_control_mode_;
59-
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr sub_operation_mode_availability_;
60-
53+
rclcpp::Node * node_;
54+
std::unique_ptr<UdpSender<Availability>> udp_availability_sender_;
55+
rclcpp::CallbackGroup::SharedPtr availability_callback_group_;
56+
rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_;
57+
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
58+
sub_control_mode_;
59+
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
60+
sub_operation_mode_availability_;
6161
};
6262

6363
} // namespace leader_election_converter
6464

65-
#endif // COMMON__AVAILABILITY_CONVERTER_HPP_
65+
#endif // COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_

system/leader_election_converter/src/common/converter/log_converter.cpp

+26-15
Original file line numberDiff line numberDiff line change
@@ -12,17 +12,17 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "rclcpp/rclcpp.hpp"
1615
#include "log_converter.hpp"
1716

17+
#include "rclcpp/rclcpp.hpp"
18+
1819
namespace leader_election_converter
1920
{
2021

2122
LogConverter::LogConverter(rclcpp::Node * node)
22-
: node_(node),
23-
is_election_comunication_running_(true),
24-
is_election_status_running_(true)
25-
{}
23+
: node_(node), is_election_comunication_running_(true), is_election_status_running_(true)
24+
{
25+
}
2626

2727
LogConverter::~LogConverter()
2828
{
@@ -38,15 +38,20 @@ LogConverter::~LogConverter()
3838
}
3939
}
4040

41-
void LogConverter::setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port)
41+
void LogConverter::setUdpElectionCommunicatioinReceiver(
42+
const std::string & src_ip, const std::string & src_port)
4243
{
43-
udp_election_comunication_thread_ = std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port);
44+
udp_election_comunication_thread_ =
45+
std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port);
4446
}
4547

46-
void LogConverter::startUdpElectionCommunicationReceiver(const std::string & src_ip, const std::string & src_port)
48+
void LogConverter::startUdpElectionCommunicationReceiver(
49+
const std::string & src_ip, const std::string & src_port)
4750
{
4851
try {
49-
udp_election_comunication_receiver_ = std::make_unique<UdpReceiver<ElectionCommunication>>(src_ip, src_port, std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1));
52+
udp_election_comunication_receiver_ = std::make_unique<UdpReceiver<ElectionCommunication>>(
53+
src_ip, src_port,
54+
std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1));
5055
while (is_election_comunication_running_) {
5156
udp_election_comunication_receiver_->receive();
5257
}
@@ -55,15 +60,20 @@ void LogConverter::startUdpElectionCommunicationReceiver(const std::string & src
5560
}
5661
}
5762

58-
void LogConverter::setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port)
63+
void LogConverter::setUdpElectionStatusReceiver(
64+
const std::string & src_ip, const std::string & src_port)
5965
{
60-
udp_election_status_thread_ = std::thread(&LogConverter::startUdpElectionStatusReceiver, this, src_ip, src_port);
66+
udp_election_status_thread_ =
67+
std::thread(&LogConverter::startUdpElectionStatusReceiver, this, src_ip, src_port);
6168
}
6269

63-
void LogConverter::startUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port)
70+
void LogConverter::startUdpElectionStatusReceiver(
71+
const std::string & src_ip, const std::string & src_port)
6472
{
6573
try {
66-
udp_election_status_receiver_ = std::make_unique<UdpReceiver<ElectionStatus>>(src_ip, src_port, std::bind(&LogConverter::convertElectionStatusToTopic, this, std::placeholders::_1));
74+
udp_election_status_receiver_ = std::make_unique<UdpReceiver<ElectionStatus>>(
75+
src_ip, src_port,
76+
std::bind(&LogConverter::convertElectionStatusToTopic, this, std::placeholders::_1));
6777
while (is_election_status_running_) {
6878
udp_election_status_receiver_->receive();
6979
}
@@ -74,8 +84,9 @@ void LogConverter::startUdpElectionStatusReceiver(const std::string & src_ip, co
7484

7585
void LogConverter::setPublisher()
7686
{
77-
pub_election_comunication_ = node_->create_publisher<tier4_system_msgs::msg::ElectionCommunication>(
78-
"~/output/election_communication", rclcpp::QoS{1});
87+
pub_election_comunication_ =
88+
node_->create_publisher<tier4_system_msgs::msg::ElectionCommunication>(
89+
"~/output/election_communication", rclcpp::QoS{1});
7990
pub_election_status_ = node_->create_publisher<tier4_system_msgs::msg::ElectionStatus>(
8091
"~/output/election_status", rclcpp::QoS{1});
8192
pub_over_all_mrm_state_ = node_->create_publisher<autoware_adapi_v1_msgs::msg::MrmState>(

system/leader_election_converter/src/common/converter/log_converter.hpp

+16-13
Original file line numberDiff line numberDiff line change
@@ -12,21 +12,21 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef COMMON__LOG_CONVERTER_HPP_
16-
#define COMMON__LOG_CONVERTER_HPP_
15+
#ifndef COMMON__CONVERTER__LOG_CONVERTER_HPP_
16+
#define COMMON__CONVERTER__LOG_CONVERTER_HPP_
1717

18+
#include "udp_receiver.hpp"
19+
#include "udp_sender.hpp"
20+
21+
#include <rclcpp/rclcpp.hpp>
22+
23+
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
1824
#include <tier4_system_msgs/msg/election_communication.hpp>
1925
#include <tier4_system_msgs/msg/election_status.hpp>
2026
#include <tier4_system_msgs/msg/mrm_state.hpp>
21-
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
2227

23-
#include <rclcpp/rclcpp.hpp>
24-
#include <thread>
2528
#include <atomic>
26-
27-
#include "udp_sender.hpp"
28-
#include "udp_receiver.hpp"
29-
29+
#include <thread>
3030

3131
namespace leader_election_converter
3232
{
@@ -59,20 +59,23 @@ class LogConverter
5959
LogConverter(rclcpp::Node * node);
6060
~LogConverter();
6161

62-
void setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port);
62+
void setUdpElectionCommunicatioinReceiver(
63+
const std::string & src_ip, const std::string & src_port);
6364
void setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port);
6465
void setPublisher();
6566

6667
private:
67-
void startUdpElectionCommunicationReceiver(const std::string & src_ip, const std::string & src_port);
68+
void startUdpElectionCommunicationReceiver(
69+
const std::string & src_ip, const std::string & src_port);
6870
void startUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port);
6971
void convertElectionCommunicationToTopic(const ElectionCommunication & node_msg);
7072
void convertElectionStatusToTopic(const ElectionStatus & status);
7173

7274
rclcpp::Node * node_;
7375
std::unique_ptr<UdpReceiver<ElectionCommunication>> udp_election_comunication_receiver_;
7476
std::unique_ptr<UdpReceiver<ElectionStatus>> udp_election_status_receiver_;
75-
rclcpp::Publisher<tier4_system_msgs::msg::ElectionCommunication>::SharedPtr pub_election_comunication_;
77+
rclcpp::Publisher<tier4_system_msgs::msg::ElectionCommunication>::SharedPtr
78+
pub_election_comunication_;
7679
rclcpp::Publisher<tier4_system_msgs::msg::ElectionStatus>::SharedPtr pub_election_status_;
7780
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_over_all_mrm_state_;
7881

@@ -84,4 +87,4 @@ class LogConverter
8487

8588
} // namespace leader_election_converter
8689

87-
#endif // COMMON__LOG_CONVERTER_HPP_
90+
#endif // COMMON__CONVERTER__LOG_CONVERTER_HPP_

0 commit comments

Comments
 (0)