diff --git a/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.cpp b/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.cpp index 3ec06996423ee..cfaee363da550 100644 --- a/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.cpp +++ b/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.cpp @@ -25,8 +25,8 @@ DummyGearCmdPublisher::DummyGearCmdPublisher(const rclcpp::NodeOptions & node_op // Subscriber // Publisher - pub_gear_cmd_ = create_publisher( - "~/output/gear_cmd", 10); + pub_gear_cmd_ = + create_publisher("~/output/gear_cmd", 10); // Service @@ -34,13 +34,12 @@ DummyGearCmdPublisher::DummyGearCmdPublisher(const rclcpp::NodeOptions & node_op // Timer using namespace std::literals::chrono_literals; - timer_ = rclcpp::create_timer( - this, get_clock(), 1s, std::bind(&DummyGearCmdPublisher::onTimer, this)); + timer_ = + rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyGearCmdPublisher::onTimer, this)); // State // Diagnostics - } void DummyGearCmdPublisher::onTimer() diff --git a/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.hpp b/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.hpp index 4ac317d8fa479..35deaeb8abe32 100644 --- a/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.hpp +++ b/dummy/dummy_gear_cmd_publisher/src/dummy_gear_cmd_publisher.hpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_GEAR_CMD_PUBLISHER__DUMMY_GEAR_CMD_PUBLISHER_HPP_ -#define DUMMY_GEAR_CMD_PUBLISHER__DUMMY_GEAR_CMD_PUBLISHER_HPP_ +#ifndef DUMMY_GEAR_CMD_PUBLISHER_HPP_ +#define DUMMY_GEAR_CMD_PUBLISHER_HPP_ // include -#include #include +#include + namespace dummy_gear_cmd_publisher { @@ -48,8 +49,7 @@ class DummyGearCmdPublisher : public rclcpp::Node // State // Diagnostics - }; } // namespace dummy_gear_cmd_publisher -#endif // DUMMY_GEAR_CMD_PUBLISHER__DUMMY_GEAR_CMD_PUBLISHER_HPP_ +#endif // DUMMY_GEAR_CMD_PUBLISHER_HPP_ diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index b8b8e347ad32b..90190dc655bb4 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -30,13 +30,15 @@ from launch_ros.substitutions import FindPackageShare import yaml + def get_control_cmd_topic(context): - is_redundant = LaunchConfiguration('launch_redundancy_system_components').perform(context) - system_run_mode = LaunchConfiguration('system_run_mode').perform(context) + is_redundant = LaunchConfiguration("launch_redundancy_system_components").perform(context) + system_run_mode = LaunchConfiguration("system_run_mode").perform(context) + + if is_redundant.lower() == "true" and system_run_mode.lower() == "planning_simulation": + return "/main/control/command/control_cmd" + return "/control/command/control_cmd" - if is_redundant.lower() == 'true' and system_run_mode.lower() == 'planning_simulation': - return '/main/control/command/control_cmd' - return '/control/command/control_cmd' def launch_setup(context, *args, **kwargs): with open(LaunchConfiguration("vehicle_param_file").perform(context), "r") as f: diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp index c7405c34f88ba..921988ff33455 100644 --- a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp @@ -22,15 +22,13 @@ ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) : Node("control_cmd_switcher", node_options) { // Subscriber - sub_main_control_cmd_ = - create_subscription( - "~/input/main/control_cmd", rclcpp::QoS{10}, - std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1)); + sub_main_control_cmd_ = create_subscription( + "~/input/main/control_cmd", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1)); - sub_sub_control_cmd_ = - create_subscription( - "~/input/sub/control_cmd", rclcpp::QoS{10}, - std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1)); + sub_sub_control_cmd_ = create_subscription( + "~/input/sub/control_cmd", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1)); sub_election_status_main_ = create_subscription( "~/input/election/status/main", rclcpp::QoS{10}, @@ -41,8 +39,8 @@ ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); // Publisher - pub_control_cmd_ = create_publisher( - "~/output/control_cmd", rclcpp::QoS{1}); + pub_control_cmd_ = + create_publisher("~/output/control_cmd", rclcpp::QoS{1}); // Initialize use_main_control_cmd_ = true; diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp index ea118f3d869bb..a730c6cc630ac 100644 --- a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp @@ -34,21 +34,16 @@ class ControlCmdSwitcher : public rclcpp::Node private: // Subscribers - rclcpp::Subscription::SharedPtr - sub_main_control_cmd_; - rclcpp::Subscription::SharedPtr - sub_sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_main_control_cmd_; + rclcpp::Subscription::SharedPtr sub_sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_election_status_main_; rclcpp::Subscription::SharedPtr sub_election_status_sub_; - void onMainControlCmd( - const autoware_control_msgs::msg::Control::ConstSharedPtr msg); - void onSubControlCmd( - const autoware_control_msgs::msg::Control::ConstSharedPtr msg); + void onMainControlCmd(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); + void onSubControlCmd(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; std::atomic use_main_control_cmd_; }; diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp index 4dfafb6109373..abae7a67a5a3d 100644 --- a/system/mrm_stop_operator/src/mrm_stop_operator.cpp +++ b/system/mrm_stop_operator/src/mrm_stop_operator.cpp @@ -33,8 +33,9 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options) sub_velocity_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); rclcpp::SubscriptionOptions velocity_options = rclcpp::SubscriptionOptions(); velocity_options.callback_group = sub_velocity_group_; - auto not_executed_callback = []([[maybe_unused]] const typename autoware_vehicle_msgs::msg:: - VelocityReport::ConstSharedPtr msg) {}; + auto not_executed_callback = + []([[maybe_unused]] const typename autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr + msg) {}; sub_velocity_ = create_subscription( "~/input/velocity", 10, not_executed_callback, velocity_options); diff --git a/system/redundancy_switcher_interface/README.md b/system/redundancy_switcher_interface/README.md index 9314752796d41..87d8aa3c9c6b2 100644 --- a/system/redundancy_switcher_interface/README.md +++ b/system/redundancy_switcher_interface/README.md @@ -10,11 +10,11 @@ The availability converter subscribes `/system/operation_mode/availability` and ### 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_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_vehicle_msgs/msg/ControlModeReport` | Ego control mode. | +| udp sender | none | `struct Availability` | Combination of the above two. | ## mrm converter @@ -42,7 +42,7 @@ The log　converter receive udp packets into a structure called `ElectionCommuni | udp receiver | none | `struct ElectionCommunication` | messages among election nodes. | | udp receiver | none | `struct ElectionStatus` | Leader Election status. | | publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. | -| publisher | `/system/election/status` | `autoware_adapi_v1_msgs/msg/MrmState` | Leader Election status. | +| publisher | `/system/election/status` | `autoware_adapi_v1_msgs/msg/MrmState` | Leader Election status. | | publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wide MRM status. | ## Parameters diff --git a/system/redundancy_switcher_interface/script/relay_to_sub.py b/system/redundancy_switcher_interface/script/relay_to_sub.py index b5eea2c15dcd0..f0ee9c8b05083 100644 --- a/system/redundancy_switcher_interface/script/relay_to_sub.py +++ b/system/redundancy_switcher_interface/script/relay_to_sub.py @@ -55,11 +55,15 @@ def operation_mode_state_callback(self, msg): if msg.mode == 2: self.operation_mode_autonomous_state = True if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state: - self.get_logger().info(f"Operation mode changed: {self.operation_mode_autonomous_state}") + self.get_logger().info( + f"Operation mode changed: {self.operation_mode_autonomous_state}" + ) else: self.operation_mode_autonomous_state = False if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state: - self.get_logger().info(f"Operation mode changed: {self.operation_mode_autonomous_state}") + self.get_logger().info( + f"Operation mode changed: {self.operation_mode_autonomous_state}" + ) def trajectory_callback(self, msg): if self.autonomous_mode or self.operation_mode_autonomous_state == False: diff --git a/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp index 8c14fcb191a63..bf68cc133be66 100644 --- a/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp +++ b/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp @@ -43,8 +43,9 @@ void AvailabilityConverter::setSubscriber() 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_vehicle_msgs::msg:: - ControlModeReport::ConstSharedPtr msg) {}; + auto not_executed_callback = + []([[maybe_unused]] const typename autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr + msg) {}; sub_operation_mode_availability_ = node_->create_subscription( @@ -52,9 +53,8 @@ void AvailabilityConverter::setSubscriber() std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1), availability_options); - sub_control_mode_ = - node_->create_subscription( - "~/input/control_mode", qos, not_executed_callback, control_mode_options); + sub_control_mode_ = node_->create_subscription( + "~/input/control_mode", qos, not_executed_callback, control_mode_options); } void AvailabilityConverter::convertToUdp( diff --git a/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp b/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp index 1e7ddc825c7ef..e4f473088db04 100644 --- a/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp +++ b/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp @@ -56,8 +56,7 @@ class AvailabilityConverter std::unique_ptr> udp_availability_sender_; rclcpp::CallbackGroup::SharedPtr availability_callback_group_; rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; };