From 84fe2de79cf3c7ba5c8f2908acc56b419dbc46b7 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 5 Sep 2024 14:33:22 +0900 Subject: [PATCH] feat(dummy_operation_mode_publisher): add an adapi topic (#1510) * feat(dummy_operation_mode_publisher): add an adapi topic Signed-off-by: Makoto Kurihara * style(pre-commit): autofix --------- Signed-off-by: Makoto Kurihara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../dummy_operation_mode_publisher.launch.xml | 1 + .../src/dummy_operation_mode_publisher.cpp | 4 ++ .../src/dummy_operation_mode_publisher.hpp | 2 + .../src/motion_velocity_smoother_node.cpp | 18 +++++--- system/leader_election_converter/package.xml | 5 +-- .../script/relay_to_sub.py | 43 +++++++++++++------ 6 files changed, 52 insertions(+), 21 deletions(-) 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()