From 443221feb6a8dca69ea6b02e277767c6a7540352 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Fri, 14 Mar 2025 18:13:17 +0900 Subject: [PATCH 1/5] feat!: replace tier4_planning_msgs/VelocityLimit to autoware_internal_planning_msgs/VelocityLimit Signed-off-by: mitsudome-r --- autoware_iv_internal_api_adaptor/src/velocity.cpp | 12 ++++++------ autoware_iv_internal_api_adaptor/src/velocity.hpp | 6 +++--- .../awapi_autoware_state_publisher.hpp | 2 +- .../awapi_awiv_adapter/awapi_autoware_util.hpp | 4 ++-- .../awapi_awiv_adapter/awapi_awiv_adapter_core.hpp | 4 ++-- .../awapi_max_velocity_publisher.hpp | 4 ++-- .../src/awapi_autoware_state_publisher.cpp | 2 +- awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp | 4 ++-- .../src/awapi_max_velocity_publisher.cpp | 4 ++-- 9 files changed, 21 insertions(+), 21 deletions(-) diff --git a/autoware_iv_internal_api_adaptor/src/velocity.cpp b/autoware_iv_internal_api_adaptor/src/velocity.cpp index c6bdc26..4396d7b 100644 --- a/autoware_iv_internal_api_adaptor/src/velocity.cpp +++ b/autoware_iv_internal_api_adaptor/src/velocity.cpp @@ -27,11 +27,11 @@ Velocity::Velocity(const rclcpp::NodeOptions & options) : Node("external_api_vel srv_velocity_ = proxy.create_service( "/api/autoware/set/velocity_limit", std::bind(&Velocity::setVelocityLimit, this, _1, _2)); - pub_api_velocity_ = create_publisher( + pub_api_velocity_ = create_publisher( "/api/autoware/get/velocity_limit", rclcpp::QoS(1).transient_local()); - pub_planning_velocity_ = create_publisher( + pub_planning_velocity_ = create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1).transient_local()); - sub_planning_velocity_ = create_subscription( + sub_planning_velocity_ = create_subscription( "/planning/scenario_planning/current_max_velocity", rclcpp::QoS(1).transient_local(), std::bind(&Velocity::onVelocityLimit, this, _1)); @@ -63,7 +63,7 @@ void Velocity::setVelocityLimit( response->status = tier4_api_utils::response_success(); } -void Velocity::onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::SharedPtr msg) +void Velocity::onVelocityLimit(const autoware_internal_planning_msgs::msg::VelocityLimit::SharedPtr msg) { // store the velocity for releasing the stop if (kVelocityEpsilon < msg->max_velocity) { @@ -75,7 +75,7 @@ void Velocity::onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::Sh void Velocity::publishApiVelocity(double velocity) { - tier4_planning_msgs::msg::VelocityLimit msg; + autoware_internal_planning_msgs::msg::VelocityLimit msg; msg.stamp = now(); msg.max_velocity = velocity; pub_api_velocity_->publish(msg); @@ -83,7 +83,7 @@ void Velocity::publishApiVelocity(double velocity) void Velocity::publishPlanningVelocity(double velocity) { - tier4_planning_msgs::msg::VelocityLimit msg; + autoware_internal_planning_msgs::msg::VelocityLimit msg; msg.stamp = now(); msg.max_velocity = velocity; pub_planning_velocity_->publish(msg); diff --git a/autoware_iv_internal_api_adaptor/src/velocity.hpp b/autoware_iv_internal_api_adaptor/src/velocity.hpp index 0bf45fb..59d2978 100644 --- a/autoware_iv_internal_api_adaptor/src/velocity.hpp +++ b/autoware_iv_internal_api_adaptor/src/velocity.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include namespace internal_api { @@ -32,7 +32,7 @@ class Velocity : public rclcpp::Node private: using PauseDriving = tier4_external_api_msgs::srv::PauseDriving; using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; - using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; + using VelocityLimit = autoware_internal_planning_msgs::msg::VelocityLimit; // ros interface tier4_api_utils::Service::SharedPtr srv_pause_; @@ -55,7 +55,7 @@ class Velocity : public rclcpp::Node void setVelocityLimit( const tier4_external_api_msgs::srv::SetVelocityLimit::Request::SharedPtr request, const tier4_external_api_msgs::srv::SetVelocityLimit::Response::SharedPtr response); - void onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::SharedPtr msg); + void onVelocityLimit(const autoware_internal_planning_msgs::msg::VelocityLimit::SharedPtr msg); // class method void publishApiVelocity(double velocity); diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 8c64d81..7ca9b62 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -58,7 +58,7 @@ class AutowareIvAutowareStatePublisher const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr & mrm_state_ptr, tier4_api_msgs::msg::AwapiAutowareStatus * status); void getCurrentMaxVelInfo( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, tier4_api_msgs::msg::AwapiAutowareStatus * status); void getHazardStatusInfo( const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 817da3d..c6820a3 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include @@ -85,7 +85,7 @@ struct AutowareInfo tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr; - tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr; + autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr; tier4_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; }; diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 592b10e..b51cc3f 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -92,7 +92,7 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_obstacle_avoid_candidate_; rclcpp::Subscription::SharedPtr sub_max_velocity_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_current_max_velocity_; rclcpp::Subscription::SharedPtr sub_temporary_stop_; rclcpp::Subscription::SharedPtr sub_autoware_traj_; @@ -145,7 +145,7 @@ class AutowareIvAdapter : public rclcpp::Node const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); void callbackMaxVelocity(const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); void callbackCurrentMaxVelocity( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); void callbackTemporaryStop(const tier4_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr); void callbackAutowareTrajectory( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp index 04754a5..377af90 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -19,7 +19,7 @@ #include -#include +#include namespace autoware_api { @@ -33,7 +33,7 @@ class AutowareIvMaxVelocityPublisher rclcpp::Clock::SharedPtr clock_; // publisher - rclcpp::Publisher::SharedPtr pub_state_; + rclcpp::Publisher::SharedPtr pub_state_; bool calcMaxVelocity( const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, diff --git a/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index bd0237b..4940f42 100644 --- a/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -114,7 +114,7 @@ void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( } void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, tier4_api_msgs::msg::AwapiAutowareStatus * status) { if (!current_max_velocity_ptr) { diff --git a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 6396c69..9b9d337 100644 --- a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -116,7 +116,7 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); sub_max_velocity_ = this->create_subscription( "input/max_velocity", 1, std::bind(&AutowareIvAdapter::callbackMaxVelocity, this, _1)); - sub_current_max_velocity_ = this->create_subscription( + sub_current_max_velocity_ = this->create_subscription( "input/current_max_velocity", durable_qos, std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1)); sub_temporary_stop_ = this->create_subscription( @@ -323,7 +323,7 @@ void AutowareIvAdapter::callbackMaxVelocity( } void AutowareIvAdapter::callbackCurrentMaxVelocity( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) { aw_info_.current_max_velocity_ptr = msg_ptr; } diff --git a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp index 90f1e71..1867300 100644 --- a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp +++ b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -21,13 +21,13 @@ AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( : clock_(node.get_clock()), default_max_velocity_(default_max_velocity) { // publisher - pub_state_ = node.create_publisher( + pub_state_ = node.create_publisher( "output/max_velocity", rclcpp::QoS{1}.transient_local()); } void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info) { - tier4_planning_msgs::msg::VelocityLimit max_velocity; + autoware_internal_planning_msgs::msg::VelocityLimit max_velocity; if (calcMaxVelocity( aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr, &max_velocity.max_velocity)) // publish info From 6889cd38a8300930ca47f1923d98ca422fdc15db Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 14 Mar 2025 09:27:57 +0000 Subject: [PATCH 2/5] style(pre-commit): autofix --- autoware_iv_internal_api_adaptor/src/velocity.cpp | 3 ++- autoware_iv_internal_api_adaptor/src/velocity.hpp | 2 +- .../awapi_awiv_adapter/awapi_autoware_state_publisher.hpp | 3 ++- .../include/awapi_awiv_adapter/awapi_autoware_util.hpp | 2 +- awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp | 3 ++- awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp | 7 ++++--- 6 files changed, 12 insertions(+), 8 deletions(-) diff --git a/autoware_iv_internal_api_adaptor/src/velocity.cpp b/autoware_iv_internal_api_adaptor/src/velocity.cpp index 4396d7b..5469199 100644 --- a/autoware_iv_internal_api_adaptor/src/velocity.cpp +++ b/autoware_iv_internal_api_adaptor/src/velocity.cpp @@ -63,7 +63,8 @@ void Velocity::setVelocityLimit( response->status = tier4_api_utils::response_success(); } -void Velocity::onVelocityLimit(const autoware_internal_planning_msgs::msg::VelocityLimit::SharedPtr msg) +void Velocity::onVelocityLimit( + const autoware_internal_planning_msgs::msg::VelocityLimit::SharedPtr msg) { // store the velocity for releasing the stop if (kVelocityEpsilon < msg->max_velocity) { diff --git a/autoware_iv_internal_api_adaptor/src/velocity.hpp b/autoware_iv_internal_api_adaptor/src/velocity.hpp index 59d2978..a13873e 100644 --- a/autoware_iv_internal_api_adaptor/src/velocity.hpp +++ b/autoware_iv_internal_api_adaptor/src/velocity.hpp @@ -18,9 +18,9 @@ #include #include +#include #include #include -#include namespace internal_api { diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 7ca9b62..26f3d76 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -58,7 +58,8 @@ class AutowareIvAutowareStatePublisher const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr & mrm_state_ptr, tier4_api_msgs::msg::AwapiAutowareStatus * status); void getCurrentMaxVelInfo( - const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr & + current_max_velocity_ptr, tier4_api_msgs::msg::AwapiAutowareStatus * status); void getHazardStatusInfo( const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index c6820a3..782f238 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -37,7 +38,6 @@ #include #include #include -#include #include #include #include diff --git a/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 4940f42..2a63174 100644 --- a/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -114,7 +114,8 @@ void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( } void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( - const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr & + current_max_velocity_ptr, tier4_api_msgs::msg::AwapiAutowareStatus * status) { if (!current_max_velocity_ptr) { diff --git a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 9b9d337..2d63fec 100644 --- a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -116,9 +116,10 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); sub_max_velocity_ = this->create_subscription( "input/max_velocity", 1, std::bind(&AutowareIvAdapter::callbackMaxVelocity, this, _1)); - sub_current_max_velocity_ = this->create_subscription( - "input/current_max_velocity", durable_qos, - std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1)); + sub_current_max_velocity_ = + this->create_subscription( + "input/current_max_velocity", durable_qos, + std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1)); sub_temporary_stop_ = this->create_subscription( "input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1)); sub_autoware_traj_ = this->create_subscription( From 94b4c516c46c04aa9bbb82dc0f5d63bd1c965735 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Fri, 14 Mar 2025 22:05:28 +0900 Subject: [PATCH 3/5] fix: update package.xml Signed-off-by: Ryohsuke Mitsudome --- autoware_iv_internal_api_adaptor/package.xml | 1 + awapi_awiv_adapter/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/autoware_iv_internal_api_adaptor/package.xml b/autoware_iv_internal_api_adaptor/package.xml index e515f6c..aafdeb3 100644 --- a/autoware_iv_internal_api_adaptor/package.xml +++ b/autoware_iv_internal_api_adaptor/package.xml @@ -11,6 +11,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_internal_planning_msgs autoware_perception_msgs autoware_planning_msgs autoware_system_msgs diff --git a/awapi_awiv_adapter/package.xml b/awapi_awiv_adapter/package.xml index 6a573c4..e0a6ea9 100644 --- a/awapi_awiv_adapter/package.xml +++ b/awapi_awiv_adapter/package.xml @@ -12,6 +12,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_planning_msgs autoware_planning_msgs autoware_system_msgs autoware_vehicle_msgs From 45a1cbbca6938c9710578a7c4679c47be7a098f2 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Tue, 18 Mar 2025 16:47:02 +0900 Subject: [PATCH 4/5] fix: change branch for Autoware Core in build_depends.repos Signed-off-by: Ryohsuke Mitsudome --- build_depends.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build_depends.repos b/build_depends.repos index 8969f3b..d551212 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -6,7 +6,7 @@ repositories: core/autoware.core: type: git url: https://github.com/autowarefoundation/autoware.core.git - version: 0.1.0 + version: main msgs/autoware_msgs: type: git url: https://github.com/autowarefoundation/autoware_msgs.git From 62160941ac098f4ee54c44fd5a47a370b88dd104 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Tue, 18 Mar 2025 17:29:52 +0900 Subject: [PATCH 5/5] fix: add autoware_utils to build_depends.repos Signed-off-by: Ryohsuke Mitsudome --- build_depends.repos | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index d551212..7007a3a 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -1,12 +1,12 @@ repositories: - core/common: - type: git - url: https://github.com/autowarefoundation/autoware_common.git - version: main core/autoware.core: type: git url: https://github.com/autowarefoundation/autoware.core.git version: main + core/autoware_utils: + type: git + url: https://github.com/autowarefoundation/autoware_utils.git + version: main msgs/autoware_msgs: type: git url: https://github.com/autowarefoundation/autoware_msgs.git