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 Jun 24, 2024
1 parent 80930c4 commit 4abddaa
Show file tree
Hide file tree
Showing 13 changed files with 198 additions and 175 deletions.
44 changes: 21 additions & 23 deletions system/leader_election_converter/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -8,45 +8,43 @@ The leader election converter node is responsible for relaying UDP packets and R

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.


### Interface

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

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

## mrm converte

Check warning on line 19 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (converte)

Check warning on line 19 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (converte)

The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet.
In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`.

### Interface

| Interface Type | Interface Name | Data Type | Description |
| -------------- | ------------------------------------- | ------------------------------------------------- | ---------------------------------- |
| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_msgs/msg/MrmState` | MRM status of each ECU. |
| udp sender | none | `struct MrmState` | Same as above. |
| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. |
| udp receiver | none | `struct MrmRequest` | Same as above. |

| Interface Type | Interface Name | Data Type | Description |
| -------------- | ------------------------------ | ----------------------------------- | ------------------------ |
| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_msgs/msg/MrmState` | MRM status of each ECU. |
| udp sender | none | `struct MrmState` | Same as above. |
| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. |
| udp receiver | none | `struct MrmRequest` | Same as above. |

## log converter
The log converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`,

The log converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`,
`/system/election/status`, and `/system/fail_safe/over_all/mrm_state`.

### Interface

| Interface Type | Interface Name | Data Type | Description |
| -------------- | ------------------------------------- | ------------------------------------------------- | ---------------------------------- |
| udp receiver | none | `struct ElectionCommunication` | messages amoung election nodes. |
| udp receiver | none | `struct ElectionStatus` | Leader Election status. |
| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. |
| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. |
| publisher | `/system/fail_safe/over_all/mrm_state`| `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. |
| Interface Type | Interface Name | Data Type | Description |
| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------- |
| udp receiver | none | `struct ElectionCommunication` | messages amoung election nodes. |

Check warning on line 42 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (amoung)

Check warning on line 42 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (amoung)
| udp receiver | none | `struct ElectionStatus` | Leader Election status. |
| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. |

Check warning on line 44 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (amoung)

Check warning on line 44 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (amoung)
| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. |
| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. |

Check warning on line 46 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (wode)

Check warning on line 46 in system/leader_election_converter/README.md

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (wode)

## Parameters

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

Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<launch>
<launch>
<arg name="param_file" default="$(find-pkg-share leader_election_converter)/config/leader_electioin_converter.param.yaml"/>

Check warning on line 2 in system/leader_election_converter/launch/leader_election_converter.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-partial

Unknown word (electioin)

Check warning on line 2 in system/leader_election_converter/launch/leader_election_converter.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (electioin)
<arg name="input_control_mode" default="/vehicle/status/control_mode"/>
<arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
Expand Down
4 changes: 2 additions & 2 deletions system/leader_election_converter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,11 +10,11 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_system_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_adapi_v1_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "availability_converter.hpp"

#include "rclcpp/rclcpp.hpp"
#include "availability_converter.hpp"

namespace leader_election_converter
{

AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node)
: node_(node) {}
AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) : node_(node)
{
}

void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port)
{
Expand All @@ -30,22 +31,27 @@ void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std:
void AvailabilityConverter::setSubscriber()
{
const auto qos = rclcpp::QoS(1).transient_local();
availability_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
availability_callback_group_ =
node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions();
availability_options.callback_group = availability_callback_group_;

control_mode_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
control_mode_callback_group_ =
node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
rclcpp::SubscriptionOptions control_mode_options = rclcpp::SubscriptionOptions();
control_mode_options.callback_group = control_mode_callback_group_;
auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) {};
control_mode_options.callback_group = control_mode_callback_group_;
auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::
ControlModeReport::ConstSharedPtr msg) {};

sub_operation_mode_availability_ = node_->create_subscription<tier4_system_msgs::msg::OperationModeAvailability>(
"~/input/operation_mode_availability", qos,
std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1), availability_options);
sub_operation_mode_availability_ =
node_->create_subscription<tier4_system_msgs::msg::OperationModeAvailability>(
"~/input/operation_mode_availability", qos,
std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1),
availability_options);

sub_control_mode_ = node_->create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
"~/input/control_mode", qos, not_executed_callback, control_mode_options);

sub_control_mode_ =
node_->create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
"~/input/control_mode", qos, not_executed_callback, control_mode_options);
}

void AvailabilityConverter::convertToUdp(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef COMMON__AVAILABILITY_CONVERTER_HPP_
#define COMMON__AVAILABILITY_CONVERTER_HPP_
#ifndef COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_
#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_

#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
#include "udp_sender.hpp"

#include <rclcpp/rclcpp.hpp>

#include "udp_sender.hpp"

#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>

namespace leader_election_converter
{
Expand All @@ -41,25 +40,26 @@ struct Availability

class AvailabilityConverter
{

public:
AvailabilityConverter(rclcpp::Node * node);
~AvailabilityConverter() = default;

void setUdpSender(const std::string & dest_ip, const std::string & dest_port);
void setSubscriber();
void convertToUdp(const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg);
void convertToUdp(
const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg);

private:
rclcpp::Node * node_;
std::unique_ptr<UdpSender<Availability>> udp_availability_sender_;
rclcpp::CallbackGroup::SharedPtr availability_callback_group_;
rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr sub_control_mode_;
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr sub_operation_mode_availability_;

rclcpp::Node * node_;
std::unique_ptr<UdpSender<Availability>> udp_availability_sender_;
rclcpp::CallbackGroup::SharedPtr availability_callback_group_;
rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
sub_control_mode_;
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
sub_operation_mode_availability_;
};

} // namespace leader_election_converter

#endif // COMMON__AVAILABILITY_CONVERTER_HPP_
#endif // COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "rclcpp/rclcpp.hpp"
#include "log_converter.hpp"

#include "rclcpp/rclcpp.hpp"

namespace leader_election_converter
{

LogConverter::LogConverter(rclcpp::Node * node)
: node_(node),
is_election_comunication_running_(true),
is_election_status_running_(true)
{}
: node_(node), is_election_comunication_running_(true), is_election_status_running_(true)
{
}

LogConverter::~LogConverter()
{
Expand All @@ -38,15 +38,20 @@ LogConverter::~LogConverter()
}
}

void LogConverter::setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port)
void LogConverter::setUdpElectionCommunicatioinReceiver(
const std::string & src_ip, const std::string & src_port)
{
udp_election_comunication_thread_ = std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port);
udp_election_comunication_thread_ =
std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port);
}

