diff --git a/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml b/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml
index ba1525699940b..4c49835a57e5b 100644
--- a/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml
+++ b/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml
@@ -3,5 +3,6 @@
+
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 b8840e0d3ebb7..876368d69b4d3 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
@@ -27,6 +27,9 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio
// Publisher
pub_operation_mode_state_ = create_publisher(
"~/output/operation_mode_state", 10);
+ pub_operation_mode_state_adapi_ =
+ create_publisher(
+ "~/output/operation_mode_state_adapi", 10);
// Service
@@ -55,6 +58,7 @@ void DummyOperationModePublisher::onTimer()
msg.is_remote_mode_available = true;
pub_operation_mode_state_->publish(msg);
+ pub_operation_mode_state_adapi_->publish(msg);
}
} // namespace dummy_operation_mode_publisher
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 43ee5b756451b..7e7f574d8b4db 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
@@ -37,6 +37,8 @@ class DummyOperationModePublisher : public rclcpp::Node
// Publisher
rclcpp::Publisher::SharedPtr
pub_operation_mode_state_;
+ rclcpp::Publisher::SharedPtr
+ pub_operation_mode_state_adapi_;
// Service
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
index a117a0ff129f1..27db245d656bf 100644
--- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
+++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -336,7 +336,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
RCLCPP_INFO(get_logger(), "external velocity limit pointer exist!!!");
// sender
external_velocity_limit_.sender = external_velocity_limit_ptr_->sender;
- RCLCPP_INFO(get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());
+ RCLCPP_INFO(
+ get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());
// on the first time, apply directly
if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
@@ -378,7 +379,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
external_velocity_limit_ptr_->use_constraints ? cstr.max_jerk : smoother_->getMaxJerk();
const auto j_min =
external_velocity_limit_ptr_->use_constraints ? cstr.min_jerk : smoother_->getMinJerk();
- RCLCPP_INFO(get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);
+ RCLCPP_INFO(
+ get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);
// If the closest acceleration is positive, velocity will increase
// until the acceleration becomes zero
@@ -388,7 +390,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
} else {
max_velocity_with_deceleration_ = v0;
}
- RCLCPP_INFO(get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);
+ RCLCPP_INFO(
+ get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);
if (external_velocity_limit_ptr_->max_velocity < max_velocity_with_deceleration_) {
// TODO(mkuri) If v0 < external_velocity_limit_ptr_->max_velocity <
@@ -409,11 +412,13 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
RCLCPP_WARN(get_logger(), "Stop distance calculation failed!");
}
external_velocity_limit_.dist = stop_dist + margin;
- RCLCPP_INFO(get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
+ RCLCPP_INFO(
+ get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
} else {
max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity;
external_velocity_limit_.dist = 0.0;
- RCLCPP_INFO(get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
+ RCLCPP_INFO(
+ get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
}
}
}
@@ -910,7 +915,8 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t
// apply external velocity limit from the inserted point
trajectory_utils::applyMaximumVelocityLimit(
*inserted_index, traj.size(), external_velocity_limit_.velocity, traj);
- RCLCPP_INFO(get_logger(), "apply external velocity limit: dist = %f, vel = %f",
+ RCLCPP_INFO(
+ get_logger(), "apply external velocity limit: dist = %f, vel = %f",
external_velocity_limit_.dist, external_velocity_limit_.velocity);
// create virtual wall
diff --git a/system/leader_election_converter/package.xml b/system/leader_election_converter/package.xml
index 47af75bcf8433..2ce13c3cfec7b 100644
--- a/system/leader_election_converter/package.xml
+++ b/system/leader_election_converter/package.xml
@@ -11,13 +11,12 @@
autoware_cmake
autoware_adapi_v1_msgs
+ autoware_auto_planning_msgs
autoware_auto_vehicle_msgs
+ geometry_msgs
rclcpp
rclcpp_components
tier4_system_msgs
- autoware_auto_planning_msgs
- geometry_msgs
-
ament_lint_auto
autoware_lint_common
diff --git a/system/leader_election_converter/script/relay_to_sub.py b/system/leader_election_converter/script/relay_to_sub.py
index c57b467b11cf4..949c38eb7b049 100644
--- a/system/leader_election_converter/script/relay_to_sub.py
+++ b/system/leader_election_converter/script/relay_to_sub.py
@@ -1,25 +1,43 @@
#!/usr/bin/env python3
-import rclpy
-from rclpy.node import Node
from autoware_adapi_v1_msgs.msg import OperationModeState
from autoware_auto_planning_msgs.msg import Trajectory
from geometry_msgs.msg import PoseWithCovarianceStamped
+import rclpy
+from rclpy.node import Node
from tier4_system_msgs.msg import OperationModeAvailability
-class RemapNode(Node):
+class RemapNode(Node):
def __init__(self):
- super().__init__('remap_node')
+ super().__init__("remap_node")
- self.create_subscription(OperationModeAvailability, '/system/operation_mode/availability', self.operation_mode_callback, 10)
- self.create_subscription(OperationModeState, '/system/operation_mode/state', self.operation_mode_state_callback, 10)
- self.sub_trajectory = self.create_subscription(Trajectory, '/planning/scenario_planning/trajectory', self.trajectory_callback, 10)
- self.sub_pose_with_covariance = self.create_subscription(PoseWithCovarianceStamped, '/localization/pose_with_covariance', self.pose_callback, 10)
+ self.create_subscription(
+ OperationModeAvailability,
+ "/system/operation_mode/availability",
+ self.operation_mode_callback,
+ 10,
+ )
+ self.create_subscription(
+ OperationModeState,
+ "/system/operation_mode/state",
+ self.operation_mode_state_callback,
+ 10,
+ )
+ self.sub_trajectory = self.create_subscription(
+ Trajectory, "/planning/scenario_planning/trajectory", self.trajectory_callback, 10
+ )
+ self.sub_pose_with_covariance = self.create_subscription(
+ PoseWithCovarianceStamped, "/localization/pose_with_covariance", self.pose_callback, 10
+ )
# self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10)
- self.pub_trajectory = self.create_publisher(Trajectory, '/to_sub/planning/scenario_planning/trajectory', 10)
- self.pub_pose_with_covariance = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/localization/pose_with_covariance', 10)
+ self.pub_trajectory = self.create_publisher(
+ Trajectory, "/to_sub/planning/scenario_planning/trajectory", 10
+ )
+ self.pub_pose_with_covariance = self.create_publisher(
+ PoseWithCovarianceStamped, "/to_sub/localization/pose_with_covariance", 10
+ )
# self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10)
self.autonomous_mode = False
@@ -36,7 +54,6 @@ def operation_mode_state_callback(self, msg):
self.operation_mode_autonomous_state == True
else:
self.operation_mode_autonomous_state == False
-
def trajectory_callback(self, msg):
if self.autonomous_mode or self.operation_mode_autonomous_state == False:
@@ -50,6 +67,7 @@ def pose_callback(self, msg):
# if self.autonomous_mode:
# self.pub_initialpose3d.publish(msg)
+
def main(args=None):
rclpy.init(args=args)
node = RemapNode()
@@ -57,5 +75,6 @@ def main(args=None):
node.destroy_node()
rclpy.shutdown()
-if __name__ == '__main__':
+
+if __name__ == "__main__":
main()