diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp index bb5c5ba377083..b8840e0d3ebb7 100644 --- a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp @@ -34,12 +34,12 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio // Timer using namespace std::literals::chrono_literals; - timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this)); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this)); // State // Diagnostics - } void DummyOperationModePublisher::onTimer() diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp index 70020d88a762e..43ee5b756451b 100644 --- a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ -#define DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#ifndef DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#define DUMMY_OPERATION_MODE_PUBLISHER_HPP_ // include #include #include - namespace dummy_operation_mode_publisher { @@ -36,7 +35,8 @@ class DummyOperationModePublisher : public rclcpp::Node // Subscriber // Publisher - rclcpp::Publisher::SharedPtr pub_operation_mode_state_; + rclcpp::Publisher::SharedPtr + pub_operation_mode_state_; // Service @@ -50,8 +50,7 @@ class DummyOperationModePublisher : public rclcpp::Node // State // Diagnostics - }; } // namespace dummy_operation_mode_publisher -#endif // DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ \ No newline at end of file +#endif // DUMMY_OPERATION_MODE_PUBLISHER_HPP_ diff --git a/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml index fbd38dccd80ec..fac0c77c5d8ee 100644 --- a/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml +++ b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml @@ -160,7 +160,7 @@ - + @@ -187,6 +187,4 @@ --> - - diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 42c57a7b4a22b..d9cb839307c96 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,11 +12,11 @@ autoware_cmake component_state_monitor + control_cmd_switcher emergency_handler + leader_election_converter system_error_monitor system_monitor - control_cmd_switcher - leader_election_converter ament_lint_auto autoware_lint_common diff --git a/system/control_cmd_switcher/README.md b/system/control_cmd_switcher/README.md index f1e018734e2d1..7c688a50919a6 100644 --- a/system/control_cmd_switcher/README.md +++ b/system/control_cmd_switcher/README.md @@ -1 +1 @@ -# control_cmd_switcher \ No newline at end of file +# control_cmd_switcher diff --git a/system/control_cmd_switcher/config/control_cmd_switcher.yaml b/system/control_cmd_switcher/config/control_cmd_switcher.yaml index e083b38a5ebe9..b908163301d69 100644 --- a/system/control_cmd_switcher/config/control_cmd_switcher.yaml +++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml @@ -2,4 +2,3 @@ --- /**: ros__parameters: - diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml index 7e0fa2bf2e97b..8ed70f43f51f1 100644 --- a/system/control_cmd_switcher/package.xml +++ b/system/control_cmd_switcher/package.xml @@ -13,8 +13,8 @@ autoware_auto_control_msgs rclcpp + rclcpp_components tier4_system_msgs - rclcpp_components ament_lint_auto autoware_lint_common 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 b038264a7af99..7aae994e3ab9d 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 @@ -18,17 +18,23 @@ #include #include -ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) : Node("control_cmd_switcher", node_options) +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 = create_subscription( - "~/input/election/status", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); + "~/input/election/status", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); // Publisher pub_control_cmd_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}); @@ -37,21 +43,24 @@ ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) use_main_control_cmd_ = true; } -void ControlCmdSwitcher::onMainControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +void ControlCmdSwitcher::onMainControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) { if (use_main_control_cmd_) { pub_control_cmd_->publish(*msg); } } -void ControlCmdSwitcher::onSubControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +void ControlCmdSwitcher::onSubControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) { if (!use_main_control_cmd_) { pub_control_cmd_->publish(*msg); } } -void ControlCmdSwitcher::onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) +void ControlCmdSwitcher::onElectionStatus( + const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) { if (((msg->path_info >> 3) & 0x01) == 1) { use_main_control_cmd_ = true; @@ -62,4 +71,3 @@ void ControlCmdSwitcher::onElectionStatus(const tier4_system_msgs::msg::Election #include RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher) - 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 1ebdf0b51551d..1f17b63eaadc3 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 @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ -#define CONTROL_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#ifndef CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ // Core +#include #include #include -#include - // Autoware #include @@ -28,28 +27,29 @@ // ROS 2 core #include - class ControlCmdSwitcher : public rclcpp::Node { public: explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options); 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; - void onMainControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); - void onSubControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onMainControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onSubControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::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_; }; -#endif // CONTROL_SWITCHER__CONTROL_SWITCHER_HPP_ +#endif // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ diff --git a/system/control_cmd_switcher/tool/rely_trajectory.py b/system/control_cmd_switcher/tool/rely_trajectory.py index b2dbef5ba4965..ef28badafdf70 100755 --- a/system/control_cmd_switcher/tool/rely_trajectory.py +++ b/system/control_cmd_switcher/tool/rely_trajectory.py @@ -1,26 +1,28 @@ #!/usr/bin/env python3 +import threading + +from autoware_auto_planning_msgs.msg import Trajectory import rclpy from rclpy.node import Node -from autoware_auto_planning_msgs.msg import Trajectory -import threading -class RelayTrajectoryNode(Node): +class RelayTrajectoryNode(Node): def __init__(self): - super().__init__('relay_trajectory') + super().__init__("relay_trajectory") self.subscription = self.create_subscription( - Trajectory, - '/tmp/planning/scenario_planning/trajectory', - self.listener_callback, - 10) - self.publisher = self.create_publisher(Trajectory, '/planning/scenario_planning/trajectory', 10) + Trajectory, "/tmp/planning/scenario_planning/trajectory", self.listener_callback, 10 + ) + self.publisher = self.create_publisher( + Trajectory, "/planning/scenario_planning/trajectory", 10 + ) self.running = True def listener_callback(self, msg): if self.running: self.publisher.publish(msg) + def main(args=None): rclpy.init(args=args) node = RelayTrajectoryNode() @@ -29,7 +31,7 @@ def input_thread(): nonlocal node while True: user_input = input("Enter 'y' to stop publishing: ") - if user_input.lower() == 'y': + if user_input.lower() == "y": node.running = False print("Publishing stopped.") break @@ -43,5 +45,6 @@ def input_thread(): node.destroy_node() rclpy.shutdown() -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp index f4bba88fe0581..f36203c66b4d6 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.cpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -33,7 +33,7 @@ void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std: void AvailabilityConverter::setSubscriber() { - const auto qos = rclcpp::QoS(1).transient_local(); + const auto qos = rclcpp::QoS(1); availability_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions(); @@ -75,7 +75,8 @@ void AvailabilityConverter::convertToUdp( availability.pull_over = availability_msg->pull_over; udp_availability_sender_->send(availability); } else { - RCLCPP_ERROR(node_->get_logger(), "Failed to take control mode report"); + RCLCPP_ERROR_THROTTLE( + node_->get_logger(), *node->get_clock(), 5000, "Failed to take control mode report"); } } diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index a7aa44d702371..9a0b798331e4a 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -25,13 +25,13 @@ // Autoware #include -#include -#include #include #include #include #include +#include #include +#include #include #include diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 7c05ef1f18998..700a2f1125b5d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -227,8 +227,8 @@ void MrmHandler::publishMrmState() void MrmHandler::operateMrm() { - using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; if (mrm_state_.state == MrmState::NORMAL) { // Cancel MRM behavior when returning to NORMAL state @@ -271,8 +271,8 @@ void MrmHandler::operateMrm() void MrmHandler::handleFailedRequest() { - using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; if (requestMrmBehavior(MrmBehavior::EMERGENCY_STOP, CALL)) { if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); @@ -286,8 +286,8 @@ bool MrmHandler::requestMrmBehavior( const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior, RequestType request_type) const { - using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; auto request = std::make_shared(); if (request_type == RequestType::CALL) { @@ -465,9 +465,9 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { - using tier4_system_msgs::msg::MrmState; - using tier4_system_msgs::msg::MrmBehavior; using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; // Check emergency const bool is_emergency = isEmergency(); @@ -522,8 +522,8 @@ void MrmHandler::updateMrmState() tier4_system_msgs::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior() { - using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::OperationModeAvailability; // State machine diff --git a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml index 5dfbb2956d668..7043b61596f76 100644 --- a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml +++ b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml @@ -3,4 +3,3 @@ min_acceleration: -4.0 # min acceleration for sub ecu mrm stop [m/s^2] max_jerk: 5.0 # max jerk for sub ecu mrm stop [m/s^3] min_jerk: -5.0 # min jerk for sub ecu mrm stop [m/s^3] - diff --git a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml index 9d5226cf99f19..58305c509c70c 100644 --- a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp index d403e886ed7a1..7ffc8aa276326 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) // Publisher pub_velocity_limit_ = create_publisher( "~/output/velocity_limit", rclcpp::QoS{10}.transient_local()); - pub_velocity_limit_clear_command_ = create_publisher( - "~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local()); + pub_velocity_limit_clear_command_ = + create_publisher( + "~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local()); // Timer @@ -48,13 +49,13 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options) initState(); // Diagnostics - } void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg) { - if (msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP && - last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) { + if ( + msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP && + last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) { tier4_planning_msgs::msg::VelocityLimit velocity_limit; velocity_limit.stamp = this->now(); velocity_limit.max_velocity = 0.0; @@ -64,7 +65,7 @@ void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::Co velocity_limit.constraints.min_jerk = params_.min_jerk; velocity_limit.sender = "mrm_stop_operator"; pub_velocity_limit_->publish(velocity_limit); - + last_mrm_request_ = *msg; } } diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.hpp b/system/mrm_stop_operator/src/mrm_stop_operator.hpp index 750c343f13062..a44b2ed4f1988 100644 --- a/system/mrm_stop_operator/src/mrm_stop_operator.hpp +++ b/system/mrm_stop_operator/src/mrm_stop_operator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ -#define MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ +#ifndef MRM_STOP_OPERATOR_HPP_ +#define MRM_STOP_OPERATOR_HPP_ // include #include @@ -51,7 +51,8 @@ class MrmStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_clear_command_; + rclcpp::Publisher::SharedPtr + pub_velocity_limit_clear_command_; // Service @@ -61,12 +62,11 @@ class MrmStopOperator : public rclcpp::Node // State tier4_system_msgs::msg::MrmBehavior last_mrm_request_; - + void initState(); // Diagnostics - }; } // namespace mrm_stop_operator -#endif // MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ \ No newline at end of file +#endif // MRM_STOP_OPERATOR_HPP_