void LogConverter::startUdpElectionCommunicationReceiver(const std::string & src_ip, const std::string & src_port)
void LogConverter::startUdpElectionCommunicationReceiver(
const std::string & src_ip, const std::string & src_port)
{
try {
udp_election_comunication_receiver_ = std::make_unique<UdpReceiver<ElectionCommunication>>(src_ip, src_port, std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1));
udp_election_comunication_receiver_ = std::make_unique<UdpReceiver<ElectionCommunication>>(
src_ip, src_port,
std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1));
while (is_election_comunication_running_) {
udp_election_comunication_receiver_->receive();
}
Expand All @@ -55,15 +60,20 @@ void LogConverter::startUdpElectionCommunicationReceiver(const std::string & src
}
}

void LogConverter::setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port)
void LogConverter::setUdpElectionStatusReceiver(
const std::string & src_ip, const std::string & src_port)
{
udp_election_status_thread_ = std::thread(&LogConverter::startUdpElectionStatusReceiver, this, src_ip, src_port);
udp_election_status_thread_ =
std::thread(&LogConverter::startUdpElectionStatusReceiver, this, src_ip, src_port);
}

void LogConverter::startUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port)
void LogConverter::startUdpElectionStatusReceiver(
const std::string & src_ip, const std::string & src_port)
{
try {
udp_election_status_receiver_ = std::make_unique<UdpReceiver<ElectionStatus>>(src_ip, src_port, std::bind(&LogConverter::convertElectionStatusToTopic, this, std::placeholders::_1));
udp_election_status_receiver_ = std::make_unique<UdpReceiver<ElectionStatus>>(
src_ip, src_port,
std::bind(&LogConverter::convertElectionStatusToTopic, this, std::placeholders::_1));
while (is_election_status_running_) {
udp_election_status_receiver_->receive();
}
Expand All @@ -74,8 +84,9 @@ void LogConverter::startUdpElectionStatusReceiver(const std::string & src_ip, co

void LogConverter::setPublisher()
{
pub_election_comunication_ = node_->create_publisher<tier4_system_msgs::msg::ElectionCommunication>(
"~/output/election_communication", rclcpp::QoS{1});
pub_election_comunication_ =
node_->create_publisher<tier4_system_msgs::msg::ElectionCommunication>(
"~/output/election_communication", rclcpp::QoS{1});
pub_election_status_ = node_->create_publisher<tier4_system_msgs::msg::ElectionStatus>(
"~/output/election_status", rclcpp::QoS{1});
pub_over_all_mrm_state_ = node_->create_publisher<autoware_adapi_v1_msgs::msg::MrmState>(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,21 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef COMMON__LOG_CONVERTER_HPP_
#define COMMON__LOG_CONVERTER_HPP_
#ifndef COMMON__CONVERTER__LOG_CONVERTER_HPP_
#define COMMON__CONVERTER__LOG_CONVERTER_HPP_

#include "udp_receiver.hpp"
#include "udp_sender.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <tier4_system_msgs/msg/election_communication.hpp>
#include <tier4_system_msgs/msg/election_status.hpp>
#include <tier4_system_msgs/msg/mrm_state.hpp>
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>

#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <atomic>

#include "udp_sender.hpp"
#include "udp_receiver.hpp"

#include <thread>

namespace leader_election_converter
{
Expand Down Expand Up @@ -59,20 +59,23 @@ class LogConverter
LogConverter(rclcpp::Node * node);
~LogConverter();

void setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port);
void setUdpElectionCommunicatioinReceiver(
const std::string & src_ip, const std::string & src_port);
void setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port);
void setPublisher();

private:
void startUdpElectionCommunicationReceiver(const std::string & src_ip, const std::string & src_port);
void startUdpElectionCommunicationReceiver(
const std::string & src_ip, const std::string & src_port);
void startUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port);
void convertElectionCommunicationToTopic(const ElectionCommunication & node_msg);
void convertElectionStatusToTopic(const ElectionStatus & status);

rclcpp::Node * node_;
std::unique_ptr<UdpReceiver<ElectionCommunication>> udp_election_comunication_receiver_;
std::unique_ptr<UdpReceiver<ElectionStatus>> udp_election_status_receiver_;
rclcpp::Publisher<tier4_system_msgs::msg::ElectionCommunication>::SharedPtr pub_election_comunication_;
rclcpp::Publisher<tier4_system_msgs::msg::ElectionCommunication>::SharedPtr
pub_election_comunication_;
rclcpp::Publisher<tier4_system_msgs::msg::ElectionStatus>::SharedPtr pub_election_status_;
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_over_all_mrm_state_;

Expand All @@ -84,4 +87,4 @@ class LogConverter

} // namespace leader_election_converter

#endif // COMMON__LOG_CONVERTER_HPP_
#endif // COMMON__CONVERTER__LOG_CONVERTER_HPP_
Loading

0 comments on commit 4abddaa

Please sign in to comment.