From a435326865729ff886b24368d64e3dd315c473b8 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 17 Dec 2021 13:09:00 +0900 Subject: [PATCH] Move api pkgs (#7) * Move awapi package * WIP * Cancel external api adaptor * Fix package name * Fix package name * Move external api msgs --- autoware_external_api_msgs/CMakeLists.txt | 28 ++ autoware_external_api_msgs/package.xml | 30 ++ autoware_external_api_msgs/srv/GetVersion.srv | 3 + autoware_iv_external_api_adaptor/package.xml | 6 +- .../src/version.cpp | 6 +- .../src/version.hpp | 8 +- .../CMakeLists.txt | 35 ++ autoware_iv_internal_api_adaptor/README.md | 1 + autoware_iv_internal_api_adaptor/package.xml | 40 ++ .../src/initial_pose.cpp | 107 +++++ .../src/initial_pose.hpp | 64 +++ .../src/iv_msgs.cpp | 84 ++++ .../src/iv_msgs.hpp | 71 +++ .../src/operator.cpp | 216 +++++++++ .../src/operator.hpp | 93 ++++ .../src/route.cpp | 151 +++++++ .../src/route.hpp | 74 +++ .../src/velocity.cpp | 95 ++++ .../src/velocity.hpp | 67 +++ awapi_awiv_adapter/CMakeLists.txt | 35 ++ awapi_awiv_adapter/Readme.md | 299 ++++++++++++ .../awapi_autoware_state_publisher.hpp | 80 ++++ .../awapi_autoware_util.hpp | 148 ++++++ .../awapi_awiv_adapter_core.hpp | 188 ++++++++ .../awapi_lane_change_state_publisher.hpp | 52 +++ .../awapi_max_velocity_publisher.hpp | 46 ++ ...api_obstacle_avoidance_state_publisher.hpp | 49 ++ .../awapi_awiv_adapter/awapi_pacmod_util.hpp | 40 ++ .../awapi_stop_reason_aggregator.hpp | 64 +++ .../awapi_v2x_aggregator.hpp | 54 +++ .../awapi_vehicle_state_publisher.hpp | 83 ++++ .../awapi_awiv_adapter/diagnostics_filter.hpp | 118 +++++ .../launch/awapi_awiv_adapter.launch.xml | 157 +++++++ .../launch/awapi_relay_container.launch.py | 425 ++++++++++++++++++ awapi_awiv_adapter/package.xml | 43 ++ .../src/awapi_autoware_state_publisher.cpp | 302 +++++++++++++ .../src/awapi_autoware_util.cpp | 108 +++++ .../src/awapi_awiv_adapter_core.cpp | 384 ++++++++++++++++ .../src/awapi_awiv_adapter_node.cpp | 28 ++ .../src/awapi_lane_change_state_publisher.cpp | 88 ++++ .../src/awapi_max_velocity_publisher.cpp | 60 +++ ...api_obstacle_avoidance_state_publisher.cpp | 74 +++ awapi_awiv_adapter/src/awapi_pacmod_util.cpp | 87 ++++ .../src/awapi_stop_reason_aggregator.cpp | 186 ++++++++ .../src/awapi_v2x_aggregator.cpp | 104 +++++ .../src/awapi_vehicle_state_publisher.cpp | 226 ++++++++++ 46 files changed, 4698 insertions(+), 9 deletions(-) create mode 100644 autoware_external_api_msgs/CMakeLists.txt create mode 100644 autoware_external_api_msgs/package.xml create mode 100644 autoware_external_api_msgs/srv/GetVersion.srv create mode 100644 autoware_iv_internal_api_adaptor/CMakeLists.txt create mode 100644 autoware_iv_internal_api_adaptor/README.md create mode 100644 autoware_iv_internal_api_adaptor/package.xml create mode 100644 autoware_iv_internal_api_adaptor/src/initial_pose.cpp create mode 100644 autoware_iv_internal_api_adaptor/src/initial_pose.hpp create mode 100644 autoware_iv_internal_api_adaptor/src/iv_msgs.cpp create mode 100644 autoware_iv_internal_api_adaptor/src/iv_msgs.hpp create mode 100644 autoware_iv_internal_api_adaptor/src/operator.cpp create mode 100644 autoware_iv_internal_api_adaptor/src/operator.hpp create mode 100644 autoware_iv_internal_api_adaptor/src/route.cpp create mode 100644 autoware_iv_internal_api_adaptor/src/route.hpp create mode 100644 autoware_iv_internal_api_adaptor/src/velocity.cpp create mode 100644 autoware_iv_internal_api_adaptor/src/velocity.hpp create mode 100644 awapi_awiv_adapter/CMakeLists.txt create mode 100644 awapi_awiv_adapter/Readme.md create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp create mode 100644 awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp create mode 100644 awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml create mode 100644 awapi_awiv_adapter/launch/awapi_relay_container.launch.py create mode 100644 awapi_awiv_adapter/package.xml create mode 100644 awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp create mode 100644 awapi_awiv_adapter/src/awapi_autoware_util.cpp create mode 100644 awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp create mode 100644 awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp create mode 100644 awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp create mode 100644 awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp create mode 100644 awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp create mode 100644 awapi_awiv_adapter/src/awapi_pacmod_util.cpp create mode 100644 awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp create mode 100644 awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp create mode 100644 awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp diff --git a/autoware_external_api_msgs/CMakeLists.txt b/autoware_external_api_msgs/CMakeLists.txt new file mode 100644 index 0000000..9a4ec4f --- /dev/null +++ b/autoware_external_api_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_external_api_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + srv/GetVersion.srv + DEPENDENCIES + tier4_external_api_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/autoware_external_api_msgs/package.xml b/autoware_external_api_msgs/package.xml new file mode 100644 index 0000000..f3dce98 --- /dev/null +++ b/autoware_external_api_msgs/package.xml @@ -0,0 +1,30 @@ + + + + + autoware_external_api_msgs + 0.0.0 + The autoware_external_api_msgs package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + rosidl_default_generators + builtin_interfaces + + rosidl_default_runtime + builtin_interfaces + + tier4_external_api_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/autoware_external_api_msgs/srv/GetVersion.srv b/autoware_external_api_msgs/srv/GetVersion.srv new file mode 100644 index 0000000..fa9f5fd --- /dev/null +++ b/autoware_external_api_msgs/srv/GetVersion.srv @@ -0,0 +1,3 @@ +--- +tier4_external_api_msgs/ResponseStatus status +string version diff --git a/autoware_iv_external_api_adaptor/package.xml b/autoware_iv_external_api_adaptor/package.xml index 79bb4fd..d3edb44 100644 --- a/autoware_iv_external_api_adaptor/package.xml +++ b/autoware_iv_external_api_adaptor/package.xml @@ -10,12 +10,14 @@ ament_cmake_auto - tier4_api_utils + autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs - tier4_external_api_msgs + autoware_external_api_msgs + tier4_api_utils tier4_auto_msgs_converter + tier4_external_api_msgs tier4_vehicle_msgs nlohmann-json-dev rclcpp diff --git a/autoware_iv_external_api_adaptor/src/version.cpp b/autoware_iv_external_api_adaptor/src/version.cpp index aa0b41e..ccea8ae 100644 --- a/autoware_iv_external_api_adaptor/src/version.cpp +++ b/autoware_iv_external_api_adaptor/src/version.cpp @@ -24,15 +24,15 @@ Version::Version(const rclcpp::NodeOptions & options) tier4_api_utils::ServiceProxyNodeInterface proxy(this); group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - srv_ = proxy.create_service( + srv_ = proxy.create_service( "/api/external/get/version", std::bind(&Version::getVersion, this, _1, _2), rmw_qos_profile_services_default, group_); } void Version::getVersion( - const tier4_external_api_msgs::srv::GetVersion::Request::SharedPtr, - const tier4_external_api_msgs::srv::GetVersion::Response::SharedPtr response) + const autoware_external_api_msgs::srv::GetVersion::Request::SharedPtr, + const autoware_external_api_msgs::srv::GetVersion::Response::SharedPtr response) { response->version = "0.2.0"; response->status = tier4_api_utils::response_success(); diff --git a/autoware_iv_external_api_adaptor/src/version.hpp b/autoware_iv_external_api_adaptor/src/version.hpp index 59c89f9..02e52b1 100644 --- a/autoware_iv_external_api_adaptor/src/version.hpp +++ b/autoware_iv_external_api_adaptor/src/version.hpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_api_utils/tier4_api_utils.hpp" -#include "tier4_external_api_msgs/srv/get_version.hpp" +#include "autoware_external_api_msgs/srv/get_version.hpp" namespace external_api { @@ -30,12 +30,12 @@ class Version : public rclcpp::Node private: // ros interface rclcpp::CallbackGroup::SharedPtr group_; - tier4_api_utils::Service::SharedPtr srv_; + tier4_api_utils::Service::SharedPtr srv_; // ros callback void getVersion( - const tier4_external_api_msgs::srv::GetVersion::Request::SharedPtr request, - const tier4_external_api_msgs::srv::GetVersion::Response::SharedPtr response); + const autoware_external_api_msgs::srv::GetVersion::Request::SharedPtr request, + const autoware_external_api_msgs::srv::GetVersion::Response::SharedPtr response); }; } // namespace external_api diff --git a/autoware_iv_internal_api_adaptor/CMakeLists.txt b/autoware_iv_internal_api_adaptor/CMakeLists.txt new file mode 100644 index 0000000..ebc92a0 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_iv_internal_api_adaptor) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/initial_pose.cpp + src/iv_msgs.cpp + src/operator.cpp + src/route.cpp + src/velocity.cpp +) +rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::InitialPose") +rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::IVMsgs") +rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::Operator") +rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::Route") +rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::Velocity") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/autoware_iv_internal_api_adaptor/README.md b/autoware_iv_internal_api_adaptor/README.md new file mode 100644 index 0000000..59536db --- /dev/null +++ b/autoware_iv_internal_api_adaptor/README.md @@ -0,0 +1 @@ +[See Github Pages](https://tier4.github.io/autoware.proj/tree/main/design/apis/ja/concept/) diff --git a/autoware_iv_internal_api_adaptor/package.xml b/autoware_iv_internal_api_adaptor/package.xml new file mode 100644 index 0000000..e90ef28 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/package.xml @@ -0,0 +1,40 @@ + + + + + autoware_iv_internal_api_adaptor + 0.0.0 + The autoware_iv_internal_api_adaptor package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + tier4_api_utils + tier4_control_msgs + tier4_external_api_msgs + tier4_localization_msgs + tier4_planning_msgs + geometry_msgs + rclcpp + rclcpp_components + std_srvs + + ament_lint_auto + autoware_lint_common + + + autoware_auto_perception_msgs + autoware_auto_system_msgs + tier4_perception_msgs + tier4_system_msgs + tier4_vehicle_msgs + tier4_auto_msgs_converter + + + ament_cmake + + + diff --git a/autoware_iv_internal_api_adaptor/src/initial_pose.cpp b/autoware_iv_internal_api_adaptor/src/initial_pose.cpp new file mode 100644 index 0000000..a75eab2 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/initial_pose.cpp @@ -0,0 +1,107 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "initial_pose.hpp" + +#include + +namespace internal_api +{ +InitialPose::InitialPose(const rclcpp::NodeOptions & options) +: Node("internal_api_initial_pose", options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + tier4_api_utils::ServiceProxyNodeInterface proxy(this); + + init_simulator_pose_ = declare_parameter("init_simulator_pose", false); + init_localization_pose_ = declare_parameter("init_localization_pose", false); + + group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_set_initialize_pose_ = proxy.create_service( + "/api/autoware/set/initialize_pose", std::bind(&InitialPose::setInitializePose, this, _1, _2), + rmw_qos_profile_services_default, group_); + srv_set_initialize_pose_auto_ = proxy.create_service( + "/api/autoware/set/initialize_pose_auto", + std::bind(&InitialPose::setInitializePoseAuto, this, _1, _2), rmw_qos_profile_services_default, + group_); + + if (init_localization_pose_) { + cli_set_initialize_pose_ = proxy.create_client( + "/localization/util/initialize_pose", rmw_qos_profile_services_default); + cli_set_initialize_pose_auto_ = proxy.create_client( + "/localization/util/initialize_pose_auto", rmw_qos_profile_services_default); + } + + if (init_simulator_pose_) { + cli_set_simulator_pose_ = proxy.create_client( + "/api/simulator/set/pose", rmw_qos_profile_services_default); + pub_initialpose2d_ = create_publisher( + "/initialpose2d", rclcpp::QoS(1)); + } +} + +void InitialPose::setInitializePose( + const tier4_external_api_msgs::srv::InitializePose::Request::SharedPtr request, + const tier4_external_api_msgs::srv::InitializePose::Response::SharedPtr response) +{ + response->status = tier4_api_utils::response_ignored("No processing."); + + if (init_simulator_pose_) { + const auto [status, resp] = cli_set_simulator_pose_->call(request); + RCLCPP_INFO(get_logger(), "%d %s", status.code, status.message.c_str()); + if (!tier4_api_utils::is_success(status)) { + response->status = status; + return; + } + pub_initialpose2d_->publish(request->pose); + response->status = resp->status; + } + + if (init_localization_pose_) { + const auto req = std::make_shared(); + req->pose_with_covariance = request->pose; + const auto [status, resp] = cli_set_initialize_pose_->call(req); + if (!tier4_api_utils::is_success(status)) { + response->status = status; + return; + } + if (resp->success) { + response->status = tier4_api_utils::response_success(); + } else { + response->status = tier4_api_utils::response_error("Internal service failed."); + } + } +} + +void InitialPose::setInitializePoseAuto( + const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr request, + const tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr response) +{ + response->status = tier4_api_utils::response_ignored("No processing."); + + if (init_localization_pose_) { + const auto [status, resp] = cli_set_initialize_pose_auto_->call(request); + if (!tier4_api_utils::is_success(status)) { + response->status = status; + return; + } + response->status = resp->status; + } +} + +} // namespace internal_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::InitialPose) diff --git a/autoware_iv_internal_api_adaptor/src/initial_pose.hpp b/autoware_iv_internal_api_adaptor/src/initial_pose.hpp new file mode 100644 index 0000000..ad926fa --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/initial_pose.hpp @@ -0,0 +1,64 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef INITIAL_POSE_HPP_ +#define INITIAL_POSE_HPP_ + +#include +#include + +#include +#include +#include +#include + +namespace internal_api +{ +class InitialPose : public rclcpp::Node +{ +public: + explicit InitialPose(const rclcpp::NodeOptions & options); + +private: + using InitializePose = tier4_external_api_msgs::srv::InitializePose; + using InitializePoseAuto = tier4_external_api_msgs::srv::InitializePoseAuto; + using PoseWithCovarianceStampedSrv = tier4_localization_msgs::srv::PoseWithCovarianceStamped; + + // ros parameter + bool init_simulator_pose_; + bool init_localization_pose_; + + // ros interface + rclcpp::CallbackGroup::SharedPtr group_; + tier4_api_utils::Service::SharedPtr srv_set_initialize_pose_; + tier4_api_utils::Service::SharedPtr srv_set_initialize_pose_auto_; + tier4_api_utils::Client::SharedPtr cli_set_initialize_pose_; + tier4_api_utils::Client::SharedPtr cli_set_initialize_pose_auto_; + tier4_api_utils::Client::SharedPtr cli_set_simulator_pose_; + + // TODO(Takagi, Isamu): workaround for topic check + rclcpp::Publisher::SharedPtr pub_initialpose2d_; + + // ros callback + void setInitializePose( + const tier4_external_api_msgs::srv::InitializePose::Request::SharedPtr request, + const tier4_external_api_msgs::srv::InitializePose::Response::SharedPtr response); + void setInitializePoseAuto( + const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr request, + const tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr response); +}; + +} // namespace internal_api + +#endif // INITIAL_POSE_HPP_ diff --git a/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp b/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp new file mode 100644 index 0000000..41544f0 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp @@ -0,0 +1,84 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "iv_msgs.hpp" + +#include + +namespace internal_api +{ +IVMsgs::IVMsgs(const rclcpp::NodeOptions & options) : Node("external_api_iv_msgs", options) +{ + using std::placeholders::_1; + + pub_state_ = create_publisher("/api/iv_msgs/autoware/state", rclcpp::QoS(1)); + sub_state_ = create_subscription( + "/autoware/state", rclcpp::QoS(1), std::bind(&IVMsgs::onState, this, _1)); + sub_emergency_ = create_subscription( + "/system/emergency/emergency_state", rclcpp::QoS(1), std::bind(&IVMsgs::onEmergency, this, _1)); + + pub_control_mode_ = + create_publisher("/api/iv_msgs/vehicle/status/control_mode", rclcpp::QoS(1)); + sub_control_mode_ = create_subscription( + "/vehicle/status/control_mode", rclcpp::QoS(1), std::bind(&IVMsgs::onControlMode, this, _1)); + + pub_trajectory_ = create_publisher( + "/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1)); + sub_trajectory_ = create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS(1), + std::bind(&IVMsgs::onTrajectory, this, _1)); + + pub_dynamic_objects_ = create_publisher( + "/api/iv_msgs/perception/object_recognition/tracking/objects", rclcpp::QoS(1)); + sub_tracked_objects_ = create_subscription( + "/perception/object_recognition/tracking/objects", rclcpp::QoS(1), + std::bind(&IVMsgs::onTrackedObjects, this, _1)); +} + +void IVMsgs::onState(const AutowareStateAuto::ConstSharedPtr message) +{ + auto state = tier4_auto_msgs_converter::convert(*message); + if (emergency_) { + switch (emergency_->state) { + case EmergencyStateAuto::MRM_OPERATING: + case EmergencyStateAuto::MRM_SUCCEEDED: + case EmergencyStateAuto::MRM_FAILED: + state.state = AutowareStateIV::EMERGENCY; + break; + } + } + pub_state_->publish(state); +} + +void IVMsgs::onEmergency(const EmergencyStateAuto::ConstSharedPtr message) { emergency_ = message; } + +void IVMsgs::onControlMode(const ControlModeAuto::ConstSharedPtr message) +{ + pub_control_mode_->publish(tier4_auto_msgs_converter::convert(*message)); +} + +void IVMsgs::onTrajectory(const TrajectoryAuto::ConstSharedPtr message) +{ + pub_trajectory_->publish(tier4_auto_msgs_converter::convert(*message)); +} + +void IVMsgs::onTrackedObjects(const TrackedObjectsAuto::ConstSharedPtr message) +{ + pub_dynamic_objects_->publish(tier4_auto_msgs_converter::convert(*message)); +} + +} // namespace internal_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::IVMsgs) diff --git a/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp b/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp new file mode 100644 index 0000000..0ec09bd --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp @@ -0,0 +1,71 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IV_STATE_HPP_ +#define IV_STATE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace internal_api +{ +class IVMsgs : public rclcpp::Node +{ +public: + explicit IVMsgs(const rclcpp::NodeOptions & options); + +private: + using EmergencyStateAuto = autoware_auto_system_msgs::msg::EmergencyState; + using AutowareStateAuto = autoware_auto_system_msgs::msg::AutowareState; + using AutowareStateIV = tier4_system_msgs::msg::AutowareState; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_state_; + rclcpp::Publisher::SharedPtr pub_state_; + + using ControlModeAuto = autoware_auto_vehicle_msgs::msg::ControlModeReport; + using ControlModeIV = tier4_vehicle_msgs::msg::ControlMode; + rclcpp::Subscription::SharedPtr sub_control_mode_; + rclcpp::Publisher::SharedPtr pub_control_mode_; + + using TrajectoryAuto = autoware_auto_planning_msgs::msg::Trajectory; + using TrajectoryIV = tier4_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_; + + using TrackedObjectsAuto = autoware_auto_perception_msgs::msg::TrackedObjects; + using DynamicObjectsIV = tier4_perception_msgs::msg::DynamicObjectArray; + rclcpp::Subscription::SharedPtr sub_tracked_objects_; + rclcpp::Publisher::SharedPtr pub_dynamic_objects_; + + void onState(const AutowareStateAuto::ConstSharedPtr message); + void onEmergency(const EmergencyStateAuto::ConstSharedPtr message); + void onControlMode(const ControlModeAuto::ConstSharedPtr message); + void onTrajectory(const TrajectoryAuto::ConstSharedPtr message); + void onTrackedObjects(const TrackedObjectsAuto::ConstSharedPtr message); + + EmergencyStateAuto::ConstSharedPtr emergency_; +}; + +} // namespace internal_api + +#endif // IV_STATE_HPP_ diff --git a/autoware_iv_internal_api_adaptor/src/operator.cpp b/autoware_iv_internal_api_adaptor/src/operator.cpp new file mode 100644 index 0000000..4dc6bd6 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/operator.cpp @@ -0,0 +1,216 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "operator.hpp" + +#include + +namespace internal_api +{ +Operator::Operator(const rclcpp::NodeOptions & options) : Node("external_api_operator", options) +{ + using namespace std::literals::chrono_literals; + using std::placeholders::_1; + using std::placeholders::_2; + tier4_api_utils::ServiceProxyNodeInterface proxy(this); + + group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_set_operator_ = proxy.create_service( + "/api/autoware/set/operator", std::bind(&Operator::setOperator, this, _1, _2), + rmw_qos_profile_services_default, group_); + srv_set_observer_ = proxy.create_service( + "/api/autoware/set/observer", std::bind(&Operator::setObserver, this, _1, _2), + rmw_qos_profile_services_default, group_); + + cli_external_select_ = proxy.create_client( + "/control/external_cmd_selector/select_external_command"); + pub_gate_mode_ = create_publisher( + "/control/gate_mode_cmd", rclcpp::QoS(1)); + pub_vehicle_engage_ = + create_publisher("/vehicle/engage", rclcpp::QoS(1)); + + pub_operator_ = create_publisher( + "/api/autoware/get/operator", rclcpp::QoS(1)); + pub_observer_ = create_publisher( + "/api/autoware/get/observer", rclcpp::QoS(1)); + + sub_external_select_ = + create_subscription( + "/control/external_cmd_selector/current_selector_mode", rclcpp::QoS(1), + std::bind(&Operator::onExternalSelect, this, _1)); + sub_gate_mode_ = create_subscription( + "/control/current_gate_mode", rclcpp::QoS(1), std::bind(&Operator::onGateMode, this, _1)); + sub_vehicle_control_mode_ = + create_subscription( + "/vehicle/status/control_mode", rclcpp::QoS(1), + std::bind(&Operator::onVehicleControlMode, this, _1)); + + timer_ = rclcpp::create_timer(this, get_clock(), 200ms, std::bind(&Operator::onTimer, this)); +} + +void Operator::setOperator( + const tier4_external_api_msgs::srv::SetOperator::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetOperator::Response::SharedPtr response) +{ + switch (request->mode.mode) { + case tier4_external_api_msgs::msg::Operator::DRIVER: + setVehicleEngage(false); + response->status = tier4_api_utils::response_success(); + return; + + case tier4_external_api_msgs::msg::Operator::AUTONOMOUS: + setGateMode(tier4_control_msgs::msg::GateMode::AUTO); + setVehicleEngage(true); + response->status = tier4_api_utils::response_success(); + return; + + case tier4_external_api_msgs::msg::Operator::OBSERVER: + // TODO(Takagi, Isamu): prohibit transition when none observer type is added + setGateMode(tier4_control_msgs::msg::GateMode::EXTERNAL); + setVehicleEngage(true); + response->status = tier4_api_utils::response_success(); + return; + + default: + response->status = tier4_api_utils::response_error("Invalid parameter."); + return; + } +} + +void Operator::setObserver( + const tier4_external_api_msgs::srv::SetObserver::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetObserver::Response::SharedPtr response) +{ + using ExternalCommandSelectorMode = tier4_control_msgs::msg::ExternalCommandSelectorMode; + + switch (request->mode.mode) { + case tier4_external_api_msgs::msg::Observer::LOCAL: + response->status = setExternalSelect(ExternalCommandSelectorMode::LOCAL); + return; + + case tier4_external_api_msgs::msg::Observer::REMOTE: + response->status = setExternalSelect(ExternalCommandSelectorMode::REMOTE); + return; + + default: + response->status = tier4_api_utils::response_error("Invalid parameter."); + return; + } +} + +void Operator::onExternalSelect( + const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr message) +{ + external_select_ = message; +} + +void Operator::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr message) +{ + gate_mode_ = message; +} + +void Operator::onVehicleControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr message) +{ + vehicle_control_mode_ = message; +} + +void Operator::onTimer() +{ + publishOperator(); + publishObserver(); +} + +void Operator::publishOperator() +{ + using OperatorMsg = tier4_external_api_msgs::msg::Operator; + using tier4_external_api_msgs::build; + + if (!vehicle_control_mode_ || !gate_mode_) { + return; + } + + if (vehicle_control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { + pub_operator_->publish(build().mode(OperatorMsg::DRIVER)); + return; + } + switch (gate_mode_->data) { + case tier4_control_msgs::msg::GateMode::AUTO: + pub_operator_->publish(build().mode(OperatorMsg::AUTONOMOUS)); + return; + + case tier4_control_msgs::msg::GateMode::EXTERNAL: + pub_operator_->publish(build().mode(OperatorMsg::OBSERVER)); + return; + } + RCLCPP_ERROR(get_logger(), "Unknown operator."); +} + +void Operator::publishObserver() +{ + using ObserverMsg = tier4_external_api_msgs::msg::Observer; + using tier4_external_api_msgs::build; + + if (!external_select_) { + return; + } + + switch (external_select_->data) { + case tier4_control_msgs::msg::ExternalCommandSelectorMode::LOCAL: + pub_observer_->publish(build().mode(ObserverMsg::LOCAL)); + return; + + case tier4_control_msgs::msg::ExternalCommandSelectorMode::REMOTE: + pub_observer_->publish(build().mode(ObserverMsg::REMOTE)); + return; + } + RCLCPP_ERROR(get_logger(), "Unknown observer."); +} + +void Operator::setVehicleEngage(bool engage) +{ + const auto msg = autoware_auto_vehicle_msgs::build() + .stamp(now()) + .engage(engage); + pub_vehicle_engage_->publish(msg); +} + +void Operator::setGateMode(tier4_control_msgs::msg::GateMode::_data_type data) +{ + const auto msg = tier4_control_msgs::build().data(data); + pub_gate_mode_->publish(msg); +} + +tier4_external_api_msgs::msg::ResponseStatus Operator::setExternalSelect( + tier4_control_msgs::msg::ExternalCommandSelectorMode::_data_type data) +{ + const auto req = std::make_shared(); + req->mode.data = data; + + const auto [status, resp] = cli_external_select_->call(req); + if (!tier4_api_utils::is_success(status)) { + return status; + } + + if (resp->success) { + return tier4_api_utils::response_success(resp->message); + } else { + return tier4_api_utils::response_error(resp->message); + } +} + +} // namespace internal_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::Operator) diff --git a/autoware_iv_internal_api_adaptor/src/operator.hpp b/autoware_iv_internal_api_adaptor/src/operator.hpp new file mode 100644 index 0000000..2136305 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/operator.hpp @@ -0,0 +1,93 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPERATOR_HPP_ +#define OPERATOR_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace internal_api +{ +class Operator : public rclcpp::Node +{ +public: + explicit Operator(const rclcpp::NodeOptions & options); + +private: + using SetOperator = tier4_external_api_msgs::srv::SetOperator; + using SetObserver = tier4_external_api_msgs::srv::SetObserver; + using GetOperator = tier4_external_api_msgs::msg::Operator; + using GetObserver = tier4_external_api_msgs::msg::Observer; + using ExternalCommandSelect = tier4_control_msgs::srv::ExternalCommandSelect; + using ExternalCommandSelectorMode = tier4_control_msgs::msg::ExternalCommandSelectorMode; + using GateMode = tier4_control_msgs::msg::GateMode; + using VehicleEngage = autoware_auto_vehicle_msgs::msg::Engage; + using VehicleControlMode = autoware_auto_vehicle_msgs::msg::ControlModeReport; + + // ros interface + rclcpp::CallbackGroup::SharedPtr group_; + tier4_api_utils::Service::SharedPtr srv_set_operator_; + tier4_api_utils::Service::SharedPtr srv_set_observer_; + tier4_api_utils::Client::SharedPtr cli_external_select_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_operator_; + rclcpp::Publisher::SharedPtr pub_observer_; + rclcpp::Subscription::SharedPtr sub_external_select_; + rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr sub_vehicle_control_mode_; + rclcpp::TimerBase::SharedPtr timer_; + + // ros callback + void setOperator( + const tier4_external_api_msgs::srv::SetOperator::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetOperator::Response::SharedPtr response); + void setObserver( + const tier4_external_api_msgs::srv::SetObserver::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetObserver::Response::SharedPtr response); + void onExternalSelect( + const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr message); + void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr message); + void onVehicleControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr message); + void onTimer(); + + // class field + tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr external_select_; + tier4_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_; + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr vehicle_control_mode_; + + // class method + void publishOperator(); + void publishObserver(); + void setVehicleEngage(bool engage); + void setGateMode(tier4_control_msgs::msg::GateMode::_data_type data); + tier4_external_api_msgs::msg::ResponseStatus setExternalSelect( + tier4_control_msgs::msg::ExternalCommandSelectorMode::_data_type data); +}; + +} // namespace internal_api + +#endif // OPERATOR_HPP_ diff --git a/autoware_iv_internal_api_adaptor/src/route.cpp b/autoware_iv_internal_api_adaptor/src/route.cpp new file mode 100644 index 0000000..964934a --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/route.cpp @@ -0,0 +1,151 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "route.hpp" + +#include +#include +#include +#include +#include + +#include + +namespace +{ +auto convertRoute(const tier4_external_api_msgs::msg::Route & route) +{ + // there is no input for start_pose + autoware_auto_planning_msgs::msg::HADMapRoute msg; + msg.header = route.goal_pose.header; + msg.goal_pose = route.goal_pose.pose; + for (const auto & section : route.route_sections) { + autoware_auto_mapping_msgs::msg::HADMapSegment segment; + segment.preferred_primitive_id = section.preferred_lane_id; + for (const auto & lane_id : section.lane_ids) { + autoware_auto_mapping_msgs::msg::MapPrimitive primitive; + primitive.id = lane_id; + segment.primitives.push_back(primitive); + } + msg.segments.push_back(segment); + } + return msg; +} + +auto convertRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) +{ + // there is no input for continued_lane_ids + tier4_external_api_msgs::msg::Route msg; + msg.goal_pose.header = route.header; + msg.goal_pose.pose = route.goal_pose; + for (const auto & segment : route.segments) { + tier4_external_api_msgs::msg::RouteSection section; + section.preferred_lane_id = segment.preferred_primitive_id; + for (const auto & primitive : segment.primitives) { + section.lane_ids.push_back(primitive.id); + } + msg.route_sections.push_back(section); + } + return msg; +} + +} // namespace + +namespace internal_api +{ +Route::Route(const rclcpp::NodeOptions & options) : Node("external_api_route", options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + tier4_api_utils::ServiceProxyNodeInterface proxy(this); + + group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_clear_route_ = proxy.create_service( + "/api/autoware/set/clear_route", std::bind(&Route::clearRoute, this, _1, _2), + rmw_qos_profile_services_default, group_); + srv_set_route_ = proxy.create_service( + "/api/autoware/set/route", std::bind(&Route::setRoute, this, _1, _2), + rmw_qos_profile_services_default, group_); + srv_set_goal_ = proxy.create_service( + "/api/autoware/set/goal", std::bind(&Route::setGoal, this, _1, _2), + rmw_qos_profile_services_default, group_); + srv_set_checkpoint_ = proxy.create_service( + "/api/autoware/set/checkpoint", std::bind(&Route::setCheckpoint, this, _1, _2), + rmw_qos_profile_services_default, group_); + + pub_get_route_ = create_publisher( + "/api/autoware/get/route", rclcpp::QoS(1).transient_local()); + + cli_clear_route_ = proxy.create_client("/autoware/reset_route"); + sub_planning_route_ = create_subscription( + "/planning/mission_planning/route", rclcpp::QoS(1).transient_local(), + std::bind(&Route::onRoute, this, _1)); + pub_planning_route_ = create_publisher( + "/planning/mission_planning/route", rclcpp::QoS(1).transient_local()); + pub_planning_goal_ = create_publisher( + "/planning/mission_planning/goal", rclcpp::QoS(1)); + pub_planning_checkpoint_ = create_publisher( + "/planning/mission_planning/checkpoint", rclcpp::QoS(1)); +} + +void Route::clearRoute( + const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr, + const tier4_external_api_msgs::srv::ClearRoute::Response::SharedPtr response) +{ + auto req = std::make_shared(); + auto [status, resp] = cli_clear_route_->call(req); + if (!tier4_api_utils::is_success(status)) { + response->status = status; + return; + } + if (resp->success) { + response->status = tier4_api_utils::response_success(resp->message); + } else { + response->status = tier4_api_utils::response_error(resp->message); + } +} + +void Route::setRoute( + const tier4_external_api_msgs::srv::SetRoute::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetRoute::Response::SharedPtr response) +{ + pub_planning_route_->publish(convertRoute(request->route)); + response->status = tier4_api_utils::response_success(); +} + +void Route::setGoal( + const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response) +{ + pub_planning_goal_->publish(request->pose); + response->status = tier4_api_utils::response_success(); +} + +void Route::setCheckpoint( + const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response) +{ + pub_planning_checkpoint_->publish(request->pose); + response->status = tier4_api_utils::response_success(); +} + +void Route::onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr message) +{ + pub_get_route_->publish(convertRoute(*message)); +} + +} // namespace internal_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::Route) diff --git a/autoware_iv_internal_api_adaptor/src/route.hpp b/autoware_iv_internal_api_adaptor/src/route.hpp new file mode 100644 index 0000000..5ca79d8 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/route.hpp @@ -0,0 +1,74 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROUTE_HPP_ +#define ROUTE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace internal_api +{ +class Route : public rclcpp::Node +{ +public: + explicit Route(const rclcpp::NodeOptions & options); + +private: + using ClearRoute = tier4_external_api_msgs::srv::ClearRoute; + using SetRoute = tier4_external_api_msgs::srv::SetRoute; + using SetPose = tier4_external_api_msgs::srv::SetPose; + using HADMapRoute = autoware_auto_planning_msgs::msg::HADMapRoute; + + // ros interface + rclcpp::CallbackGroup::SharedPtr group_; + tier4_api_utils::Service::SharedPtr srv_clear_route_; + tier4_api_utils::Service::SharedPtr srv_set_route_; + tier4_api_utils::Service::SharedPtr srv_set_goal_; + tier4_api_utils::Service::SharedPtr srv_set_checkpoint_; + tier4_api_utils::Client::SharedPtr cli_clear_route_; + rclcpp::Subscription::SharedPtr sub_planning_route_; + rclcpp::Publisher::SharedPtr pub_planning_route_; + rclcpp::Publisher::SharedPtr pub_planning_goal_; + rclcpp::Publisher::SharedPtr pub_planning_checkpoint_; + rclcpp::Publisher::SharedPtr pub_get_route_; + + // ros callback + void clearRoute( + const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr request, + const tier4_external_api_msgs::srv::ClearRoute::Response::SharedPtr response); + void setRoute( + const tier4_external_api_msgs::srv::SetRoute::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetRoute::Response::SharedPtr response); + void setGoal( + const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response); + void setCheckpoint( + const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response); + + void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr message); +}; + +} // namespace internal_api + +#endif // ROUTE_HPP_ diff --git a/autoware_iv_internal_api_adaptor/src/velocity.cpp b/autoware_iv_internal_api_adaptor/src/velocity.cpp new file mode 100644 index 0000000..22edfd2 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/velocity.cpp @@ -0,0 +1,95 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "velocity.hpp" + +namespace internal_api +{ +Velocity::Velocity(const rclcpp::NodeOptions & options) : Node("external_api_velocity", options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + tier4_api_utils::ServiceProxyNodeInterface proxy(this); + + srv_pause_ = proxy.create_service( + "/api/autoware/set/pause_driving", std::bind(&Velocity::setPauseDriving, this, _1, _2)); + srv_velocity_ = proxy.create_service( + "/api/autoware/set/velocity_limit", std::bind(&Velocity::setVelocityLimit, this, _1, _2)); + + pub_api_velocity_ = create_publisher( + "/api/autoware/get/velocity_limit", rclcpp::QoS(1).transient_local()); + pub_planning_velocity_ = create_publisher( + "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1).transient_local()); + sub_planning_velocity_ = create_subscription( + "/planning/scenario_planning/current_max_velocity", rclcpp::QoS(1).transient_local(), + std::bind(&Velocity::onVelocityLimit, this, _1)); + + is_ready_ = false; + velocity_limit_ = 0.0; +} + +void Velocity::setPauseDriving( + const tier4_external_api_msgs::srv::PauseDriving::Request::SharedPtr request, + const tier4_external_api_msgs::srv::PauseDriving::Response::SharedPtr response) +{ + if (!is_ready_) { + response->status = tier4_api_utils::response_error("It is not ready to set velocity."); + return; + } + publishPlanningVelocity(request->pause ? 0.0 : velocity_limit_); + response->status = tier4_api_utils::response_success(); +} + +void Velocity::setVelocityLimit( + const tier4_external_api_msgs::srv::SetVelocityLimit::Request::SharedPtr request, + const tier4_external_api_msgs::srv::SetVelocityLimit::Response::SharedPtr response) +{ + if (!is_ready_) { + response->status = tier4_api_utils::response_error("It is not ready to set velocity."); + return; + } + publishPlanningVelocity(request->velocity); + response->status = tier4_api_utils::response_success(); +} + +void Velocity::onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::SharedPtr msg) +{ + // store the velocity for releasing the stop + if (kVelocityEpsilon < msg->max_velocity) { + velocity_limit_ = msg->max_velocity; + } + is_ready_ = true; + publishApiVelocity(msg->max_velocity); +} + +void Velocity::publishApiVelocity(double velocity) +{ + tier4_planning_msgs::msg::VelocityLimit msg; + msg.stamp = now(); + msg.max_velocity = velocity; + pub_api_velocity_->publish(msg); +} + +void Velocity::publishPlanningVelocity(double velocity) +{ + tier4_planning_msgs::msg::VelocityLimit msg; + msg.stamp = now(); + msg.max_velocity = velocity; + pub_planning_velocity_->publish(msg); +} + +} // namespace internal_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::Velocity) diff --git a/autoware_iv_internal_api_adaptor/src/velocity.hpp b/autoware_iv_internal_api_adaptor/src/velocity.hpp new file mode 100644 index 0000000..fc04a27 --- /dev/null +++ b/autoware_iv_internal_api_adaptor/src/velocity.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef VELOCITY_HPP_ +#define VELOCITY_HPP_ + +#include +#include + +#include +#include +#include + +namespace internal_api +{ +class Velocity : public rclcpp::Node +{ +public: + explicit Velocity(const rclcpp::NodeOptions & options); + +private: + using PauseDriving = tier4_external_api_msgs::srv::PauseDriving; + using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; + using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; + + // ros interface + tier4_api_utils::Service::SharedPtr srv_pause_; + tier4_api_utils::Service::SharedPtr srv_velocity_; + rclcpp::Publisher::SharedPtr pub_api_velocity_; + rclcpp::Publisher::SharedPtr pub_planning_velocity_; + rclcpp::Subscription::SharedPtr sub_planning_velocity_; + + // class constants + static constexpr double kVelocityEpsilon = 1e-5; + + // class state + bool is_ready_; + double velocity_limit_; + + // ros callback + void setPauseDriving( + const tier4_external_api_msgs::srv::PauseDriving::Request::SharedPtr request, + const tier4_external_api_msgs::srv::PauseDriving::Response::SharedPtr response); + 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); + + // class method + void publishApiVelocity(double velocity); + void publishPlanningVelocity(double velocity); +}; + +} // namespace internal_api + +#endif // VELOCITY_HPP_ diff --git a/awapi_awiv_adapter/CMakeLists.txt b/awapi_awiv_adapter/CMakeLists.txt new file mode 100644 index 0000000..2f6171b --- /dev/null +++ b/awapi_awiv_adapter/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(awapi_awiv_adapter) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(awapi_awiv_adapter + src/awapi_awiv_adapter_node.cpp + src/awapi_awiv_adapter_core.cpp + src/awapi_vehicle_state_publisher.cpp + src/awapi_autoware_state_publisher.cpp + src/awapi_stop_reason_aggregator.cpp + src/awapi_v2x_aggregator.cpp + src/awapi_lane_change_state_publisher.cpp + src/awapi_obstacle_avoidance_state_publisher.cpp + src/awapi_max_velocity_publisher.cpp + src/awapi_autoware_util.cpp + src/awapi_pacmod_util.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/awapi_awiv_adapter/Readme.md b/awapi_awiv_adapter/Readme.md new file mode 100644 index 0000000..b762812 --- /dev/null +++ b/awapi_awiv_adapter/Readme.md @@ -0,0 +1,299 @@ +# awapi_awiv_adapter + +✓: confirmed, (blank): not confirmed + +## get topic + +### /awapi/vehicle/get/status + +- get vehicle status +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus + +| ✓ | type | name | unit | note | +| --- | :----------------------- | :----------------------- | :----------------------------------------- | :--------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | geometry_msgs/Pose | pose | position:[m] | | +| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | +| | geographic_msgs/GeoPoint | geo_point | | lat/lon/alt | +| ✓ | float64 | velocity | [m/s] | | +| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | +| ✓ | float64 | steering | [rad] | | +| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | +| ✓ | float64 | angular_velocity | [rad/s] | | +| | int32 | gear | according to tier4_vehicle_msgs/Shift | | +| | float32 | energy_level | | available only for golf-cart | +| ✓ | int32 | turn_signal | according to tier4_vehicle_msgs/TurnSignal | | +| ✓ | float64 | target_velocity | [m/s] | | +| ✓ | float64 | target_acceleration | [m/ss] | | +| ✓ | float64 | target_steering | [rad] | | +| ✓ | float64 | target_steering_velocity | [rad/s] | | + +### /awapi/autoware/get/status + +- get autoware status +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus + +| ✓ | type | name | unit | note | +| --- | :---------------------------------- | :------------------- | :------------------------------------------ | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | string | autoware_state | | | +| ✓ | int32 | control_mode | according to tier4_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | tier4_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| ✓ | bool | emergency_stopped | True in emergency mode | | +| ✓ | float32 | current_max_velocity | [m/s] | | +| | tier4_system_msgs/HazardStatus | hazard_status | | system hazard status | +| ✓ | tier4_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | error_diagnostics | | diagnostics that are the cause of system emergency | +| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WaitingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | + +- specification of stop_reason + - stop_reason is output only when the following conditions are met. + - stop_point in stop_reason is close to /planning/scenario_planning/trajectory (within 10m). + - The distance between current position and stop_point is less than stop_reason_thresh_dist. + +### /awapi/autoware/get/route + +- get route +- MessageType: tier4_planning_msgs/Route + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :--- | :--- | :--- | +| ✓ | tier4_planning_msgs/Route | | | | + +### /awapi/autoware/get/stop_speed_exceeded + +- get flag of exceeding stop speed or not + - True: exceed the stop speed ( = "cannot stop before the stop line") + - False: not exceed the stop speed ( = "no stop line in the trajectory" or "possible to stop before the stop line" ) +- MessageType: tier4_planning_msgs/StopSpeedExceedStatus + +| ✓ | type | name | unit | note | +| --- | :---------------------------------------- | :--- | :--- | :--- | +| | tier4_planning_msgs/StopSpeedExceedStatus | | - | | + +### /awapi/prediction/get/objects + +- get predicted object +- MessageType: tier4_api_msgs/DynamicObjectArray + +| ✓ | type | name | unit | note | +| --- | :-------------------------------- | :--- | :--- | :--- | +| ✓ | tier4_api_msgs/DynamicObjectArray | | | | + +### /awapi/lane_change/get/status + +- get lane change information +- MessageType: awapi_awiv_adapter/LaneChangeStatus + +| ✓ | type | name | unit | note | +| --- | :----------------------- | :-------------------------- | :------------------------------------ | :--------------------------------------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | force_lane_change_available | True when lane change is available | available: Physically lane changeable state (do not consider other vehicle) | +| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | +| | tier4_planning_msgs/Path | candidate_path | according to tier4_planning_msgs/Path | | + +### /awapi/object_avoidance/get/status + +- get obstacle avoidance information +- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus + +| ✓ | type | name | unit | note | +| --- | :----------------------------- | :----------------------- | :------------------------------------------ | :---------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | +| | tier4_planning_msgs/Trajectory | candidate_path | according to tier4_planning_msgs/Trajectory | Msg type is different from lane change candidate path | + +### /awapi/traffic_light/get/traffic_signals + +- get recognition result of traffic light +- MessageType: autoware_auto_perception_msgs/msg/TrafficSignalArray + +| ✓ | type | name | unit | note | +| --- | :--------------------------------------------------- | :--- | :--- | :--- | +| | autoware_auto_perception_msgs/msg/TrafficSignalArray | | | | + +### /awapi/traffic_light/get/nearest_traffic_signal + +- get recognition result of nearest traffic light +- MessageType: autoware_auto_perception_msgs/LookingTrafficSignal + +| | type | name | unit | note | +| --- | :--------------------------------------------------- | :--------- | :--- | :------------------------------------------------------------ | +| | std_msgs/Header | header | | | +| | autoware_auto_perception_msgs/TrafficSignalWithJudge | perception | | traffic light information from autoware perception module | +| | autoware_auto_perception_msgs/TrafficSignalWithJudge | external | | traffic light information from external tool/module | +| | autoware_auto_perception_msgs/TrafficSignalWithJudge | final | | traffic light information used by the planning module finally | + +- The contents of TrafficSignalWithJudge.msg is following. + +| | type | name | unit | note | +| --- | :------------------------------------------ | :----- | :------------------- | :------------------------------------------------------------- | +| | autoware_auto_perception_msgs/TrafficSignal | signal | | traffic light color/arrow | +| | uint8 | judge | 0:NONE, 1:STOP, 2:GO | go/stop judgment based on the color/arrow of the traffic light | + +### /awapi/vehicle/get/door + +- get door status +- MessageType: tier4_api_msgs/DoorStatus.msg + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :----- | :--------------------------------------------------------------------------------------- | :------------------------------------------ | +| | tier4_api_msgs/DoorStatus | status | 0:UNKNOWN, 1:DOOR_OPENED, 2:DOOR_CLOSED 3:DOOR_OPENING, 4:DOOR_CLOSING, 5:NOT_APPLICABLE | available only for the vehicle using pacmod | + +- Now, available status is following: (0:UNKNOWN, 1:DOOR_OPENED, 2:DOOR_CLOSED, 5:NOT_APPLICABLE ). +- 5 (NOT_APPLICABLE) is published if the pacmod is not used +- Due to the specifications of pacmod, the last door open / close command is published as the status. +- The status is 0 (UNKNOWN) until the door open / close command is published once. + +## put topic + +### /awapi/vehicle/put/velocity + +- set upper velocity +- MessageType: tier4_api_msgs/VelocityLimit + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :----------- | +| ✓ | tier4_api_msgs/VelocityLimit | | | max velocity | + +### /awapi/vehicle/put/stop + +- set temporary stop signal +- MessageType: tier4_api_msgs/StopCommand +- Specification + + - send True: send upper velocity to 0 + - send False: resend last received upper velocity + - (if upper velocity have never received, send _default_max_velocity_ value.) + - _default_max_velocity_ refers to the param: _/planning/scenario_planning/motion_velocity_optimizer/max_velocity_ + + | ✓ | type | name | unit | note | + | --- | :------------------------- | :--- | :--- | :--- | + | ✓ | tier4_api_msgs/StopCommand | | | | + +### /awapi/autoware/put/gate_mode + +- send gate mode (auto/remote) +- MessageType: tier4_control_msgs/GateMode + +| ✓ | type | name | unit | note | +| --- | :-------------------------- | :--- | :--- | :--- | +| | tier4_control_msgs/GateMode | | | | + +### /awapi/autoware/put/engage + +- send engage signal (both of autoware/engage and vehicle/engage) +- MessageType: tier4_vehicle_msgs/Engage + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :--- | :--- | :--- | +| ✓ | tier4_vehicle_msgs/Engage | | | | + +### /awapi/autoware/put/goal + +- send goal pose +- MessageType: geometry_msgs/PoseStamped + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :--- | :--- | :--- | +| | geometry_msgs/PoseStamped | | | | + +### /awapi/autoware/put/route + +- send route +- MessageType: tier4_planning_msgs/Route + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :--- | :--- | :--- | +| ✓ | tier4_planning_msgs/Route | | | | + +### /awapi/lane_change/put/approval + +- send lane change approval flag +- send True: start lane change when **lane_change_ready** is true +- MessageType: tier4_planning_msgs/LaneChangeCommand + +| ✓ | type | name | unit | note | +| --- | :---------------------------------------- | :--- | :--- | :--- | +| | tier4_planning_msgs/msg/LaneChangeCommand | | | | + +### /awapi/lane_change/put/force + +- send force lane change flag +- send True: start lane change when **force_lane_change_available** is true +- MessageType: tier4_planning_msgs/LaneChangeCommand + +| ✓ | type | name | unit | note | +| --- | :------------------------------------ | :--- | :--- | :--- | +| | tier4_planning_msgs/LaneChangeCommand | | | | + +### /awapi/object_avoidance/put/approval + +- send object avoidance approval flag +- MessageType: tier4_planning_msgs/EnableAvoidance + +| ✓ | type | name | unit | note | +| --- | :---------------------------------- | :--- | :--- | :--- | +| | tier4_planning_msgs/EnableAvoidance | | | | + +### /awapi/object_avoidance/put/force + +- send force object avoidance flag +- **not implemented (Autoware does not have corresponded topic)** + +| ✓ | type | name | unit | note | +| --- | :--- | :--- | :--- | :--- | + +### /awapi/traffic_light/put/traffic_signals + +- Overwrite the recognition result of traffic light +- MessageType: autoware_auto_perception_msgs/TrafficSignalArray + +| ✓ | type | name | unit | note | +| --- | :----------------------------------------------- | :--- | :--- | :--- | +| | autoware_auto_perception_msgs/TrafficSignalArray | | | | + +### /awapi/vehicle/put/door + +- send door command +- MessageType: tier4_api_msgs/DoorCommand + - send True: open door + - send False: close door + +| ✓ | type | name | unit | note | +| --- | :------------------------- | :--- | :--- | :------------------------------------------ | +| | tier4_api_msgs/DoorCommand | | | available only for the vehicle using pacmod | + +### /awapi/autoware/put/crosswalk_states + +- send crosswalk status + - forcibly rewrite the internal state of crosswalk module +- MessageType: tier4_api_msgs/CrossWalkStatus + +| ✓ | type | name | unit | note | +| --- | :-------------- | :----- | :----------------------- | :--- | +| | std_msgs/Header | header | | | +| | int32 | status | 0:STOP, 1:GO, 2:SLOWDOWN | | + +### /awapi/autoware/put/intersection_states + +- send intersection status + - forcibly rewrite the internal state of intersection module +- MessageType: tier4_api_msgs/CrosswalkStatus + +| ✓ | type | name | unit | note | +| --- | :-------------- | :----- | :----------- | :--- | +| | std_msgs/Header | header | | | +| | int32 | status | 0:STOP, 1:GO | | + +### /awapi/autoware/put/expand_stop_range + +- send expand range of the polygon used by obstacle stop [m] +- MessageType: tier4_planning_msgs/ExpandStopRange + +| ✓ | type | name | unit | note | +| --- | :---------------------------------- | :--- | :--- | :--- | +| | tier4_planning_msgs/ExpandStopRange | | | | + +--- 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 new file mode 100644 index 0000000..7352e43 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -0,0 +1,80 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + +#include +#include +#include + +namespace autoware_api +{ +class AutowareIvAutowareStatePublisher +{ +public: + explicit AutowareIvAutowareStatePublisher(rclcpp::Node & node); + void statePublisher(const AutowareInfo & aw_info); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + // publisher + rclcpp::Publisher::SharedPtr pub_state_; + + // parameter + + /* parameter for judging goal now */ + bool arrived_goal_; + autoware_auto_system_msgs::msg::AutowareState::_state_type prev_state_; + + void getAutowareStateInfo( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getControlModeInfo( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getGateModeInfo( + const tier4_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getEmergencyStateInfo( + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getCurrentMaxVelInfo( + const tier4_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); + void getStopReasonInfo( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getDiagInfo(const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getErrorDiagInfo( + const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); + void getGlobalRptInfo( + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status); + + bool isGoal(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state); +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ 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 new file mode 100644 index 0000000..bacca29 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -0,0 +1,148 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware_api +{ +struct AutowareInfo +{ + std::shared_ptr current_pose_ptr; + autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr steer_ptr; + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr vehicle_cmd_ptr; + autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr turn_indicators_ptr; + autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr hazard_lights_ptr; + nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr; + autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr gear_ptr; + tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr battery_ptr; + sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr; + autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr; + tier4_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; + autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr emergency_state_ptr; + autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; + tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; + tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr; + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; + pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; + tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; + tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr; + autoware_auto_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; + tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr; + autoware_auto_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; + tier4_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; + pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; +}; + +template +T waitForParam( + rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) +{ + std::chrono::seconds sec(1); + + auto param_client = std::make_shared(node, remote_node_name); + + while (!param_client->wait_for_service(sec)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service."); + return {}; + } + RCLCPP_INFO_THROTTLE( + node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n", + remote_node_name.c_str(), param_name.c_str()); + } + + if (param_client->has_parameter(param_name)) { + return param_client->get_parameter(param_name); + } + + return {}; +} + +double lowpass_filter(const double current_value, const double prev_value, const double gain); + +namespace planning_util +{ +bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + size_t & output_closest_idx, const double dist_thr = 10.0, const double angle_thr = M_PI / 2.0); + +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const int idx) +{ + return traj.points.at(idx).pose; +} + +inline double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) +{ + return std::hypot((a.x - b.x), (a.y - b.y)); +} + +double normalizeEulerAngle(double euler); + +double calcArcLengthFromWayPoint( + const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const size_t dst_idx); + +double calcDistanceAlongTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose); + +} // namespace planning_util + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ 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 new file mode 100644 index 0000000..61d92f6 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -0,0 +1,188 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_max_velocity_publisher.hpp" +#include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_pacmod_util.hpp" +#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" +#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" +#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace autoware_api +{ +class AutowareIvAdapter : public rclcpp::Node +{ +public: + AutowareIvAdapter(); + +private: + // subscriber + rclcpp::Subscription::SharedPtr sub_steer_; + rclcpp::Subscription::SharedPtr + sub_vehicle_cmd_; + rclcpp::Subscription::SharedPtr + sub_turn_indicators_; + rclcpp::Subscription::SharedPtr + sub_hazard_lights_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_battery_; + rclcpp::Subscription::SharedPtr sub_nav_sat_; + rclcpp::Subscription::SharedPtr + sub_autoware_state_; + rclcpp::Subscription::SharedPtr + sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr + sub_hazard_status_; + rclcpp::Subscription::SharedPtr sub_stop_reason_; + rclcpp::Subscription::SharedPtr sub_v2x_command_; + rclcpp::Subscription::SharedPtr + sub_v2x_state_; + rclcpp::Subscription::SharedPtr sub_diagnostics_; + rclcpp::Subscription::SharedPtr sub_global_rpt_; + rclcpp::Subscription::SharedPtr + sub_lane_change_available_; + rclcpp::Subscription::SharedPtr + sub_lane_change_ready_; + rclcpp::Subscription::SharedPtr + sub_lane_change_candidate_; + rclcpp::Subscription::SharedPtr + sub_obstacle_avoid_ready_; + rclcpp::Subscription::SharedPtr + sub_obstacle_avoid_candidate_; + rclcpp::Subscription::SharedPtr sub_max_velocity_; + rclcpp::Subscription::SharedPtr + sub_current_max_velocity_; + rclcpp::Subscription::SharedPtr sub_temporary_stop_; + rclcpp::Subscription::SharedPtr sub_autoware_traj_; + rclcpp::Subscription::SharedPtr sub_door_control_; + rclcpp::Subscription::SharedPtr sub_door_status_; + + // publisher + rclcpp::Publisher::SharedPtr pub_door_control_; + rclcpp::Publisher::SharedPtr pub_door_status_; + rclcpp::Publisher::SharedPtr pub_v2x_command_; + rclcpp::Publisher::SharedPtr pub_v2x_state_; + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // callback function + void callbackSteer(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr); + void callbackVehicleCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg_ptr); + void callbackTurnIndicators( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr); + void callbackHazardLights( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg_ptr); + void callbackTwist(const nav_msgs::msg::Odometry::ConstSharedPtr msg_ptr); + void callbackGear(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg_ptr); + void callbackBattery(const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr msg_ptr); + void callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr); + void callbackAutowareState( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); + void callbackControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg_ptr); + void callbackGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); + void callbackEmergencyState( + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg_ptr); + void callbackHazardStatus( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); + void callbackStopReason(const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); + void callbackV2XCommand( + const tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr); + void callbackV2XState( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr); + void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); + void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); + void callbackLaneChangeAvailable( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); + void callbackLaneChangeReady( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); + void callbackLaneChangeCandidatePath( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); + void callbackLaneObstacleAvoidReady( + const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr); + void callbackLaneObstacleAvoidCandidatePath( + const autoware_auto_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); + void callbackTemporaryStop(const tier4_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr); + void callbackAutowareTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + void callbackDoorControl(const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr); + void callbackDoorStatus(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); + + // timer function + void timerCallback(); + + void emergencyParamCheck(const bool emergency_stop_param); + void getCurrentPose(); + + // parameter + AutowareInfo aw_info_; + std::unique_ptr vehicle_state_publisher_; + std::unique_ptr autoware_state_publisher_; + std::unique_ptr stop_reason_aggregator_; + std::unique_ptr v2x_aggregator_; + std::unique_ptr lane_change_state_publisher_; + std::unique_ptr obstacle_avoidance_state_publisher_; + std::unique_ptr max_velocity_publisher_; + double status_pub_hz_; + double stop_reason_timeout_; + double stop_reason_thresh_dist_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp new file mode 100644 index 0000000..c540aa7 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -0,0 +1,52 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + +namespace autoware_api +{ +class AutowareIvLaneChangeStatePublisher +{ +public: + explicit AutowareIvLaneChangeStatePublisher(rclcpp::Node & node); + void statePublisher(const AutowareInfo & aw_info); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + // publisher + rclcpp::Publisher::SharedPtr pub_state_; + + void getLaneChangeAvailableInfo( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, + tier4_api_msgs::msg::LaneChangeStatus * status); + void getLaneChangeReadyInfo( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, + tier4_api_msgs::msg::LaneChangeStatus * status); + void getCandidatePathInfo( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + tier4_api_msgs::msg::LaneChangeStatus * status); +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ 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 new file mode 100644 index 0000000..507afd6 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + +namespace autoware_api +{ +class AutowareIvMaxVelocityPublisher +{ +public: + AutowareIvMaxVelocityPublisher(rclcpp::Node & node, const double default_max_velocity); + void statePublisher(const AutowareInfo & aw_info); + +private: + // publisher + rclcpp::Publisher::SharedPtr pub_state_; + + bool calcMaxVelocity( + const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, + const tier4_api_msgs::msg::StopCommand::ConstSharedPtr & temporary_stop_ptr, + float * max_velocity); + + double default_max_velocity_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp new file mode 100644 index 0000000..7ac755e --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -0,0 +1,49 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + +namespace autoware_api +{ +class AutowareIvObstacleAvoidanceStatePublisher +{ +public: + explicit AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node & node); + void statePublisher(const AutowareInfo & aw_info); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + // publisher + rclcpp::Publisher::SharedPtr pub_state_; + + void getObstacleAvoidReadyInfo( + const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, + tier4_api_msgs::msg::ObstacleAvoidanceStatus * status); + void getCandidatePathInfo( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + tier4_api_msgs::msg::ObstacleAvoidanceStatus * status); +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp new file mode 100644 index 0000000..8426a38 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp @@ -0,0 +1,40 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ + +#include + +#include +#include +#include +#include + +namespace autoware_api +{ +namespace pacmod_util +{ +tier4_api_msgs::msg::DoorStatus getDoorStatusMsg( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr); +pacmod3_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( + const rclcpp::Clock::SharedPtr & clock); +pacmod3_msgs::msg::SystemCmdInt createDoorCommand( + const rclcpp::Clock::SharedPtr & clock, + const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr); +} // namespace pacmod_util + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp new file mode 100644 index 0000000..b05ec18 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -0,0 +1,64 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + +namespace autoware_api +{ +class AutowareIvStopReasonAggregator +{ +public: + AutowareIvStopReasonAggregator( + rclcpp::Node & node, const double timeout, const double thresh_dist_to_stop_pose); + tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info); + +private: + void applyUpdate( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info); + bool checkMatchingReason( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, + const tier4_planning_msgs::msg::StopReasonArray & stop_reason_array); + void applyTimeOut(); + void appendStopReasonToArray( + const tier4_planning_msgs::msg::StopReason & stop_reason, + tier4_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info); + tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr makeStopReasonArray( + const AutowareInfo & aw_info); + tier4_planning_msgs::msg::StopReason inputStopDistToStopReason( + const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); + double calcStopDistToStopFactor( + const tier4_planning_msgs::msg::StopFactor & stop_factor, const AutowareInfo & aw_info); + tier4_planning_msgs::msg::StopReason getNearStopReason( + const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); + + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + double timeout_; + double thresh_dist_to_stop_pose_; + std::vector stop_reason_array_vec_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp new file mode 100644 index 0000000..11406da --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace autoware_api +{ +using Command = tier4_v2x_msgs::msg::InfrastructureCommand; +using CommandArray = tier4_v2x_msgs::msg::InfrastructureCommandArray; +using State = tier4_v2x_msgs::msg::VirtualTrafficLightState; +using StateArray = tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; + +class AutowareIvV2XAggregator +{ +public: + explicit AutowareIvV2XAggregator(rclcpp::Node & node); + + CommandArray::ConstSharedPtr updateV2XCommand(const CommandArray::ConstSharedPtr & msg); + + StateArray::ConstSharedPtr updateV2XState(const StateArray::ConstSharedPtr & msg); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + const double max_delay_sec_{5.0}; + const double max_clock_error_sec_{300.0}; + std::map command_map_; + std::map state_map_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp new file mode 100644 index 0000000..20d4429 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -0,0 +1,83 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + +#include + +namespace autoware_api +{ +class AutowareIvVehicleStatePublisher +{ +public: + explicit AutowareIvVehicleStatePublisher(rclcpp::Node & node); + void statePublisher(const AutowareInfo & aw_info); + +private: + // publisher + rclcpp::Publisher::SharedPtr pub_state_; + + tier4_api_msgs::msg::AwapiVehicleStatus initVehicleStatus(); + void getPoseInfo( + const std::shared_ptr & pose_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getSteerInfo( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getVehicleCmdInfo( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & + vehicle_cmd_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getTurnSignalInfo( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & + turn_indicators_ptr, + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & hazard_lights_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getTwistInfo( + const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getGearInfo( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getBatteryInfo( + const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + void getGpsInfo( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status); + + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + // parameters + nav_msgs::msg::Odometry::ConstSharedPtr previous_odometry_ptr_; + autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr previous_steer_ptr_; + double prev_accel_; + double prev_steer_vel_; + + // defined value + const double accel_lowpass_gain_ = 0.2; + const double steer_vel_lowpass_gain_ = 0.2; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp new file mode 100644 index 0000000..87e8486 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp @@ -0,0 +1,118 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ +#define AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ + +#include + +#include +#include +#include + +namespace diagnostics_filter +{ +inline std::string splitStringByLastSlash(const std::string & str) +{ + const auto last_slash = str.find_last_of("/"); + + if (last_slash == std::string::npos) { + return ""; + } + + return str.substr(0, last_slash); +} + +std::vector getAllParentNames(const std::string & diag_name) +{ + std::vector all_parent_names; + + auto parent_name = splitStringByLastSlash(diag_name); + while (parent_name != "") { + all_parent_names.push_back(parent_name); + parent_name = splitStringByLastSlash(parent_name); + } + + return all_parent_names; +} + +inline bool isChild( + const diagnostic_msgs::msg::DiagnosticStatus & child, + const diagnostic_msgs::msg::DiagnosticStatus & parent) +{ + auto name = splitStringByLastSlash(child.name); + while (name != "") { + if (name == parent.name) { + return true; + } + + name = splitStringByLastSlash(name); + } + + return false; +} + +inline bool isLeaf( + const std::unordered_set & diag_name_set, + const diagnostic_msgs::msg::DiagnosticStatus & diag) +{ + return diag_name_set.count(diag.name) == 0; +} + +inline std::unordered_set createDiagNameSet( + const std::vector & diagnostics) +{ + std::unordered_set diag_name_set; + + for (const auto & diag : diagnostics) { + for (const auto & parent_name : getAllParentNames(diag.name)) { + diag_name_set.insert(parent_name); + } + } + + return diag_name_set; +} + +inline std::vector extractLeafDiagnostics( + const std::vector & diagnostics) +{ + const auto diag_name_set = createDiagNameSet(diagnostics); + + std::vector leaf_diagnostics; + for (const auto & diag : diagnostics) { + if (isLeaf(diag_name_set, diag)) { + leaf_diagnostics.push_back(diag); + } + } + + return leaf_diagnostics; +} + +inline std::vector extractLeafChildrenDiagnostics( + const diagnostic_msgs::msg::DiagnosticStatus & parent, + const std::vector & diagnostics) +{ + std::vector leaf_children_diagnostics; + for (const auto & diag : extractLeafDiagnostics(diagnostics)) { + if (isChild(diag, parent)) { + leaf_children_diagnostics.push_back(diag); + } + } + + return leaf_children_diagnostics; +} + +} // namespace diagnostics_filter + +#endif // AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ diff --git a/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml new file mode 100644 index 0000000..5038612 --- /dev/null +++ b/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi_awiv_adapter/launch/awapi_relay_container.launch.py new file mode 100644 index 0000000..e743f0a --- /dev/null +++ b/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -0,0 +1,425 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + relay_components = [] + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="route_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_route"), + "output_topic": LaunchConfiguration("get_route"), + "type": "autoware_auto_planning_msgs/msg/HADMapRoute", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="predict_object_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_object"), + "output_topic": LaunchConfiguration("get_predicted_object"), + "type": "autoware_auto_perception_msgs/msg/PredictedObjects", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="nearest_traffic_signal_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_nearest_traffic_signal"), + "output_topic": LaunchConfiguration("get_nearest_traffic_signal"), + "type": "autoware_auto_perception_msgs/msg/LookingTrafficSignal", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="ready_module_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_path_change_ready"), + "output_topic": LaunchConfiguration("get_path_change_ready"), + "type": "tier4_planning_msgs/msg/PathChangeModule", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="force_available_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_path_change_force_available"), + "output_topic": LaunchConfiguration("get_path_change_force_available"), + "type": "tier4_planning_msgs/msg/PathChangeModuleArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="running_modules_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_path_change_running"), + "output_topic": LaunchConfiguration("get_path_change_running"), + "type": "tier4_planning_msgs/msg/PathChangeModuleArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="autoware_engage_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_engage"), + "output_topic": LaunchConfiguration("output_autoware_engage"), + "type": "autoware_auto_vehicle_msgs/msg/Engage", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="vehicle_engage_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_engage"), + "output_topic": LaunchConfiguration("output_vehicle_engage"), + "type": "autoware_auto_vehicle_msgs/msg/Engage", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="put_route_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_route"), + "output_topic": LaunchConfiguration("output_route"), + "type": "autoware_auto_planning_msgs/msg/HADMapRoute", + "durability": "transient_local", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="put_goal_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_goal"), + "output_topic": LaunchConfiguration("output_goal"), + "type": "geometry_msgs/msg/PoseStamped", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="lane_change_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_lane_change_approval"), + "output_topic": LaunchConfiguration("output_lane_change_approval"), + "type": "tier4_planning_msgs/msg/LaneChangeCommand", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="force_lane_change_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_force_lane_change"), + "output_topic": LaunchConfiguration("output_force_lane_change"), + "type": "tier4_planning_msgs/msg/LaneChangeCommand", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="external_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_path_change_approval"), + "output_topic": LaunchConfiguration("output_path_change_approval"), + "type": "tier4_planning_msgs/msg/Approval", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="force_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_path_change_force"), + "output_topic": LaunchConfiguration("output_path_change_force"), + "type": "tier4_planning_msgs/msg/PathChangeModule", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="obstacle_avoid_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_obstacle_avoid_approval"), + "output_topic": LaunchConfiguration("output_obstacle_avoid_approval"), + "type": "tier4_planning_msgs/msg/EnableAvoidance", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="traffic_signal_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_traffic_signals"), + "output_topic": LaunchConfiguration("get_traffic_signals"), + "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="overwrite_traffic_signals_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_overwrite_traffic_signals"), + "output_topic": LaunchConfiguration("output_overwrite_traffic_signals"), + "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="speed_exceeded_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_stop_speed_exceeded"), + "output_topic": LaunchConfiguration("get_stop_speed_exceeded"), + "type": "tier4_planning_msgs/msg/StopSpeedExceeded", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="crosswalk_status_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_crosswalk_status"), + "output_topic": LaunchConfiguration("input_external_crosswalk_status"), + "type": "tier4_api_msgs/msg/CrosswalkStatus", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="intersection_status_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_intersection_status"), + "output_topic": LaunchConfiguration("input_external_intersection_status"), + "type": "tier4_api_msgs/msg/IntersectionStatus", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="expand_stop_range_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_expand_stop_range"), + "output_topic": LaunchConfiguration("input_expand_stop_range"), + "type": "tier4_planning_msgs/msg/ExpandStopRange", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="pose_initialization_request_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_pose_initialization_request"), + "output_topic": LaunchConfiguration("input_pose_initialization_request"), + "type": "tier4_localization_msgs/msg/PoseInitializationRequest", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + container = ComposableNodeContainer( + name="awapi_relay_container", + namespace="awapi", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=relay_components, + output="screen", + ) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + [set_container_executable, set_container_mt_executable] + [container] + ) diff --git a/awapi_awiv_adapter/package.xml b/awapi_awiv_adapter/package.xml new file mode 100644 index 0000000..0edb0a9 --- /dev/null +++ b/awapi_awiv_adapter/package.xml @@ -0,0 +1,43 @@ + + + awapi_awiv_adapter + 0.1.0 + The awapi_awiv_adapter package + Tomoya Kimura + Tomoya Kimura + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_control_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + diagnostic_msgs + geometry_msgs + nav_msgs + pacmod3_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tier4_api_msgs + tier4_auto_msgs_converter + tier4_control_msgs + tier4_external_api_msgs + tier4_planning_msgs + tier4_v2x_msgs + tier4_vehicle_msgs + + autoware_auto_perception_msgs + tier4_perception_msgs + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp new file mode 100644 index 0000000..12e7af3 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -0,0 +1,302 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" + +#include "awapi_awiv_adapter/diagnostics_filter.hpp" +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" + +#include +#include +#include + +namespace autoware_api +{ +AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node & node) +: logger_(node.get_logger().get_child("awapi_awiv_autoware_state_publisher")), + clock_(node.get_clock()), + arrived_goal_(false) +{ + // publisher + pub_state_ = + node.create_publisher("output/autoware_status", 1); +} + +void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + tier4_api_msgs::msg::AwapiAutowareStatus status; + + // input header + status.header.frame_id = "base_link"; + status.header.stamp = clock_->now(); + + // get all info + getAutowareStateInfo(aw_info.autoware_state_ptr, &status); + getControlModeInfo(aw_info.control_mode_ptr, &status); + getGateModeInfo(aw_info.gate_mode_ptr, &status); + getEmergencyStateInfo(aw_info.emergency_state_ptr, &status); + getCurrentMaxVelInfo(aw_info.current_max_velocity_ptr, &status); + getHazardStatusInfo(aw_info, &status); + getStopReasonInfo(aw_info.stop_reason_ptr, &status); + getDiagInfo(aw_info, &status); + getErrorDiagInfo(aw_info, &status); + getGlobalRptInfo(aw_info.global_rpt_ptr, &status); + + // publish info + pub_state_->publish(status); +} + +void AutowareIvAutowareStatePublisher::getAutowareStateInfo( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!autoware_state_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "autoware_state is nullptr"); + return; + } + + // get autoware_state + using tier4_auto_msgs_converter::convert; + status->autoware_state = convert(*autoware_state_ptr).state; + status->arrived_goal = isGoal(autoware_state_ptr); +} + +void AutowareIvAutowareStatePublisher::getControlModeInfo( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!control_mode_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "control mode is nullptr"); + return; + } + + // get control mode + using tier4_auto_msgs_converter::convert; + status->control_mode = convert(*control_mode_ptr).data; +} + +void AutowareIvAutowareStatePublisher::getGateModeInfo( + const tier4_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!gate_mode_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gate mode is nullptr"); + return; + } + + // get control mode + status->gate_mode = gate_mode_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!emergency_state_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "emergency_state is nullptr"); + return; + } + + // get emergency + using autoware_auto_system_msgs::msg::EmergencyState; + status->emergency_stopped = (emergency_state_ptr->state == EmergencyState::MRM_OPERATING) || + (emergency_state_ptr->state == EmergencyState::MRM_SUCCEEDED) || + (emergency_state_ptr->state == EmergencyState::MRM_FAILED); +} + +void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!current_max_velocity_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] current_max_velocity is nullptr"); + return; + } + + // get current max velocity + status->current_max_velocity = current_max_velocity_ptr->max_velocity; +} + +void AutowareIvAutowareStatePublisher::getHazardStatusInfo( + const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!aw_info.autoware_state_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + return; + } + + if (!aw_info.control_mode_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] control_mode is nullptr"); + return; + } + + if (!aw_info.hazard_status_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] hazard_status is nullptr"); + return; + } + + // get emergency + using tier4_auto_msgs_converter::convert; + status->hazard_status = convert(*aw_info.hazard_status_ptr); + + // filter leaf diagnostics + status->hazard_status.status.diagnostics_spf = + diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_spf); + status->hazard_status.status.diagnostics_lf = + diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_lf); + status->hazard_status.status.diagnostics_sf = + diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_sf); + status->hazard_status.status.diagnostics_nf = + diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_nf); +} + +void AutowareIvAutowareStatePublisher::getStopReasonInfo( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!stop_reason_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "stop reason is nullptr"); + return; + } + + status->stop_reason = *stop_reason_ptr; +} + +void AutowareIvAutowareStatePublisher::getDiagInfo( + const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!aw_info.diagnostic_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + return; + } + + // get diag + status->diagnostics = diagnostics_filter::extractLeafDiagnostics(aw_info.diagnostic_ptr->status); +} + +// This function is tentative and should be replaced with getHazardStatusInfo. +// TODO(Kenji Miyake): Make getErrorDiagInfo users to use getHazardStatusInfo. +void AutowareIvAutowareStatePublisher::getErrorDiagInfo( + const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + using autoware_auto_system_msgs::msg::AutowareState; + using autoware_auto_vehicle_msgs::msg::ControlModeReport; + + if (!aw_info.autoware_state_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + return; + } + + if (!aw_info.control_mode_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] control mode is nullptr"); + return; + } + + if (!aw_info.diagnostic_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + return; + } + + if (!aw_info.hazard_status_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] hazard_status is nullptr"); + return; + } + + // get diag + using diagnostic_msgs::msg::DiagnosticStatus; + const auto & hazard_status = aw_info.hazard_status_ptr->status; + std::vector error_diagnostics; + + for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { + auto diag = hazard_diag; + diag.message = "[Single Point Fault]" + hazard_diag.message; + error_diagnostics.push_back(diag); + } + for (const auto & hazard_diag : hazard_status.diag_latent_fault) { + auto diag = hazard_diag; + diag.message = "[Latent Fault]" + hazard_diag.message; + error_diagnostics.push_back(diag); + } + for (const auto & hazard_diag : hazard_status.diag_safe_fault) { + auto diag = hazard_diag; + diag.message = "[Safe Fault]" + hazard_diag.message; + error_diagnostics.push_back(diag); + } + for (const auto & hazard_diag : hazard_status.diag_no_fault) { + auto diag = hazard_diag; + diag.message = "[No Fault]" + hazard_diag.message; + diag.level = DiagnosticStatus::OK; + error_diagnostics.push_back(diag); + } + + // filter leaf diag + status->error_diagnostics = diagnostics_filter::extractLeafDiagnostics(error_diagnostics); +} + +void AutowareIvAutowareStatePublisher::getGlobalRptInfo( + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + tier4_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!global_rpt_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "global_rpt is nullptr"); + return; + } + + // get global_rpt + status->autonomous_overridden = global_rpt_ptr->override_active; +} + +bool AutowareIvAutowareStatePublisher::isGoal( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state) +{ + // rename + const auto & aw_state = autoware_state->state; + + if (aw_state == autoware_auto_system_msgs::msg::AutowareState::ARRIVED_GOAL) { + arrived_goal_ = true; + } else if ( // NOLINT + prev_state_ == autoware_auto_system_msgs::msg::AutowareState::DRIVING && + aw_state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { + arrived_goal_ = true; + } + + if ( + aw_state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || + aw_state == autoware_auto_system_msgs::msg::AutowareState::DRIVING) { + // cancel goal state + arrived_goal_ = false; + } + + prev_state_ = aw_state; + + return arrived_goal_; +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi_awiv_adapter/src/awapi_autoware_util.cpp new file mode 100644 index 0000000..7d6ccba --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -0,0 +1,108 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +namespace autoware_api +{ +double lowpass_filter(const double current_value, const double prev_value, const double gain) +{ + return gain * prev_value + (1.0 - gain) * current_value; +} + +namespace planning_util +{ +bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + size_t & output_closest_idx, const double dist_thr, const double angle_thr) +{ + double dist_min = std::numeric_limits::max(); + const double yaw_pose = tf2::getYaw(pose.orientation); + int closest_idx = -1; + + for (int i = 0; i < static_cast(traj.points.size()); ++i) { + const double dist = calcDist2d(getPose(traj, i).position, pose.position); + + /* check distance threshold */ + if (dist > dist_thr) { + continue; + } + + /* check angle threshold */ + double yaw_i = tf2::getYaw(getPose(traj, i).orientation); + double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_i); + + if (std::fabs(yaw_diff) > angle_thr) { + continue; + } + + if (dist < dist_min) { + dist_min = dist; + closest_idx = i; + } + } + + output_closest_idx = static_cast(closest_idx); + + return closest_idx != -1; +} + +double normalizeEulerAngle(double euler) +{ + double res = euler; + while (res > M_PI) { + res -= (2.0 * M_PI); + } + while (res < -M_PI) { + res += 2.0 * M_PI; + } + + return res; +} + +double calcArcLengthFromWayPoint( + const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const size_t dst_idx) +{ + double length = 0; + for (size_t i = src_idx; i < dst_idx; ++i) { + const double dx_wp = + input_path.points.at(i + 1).pose.position.x - input_path.points.at(i).pose.position.x; + const double dy_wp = + input_path.points.at(i + 1).pose.position.y - input_path.points.at(i).pose.position.y; + length += std::hypot(dx_wp, dy_wp); + } + return length; +} + +double calcDistanceAlongTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose) +{ + size_t self_idx; + size_t stop_idx; + if ( + !calcClosestIndex(trajectory, current_pose, self_idx) || + !calcClosestIndex(trajectory, target_pose, stop_idx)) { + return std::numeric_limits::max(); + } + const double dist_to_stop_pose = calcArcLengthFromWayPoint(trajectory, self_idx, stop_idx); + return dist_to_stop_pose; +} + +} // namespace planning_util + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp new file mode 100644 index 0000000..f1ba0b1 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -0,0 +1,384 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" + +#include +#include +#include + +namespace autoware_api +{ +using std::placeholders::_1; + +AutowareIvAdapter::AutowareIvAdapter() +: Node("awapi_awiv_adapter_node"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +{ + // get param + status_pub_hz_ = this->declare_parameter("status_pub_hz", 5.0); + stop_reason_timeout_ = this->declare_parameter("stop_reason_timeout", 0.5); + stop_reason_thresh_dist_ = this->declare_parameter("stop_reason_thresh_dist", 100.0); + const double default_max_velocity = waitForParam( + this, declare_parameter("node/max_velocity", ""), declare_parameter("param/max_velocity", "")); + const bool em_stop_param = waitForParam( + this, declare_parameter("node/emergency_stop", ""), + declare_parameter("param/emergency_stop", "")); + emergencyParamCheck(em_stop_param); + + // setup instance + vehicle_state_publisher_ = std::make_unique(*this); + autoware_state_publisher_ = std::make_unique(*this); + stop_reason_aggregator_ = std::make_unique( + *this, stop_reason_timeout_, stop_reason_thresh_dist_); + v2x_aggregator_ = std::make_unique(*this); + lane_change_state_publisher_ = std::make_unique(*this); + obstacle_avoidance_state_publisher_ = + std::make_unique(*this); + max_velocity_publisher_ = + std::make_unique(*this, default_max_velocity); + + // publisher + pub_door_control_ = + this->create_publisher("output/door_control", 1); + pub_door_status_ = + this->create_publisher("output/door_status", 1); + pub_v2x_command_ = this->create_publisher( + "output/v2x_command", 1); + pub_v2x_state_ = this->create_publisher( + "output/v2x_state", 1); + + // subscriber + + auto durable_qos = rclcpp::QoS{1}.transient_local(); + + sub_steer_ = this->create_subscription( + "input/steer", 1, std::bind(&AutowareIvAdapter::callbackSteer, this, _1)); + sub_vehicle_cmd_ = + this->create_subscription( + "input/vehicle_cmd", durable_qos, + std::bind(&AutowareIvAdapter::callbackVehicleCmd, this, _1)); + sub_turn_indicators_ = + this->create_subscription( + "input/turn_indicators", 1, std::bind(&AutowareIvAdapter::callbackTurnIndicators, this, _1)); + sub_hazard_lights_ = + this->create_subscription( + "input/hazard_lights", 1, std::bind(&AutowareIvAdapter::callbackHazardLights, this, _1)); + sub_odometry_ = this->create_subscription( + "input/odometry", 1, std::bind(&AutowareIvAdapter::callbackTwist, this, _1)); + sub_gear_ = this->create_subscription( + "input/gear", 1, std::bind(&AutowareIvAdapter::callbackGear, this, _1)); + sub_battery_ = this->create_subscription( + "input/battery", 1, std::bind(&AutowareIvAdapter::callbackBattery, this, _1)); + sub_nav_sat_ = this->create_subscription( + "input/nav_sat", 1, std::bind(&AutowareIvAdapter::callbackNavSat, this, _1)); + sub_autoware_state_ = this->create_subscription( + "input/autoware_state", 1, std::bind(&AutowareIvAdapter::callbackAutowareState, this, _1)); + sub_control_mode_ = this->create_subscription( + "input/control_mode", 1, std::bind(&AutowareIvAdapter::callbackControlMode, this, _1)); + sub_gate_mode_ = this->create_subscription( + "input/gate_mode", durable_qos, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); + sub_emergency_ = this->create_subscription( + "input/emergency_state", 1, std::bind(&AutowareIvAdapter::callbackEmergencyState, this, _1)); + sub_hazard_status_ = + this->create_subscription( + "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); + sub_stop_reason_ = this->create_subscription( + "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); + sub_v2x_command_ = this->create_subscription( + "input/v2x_command", 100, std::bind(&AutowareIvAdapter::callbackV2XCommand, this, _1)); + sub_v2x_state_ = this->create_subscription( + "input/v2x_state", 100, std::bind(&AutowareIvAdapter::callbackV2XState, this, _1)); + sub_diagnostics_ = this->create_subscription( + "input/diagnostics", 1, std::bind(&AutowareIvAdapter::callbackDiagnostics, this, _1)); + sub_global_rpt_ = this->create_subscription( + "input/global_rpt", 1, std::bind(&AutowareIvAdapter::callbackGlobalRpt, this, _1)); + sub_lane_change_available_ = + this->create_subscription( + "input/lane_change_available", 1, + std::bind(&AutowareIvAdapter::callbackLaneChangeAvailable, this, _1)); + sub_lane_change_ready_ = this->create_subscription( + "input/lane_change_ready", 1, std::bind(&AutowareIvAdapter::callbackLaneChangeReady, this, _1)); + sub_lane_change_candidate_ = this->create_subscription( + "input/lane_change_candidate_path", 1, + std::bind(&AutowareIvAdapter::callbackLaneChangeCandidatePath, this, _1)); + sub_obstacle_avoid_ready_ = + this->create_subscription( + "input/obstacle_avoid_ready", durable_qos, + std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); + sub_obstacle_avoid_candidate_ = + this->create_subscription( + "input/obstacle_avoid_candidate_path", durable_qos, + 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_temporary_stop_ = this->create_subscription( + "input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1)); + sub_autoware_traj_ = this->create_subscription( + "input/autoware_trajectory", 1, + std::bind(&AutowareIvAdapter::callbackAutowareTrajectory, this, _1)); + sub_door_control_ = this->create_subscription( + "input/door_control", 1, std::bind(&AutowareIvAdapter::callbackDoorControl, this, _1)); + sub_door_status_ = this->create_subscription( + "input/door_status", 1, std::bind(&AutowareIvAdapter::callbackDoorStatus, this, _1)); + + // timer + auto timer_callback = std::bind(&AutowareIvAdapter::timerCallback, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / status_pub_hz_)); + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void AutowareIvAdapter::emergencyParamCheck(const bool emergency_stop_param) +{ + if (!emergency_stop_param) { + RCLCPP_WARN_STREAM(get_logger(), "parameter[use_external_emergency_stop] is false."); + RCLCPP_WARN_STREAM(get_logger(), "autoware/put/emergency is not valid"); + } +} + +void AutowareIvAdapter::timerCallback() +{ + // get current pose + getCurrentPose(); + + // publish vehicle state + vehicle_state_publisher_->statePublisher(aw_info_); + + // publish autoware state + autoware_state_publisher_->statePublisher(aw_info_); + + // publish lane change state + lane_change_state_publisher_->statePublisher(aw_info_); + + // publish obstacle_avoidance state + obstacle_avoidance_state_publisher_->statePublisher(aw_info_); + + // publish pacmod door status + pub_door_status_->publish(pacmod_util::getDoorStatusMsg(aw_info_.door_state_ptr)); + + // publish v2x command and state + if (aw_info_.v2x_command_ptr) { + pub_v2x_command_->publish(*aw_info_.v2x_command_ptr); + } + if (aw_info_.v2x_state_ptr) { + pub_v2x_state_->publish(*aw_info_.v2x_state_ptr); + } +} + +void AutowareIvAdapter::callbackSteer( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) +{ + aw_info_.steer_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackVehicleCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg_ptr) +{ + aw_info_.vehicle_cmd_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTurnIndicators( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) +{ + aw_info_.turn_indicators_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackHazardLights( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg_ptr) +{ + aw_info_.hazard_lights_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTwist(const nav_msgs::msg::Odometry::ConstSharedPtr msg_ptr) +{ + aw_info_.odometry_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGear( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg_ptr) +{ + aw_info_.gear_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackBattery( + const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr msg_ptr) +{ + aw_info_.battery_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr) +{ + aw_info_.nav_sat_ptr = msg_ptr; +} + +void AutowareIvAdapter::getCurrentPose() +{ + try { + auto transform = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero); + geometry_msgs::msg::PoseStamped ps; + ps.header = transform.header; + ps.pose.position.x = transform.transform.translation.x; + ps.pose.position.y = transform.transform.translation.y; + ps.pose.position.z = transform.transform.translation.z; + ps.pose.orientation = transform.transform.rotation; + aw_info_.current_pose_ptr = std::make_shared(ps); + } catch (tf2::TransformException & ex) { + RCLCPP_DEBUG_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 2000 /* ms */, "cannot get self pose"); + } +} + +void AutowareIvAdapter::callbackAutowareState( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr) +{ + aw_info_.autoware_state_ptr = msg_ptr; +} +void AutowareIvAdapter::callbackControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg_ptr) +{ + aw_info_.control_mode_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGateMode( + const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr) +{ + aw_info_.gate_mode_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackEmergencyState( + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg_ptr) +{ + aw_info_.emergency_state_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackHazardStatus( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) +{ + aw_info_.hazard_status_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackStopReason( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr) +{ + aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_); +} + +void AutowareIvAdapter::callbackV2XCommand( + const tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr) +{ + aw_info_.v2x_command_ptr = v2x_aggregator_->updateV2XCommand(msg_ptr); +} + +void AutowareIvAdapter::callbackV2XState( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr) +{ + aw_info_.v2x_state_ptr = v2x_aggregator_->updateV2XState(msg_ptr); +} + +void AutowareIvAdapter::callbackDiagnostics( + const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr) +{ + aw_info_.diagnostic_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGlobalRpt( + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr) +{ + aw_info_.global_rpt_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeAvailable( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr) +{ + aw_info_.lane_change_available_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeReady( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr) +{ + aw_info_.lane_change_ready_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeCandidatePath( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) +{ + aw_info_.lane_change_candidate_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneObstacleAvoidReady( + const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr) +{ + aw_info_.obstacle_avoid_ready_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) +{ + aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackMaxVelocity( + const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) +{ + aw_info_.max_velocity_ptr = msg_ptr; + max_velocity_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackCurrentMaxVelocity( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) +{ + aw_info_.current_max_velocity_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTemporaryStop( + const tier4_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr) +{ + if (aw_info_.temporary_stop_ptr) { + if (aw_info_.temporary_stop_ptr->stop == msg_ptr->stop) { + // if same value as last time is sent, ignore msg. + return; + } + } + + aw_info_.temporary_stop_ptr = msg_ptr; + max_velocity_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackAutowareTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) +{ + aw_info_.autoware_planning_traj_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackDoorControl( + const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr) +{ + pub_door_control_->publish(pacmod_util::createClearOverrideDoorCommand(this->get_clock())); + rclcpp::Rate(10.0).sleep(); // avoid message loss + pub_door_control_->publish(pacmod_util::createDoorCommand(this->get_clock(), msg_ptr)); +} + +void AutowareIvAdapter::callbackDoorStatus( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr) +{ + aw_info_.door_state_ptr = msg_ptr; +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp new file mode 100644 index 0000000..c3bc000 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp new file mode 100644 index 0000000..cb74bf2 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -0,0 +1,88 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" + +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" + +namespace autoware_api +{ +AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::Node & node) +: logger_(node.get_logger().get_child("awapi_awiv_lane_change_state_publisher")), + clock_(node.get_clock()) +{ + // publisher + pub_state_ = + node.create_publisher("output/lane_change_status", 1); +} + +void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + tier4_api_msgs::msg::LaneChangeStatus status; + + // input header + status.header.frame_id = "base_link"; + status.header.stamp = clock_->now(); + + // get all info + getLaneChangeAvailableInfo(aw_info.lane_change_available_ptr, &status); + getLaneChangeReadyInfo(aw_info.lane_change_ready_ptr, &status); + getCandidatePathInfo(aw_info.lane_change_candidate_ptr, &status); + + // publish info + pub_state_->publish(status); +} + +void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, + tier4_api_msgs::msg::LaneChangeStatus * status) +{ + if (!available_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "lane change available is nullptr"); + return; + } + + // get lane change available info + status->force_lane_change_available = available_ptr->status; +} + +void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( + const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, + tier4_api_msgs::msg::LaneChangeStatus * status) +{ + if (!ready_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "lane change ready is nullptr"); + return; + } + + // get lane change available info + status->lane_change_ready = ready_ptr->status; +} + +void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + tier4_api_msgs::msg::LaneChangeStatus * status) +{ + if (!path_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "lane_change_candidate_path is nullptr"); + return; + } + + using tier4_auto_msgs_converter::convert; + status->candidate_path = convert(*path_ptr); +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp new file mode 100644 index 0000000..0866ae2 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -0,0 +1,60 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_max_velocity_publisher.hpp" + +namespace autoware_api +{ +AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( + rclcpp::Node & node, const double default_max_velocity) +: default_max_velocity_(default_max_velocity) +{ + // publisher + pub_state_ = + node.create_publisher("output/max_velocity", 1); +} + +void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info) +{ + tier4_planning_msgs::msg::VelocityLimit max_velocity; + if (calcMaxVelocity( + aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr, + &max_velocity.max_velocity)) // publish info + { + pub_state_->publish(max_velocity); + } +} + +bool AutowareIvMaxVelocityPublisher::calcMaxVelocity( + const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, + const tier4_api_msgs::msg::StopCommand::ConstSharedPtr & temporary_stop_ptr, float * max_velocity) +{ + if (!max_velocity_ptr && !temporary_stop_ptr) { + // receive no max velocity information + return false; + } + + // input max velocity + *max_velocity = + static_cast(max_velocity_ptr ? max_velocity_ptr->max_velocity : default_max_velocity_); + + if (temporary_stop_ptr && temporary_stop_ptr->stop) { + // if temporary_stop is true, max velocity is 0 + *max_velocity = static_cast(0.0); + } + + return true; +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp new file mode 100644 index 0000000..e877a47 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -0,0 +1,74 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" + +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" + +namespace autoware_api +{ +AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher( + rclcpp::Node & node) +: logger_(node.get_logger().get_child("awapi_awiv_obstacle_avoidance_state_publisher")), + clock_(node.get_clock()) +{ + // publisher + pub_state_ = node.create_publisher( + "output/obstacle_avoid_status", 1); +} + +void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + tier4_api_msgs::msg::ObstacleAvoidanceStatus status; + + // input header + status.header.frame_id = "base_link"; + status.header.stamp = clock_->now(); + + // get all info + getObstacleAvoidReadyInfo(aw_info.obstacle_avoid_ready_ptr, &status); + getCandidatePathInfo(aw_info.obstacle_avoid_candidate_ptr, &status); + + // publish info + pub_state_->publish(status); +} + +void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( + const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, + tier4_api_msgs::msg::ObstacleAvoidanceStatus * status) +{ + if (!ready_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_ready is nullptr"); + return; + } + + status->obstacle_avoidance_ready = ready_ptr->is_avoidance_possible; +} + +void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + tier4_api_msgs::msg::ObstacleAvoidanceStatus * status) +{ + if (!path_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_candidate_path is nullptr"); + return; + } + + using tier4_auto_msgs_converter::convert; + status->candidate_path = convert(*path_ptr); +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_pacmod_util.cpp b/awapi_awiv_adapter/src/awapi_pacmod_util.cpp new file mode 100644 index 0000000..e77b55c --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_pacmod_util.cpp @@ -0,0 +1,87 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_pacmod_util.hpp" + +namespace autoware_api +{ +namespace pacmod_util +{ +tier4_api_msgs::msg::DoorStatus getDoorStatusMsg( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr) +{ + using pacmod3_msgs::msg::SystemRptInt; + using tier4_api_msgs::msg::DoorStatus; + DoorStatus door_status; + + if (!msg_ptr) { + door_status.status = DoorStatus::NOT_APPLICABLE; + return door_status; + } + + door_status.status = DoorStatus::UNKNOWN; + + if (msg_ptr->command == SystemRptInt::DOOR_CLOSE && msg_ptr->output == SystemRptInt::DOOR_OPEN) { + // do not used (command & output are always the same value) + door_status.status = DoorStatus::DOOR_CLOSING; + } else if ( // NOLINT + msg_ptr->command == SystemRptInt::DOOR_OPEN && msg_ptr->output == SystemRptInt::DOOR_CLOSE) { + // do not used (command & output are always the same value) + door_status.status = DoorStatus::DOOR_OPENING; + } else if (msg_ptr->output == SystemRptInt::DOOR_CLOSE) { + door_status.status = DoorStatus::DOOR_CLOSED; + } else if (msg_ptr->output == SystemRptInt::DOOR_OPEN) { + door_status.status = DoorStatus::DOOR_OPENED; + } + + return door_status; +} + +pacmod3_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( + const rclcpp::Clock::SharedPtr & clock) +{ + pacmod3_msgs::msg::SystemCmdInt door_cmd; + door_cmd.header.frame_id = "base_link"; + door_cmd.header.stamp = clock->now(); + door_cmd.clear_override = true; + return door_cmd; +} + +pacmod3_msgs::msg::SystemCmdInt createDoorCommand( + const rclcpp::Clock::SharedPtr & clock, + const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr) +{ + using pacmod3_msgs::msg::SystemCmdInt; + + SystemCmdInt door_cmd; + door_cmd.header.frame_id = "base_link"; + door_cmd.header.stamp = clock->now(); + door_cmd.enable = true; + door_cmd.command = SystemCmdInt::DOOR_NEUTRAL; + + if (!msg_ptr) { + return door_cmd; + } + + if (msg_ptr->open) { + door_cmd.command = SystemCmdInt::DOOR_OPEN; + } else { + door_cmd.command = SystemCmdInt::DOOR_CLOSE; + } + return door_cmd; +} + +} // namespace pacmod_util + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp new file mode 100644 index 0000000..a43881c --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -0,0 +1,186 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" + +#include +#include + +namespace autoware_api +{ +AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator( + rclcpp::Node & node, const double timeout, const double thresh_dist_to_stop_pose) +: logger_(node.get_logger().get_child("awapi_awiv_stop_reason_aggregator")), + clock_(node.get_clock()), + timeout_(timeout), + thresh_dist_to_stop_pose_(thresh_dist_to_stop_pose) +{ +} + +tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr +AutowareIvStopReasonAggregator::updateStopReasonArray( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info) +{ + applyUpdate(msg_ptr, aw_info); + applyTimeOut(); + return makeStopReasonArray(aw_info); +} + +void AutowareIvStopReasonAggregator::applyUpdate( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + [[maybe_unused]] const AutowareInfo & aw_info) +{ + /* remove old stop_reason that matches reason with received msg */ + // make reason-matching msg list + + std::vector remove_idx; + if (!stop_reason_array_vec_.empty()) { + for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { + if (checkMatchingReason(msg_ptr, stop_reason_array_vec_.at(i))) { + remove_idx.emplace_back(i); + } + } + } + + // remove reason matching msg + for (const auto idx : remove_idx) { + stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); + } + + // add new reason msg + stop_reason_array_vec_.emplace_back(*msg_ptr); +} + +bool AutowareIvStopReasonAggregator::checkMatchingReason( + const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, + const tier4_planning_msgs::msg::StopReasonArray & stop_reason_array) +{ + for (const auto & msg_stop_reason : msg_stop_reason_array->stop_reasons) { + for (const auto & stop_reason : stop_reason_array.stop_reasons) { + if (msg_stop_reason.reason == stop_reason.reason) { + // find matching reason + return true; + } + } + } + // cannot find matching reason + return false; +} + +void AutowareIvStopReasonAggregator::applyTimeOut() +{ + const auto current_time = clock_->now(); + + // make timeout-msg list + std::vector remove_idx; + if (!stop_reason_array_vec_.empty()) { + for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { + if ( + (current_time - rclcpp::Time(stop_reason_array_vec_.at(i).header.stamp)).seconds() > + timeout_) { + remove_idx.emplace_back(i); + } + } + } + // remove timeout-msg + for (const auto idx : remove_idx) { + stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); + } +} + +void AutowareIvStopReasonAggregator::appendStopReasonToArray( + const tier4_planning_msgs::msg::StopReason & stop_reason, + tier4_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info) +{ + // calculate dist_to_stop_pose + const auto stop_reason_with_dist = inputStopDistToStopReason(stop_reason, aw_info); + + // cut stop reason + const auto near_stop_reason = getNearStopReason(stop_reason_with_dist, aw_info); + + // if stop factors is empty, not append + if (near_stop_reason.stop_factors.empty()) { + return; + } + + // if already exists same reason msg in stop_reason_array_msg, append stop_factors to there + for (auto & base_stop_reasons : stop_reason_array->stop_reasons) { + if (base_stop_reasons.reason == near_stop_reason.reason) { + base_stop_reasons.stop_factors.insert( + base_stop_reasons.stop_factors.end(), near_stop_reason.stop_factors.begin(), + near_stop_reason.stop_factors.end()); + return; + } + } + + // if not exist same reason msg, append new stop reason + stop_reason_array->stop_reasons.emplace_back(near_stop_reason); +} + +tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr +AutowareIvStopReasonAggregator::makeStopReasonArray(const AutowareInfo & aw_info) +{ + tier4_planning_msgs::msg::StopReasonArray stop_reason_array_msg; + // input header + stop_reason_array_msg.header.frame_id = "map"; + stop_reason_array_msg.header.stamp = clock_->now(); + + // input stop reason + for (const auto & stop_reason_array : stop_reason_array_vec_) { + for (const auto & stop_reason : stop_reason_array.stop_reasons) { + appendStopReasonToArray(stop_reason, &stop_reason_array_msg, aw_info); + } + } + return std::make_shared(stop_reason_array_msg); +} + +tier4_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::inputStopDistToStopReason( + const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info) +{ + if (!aw_info.autoware_planning_traj_ptr || !aw_info.current_pose_ptr) { + // pass through all stop reason + return stop_reason; + } + + auto stop_reason_with_dist = stop_reason; + for (auto & stop_factor : stop_reason_with_dist.stop_factors) { + const auto & trajectory = *aw_info.autoware_planning_traj_ptr; + const auto & current_pose = aw_info.current_pose_ptr->pose; + stop_factor.dist_to_stop_pose = + planning_util::calcDistanceAlongTrajectory(trajectory, current_pose, stop_factor.stop_pose); + } + return stop_reason_with_dist; +} + +tier4_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::getNearStopReason( + const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info) +{ + if (!aw_info.autoware_planning_traj_ptr || !aw_info.current_pose_ptr) { + // pass through all stop reason + return stop_reason; + } + + tier4_planning_msgs::msg::StopReason near_stop_reason; + near_stop_reason.reason = stop_reason.reason; + for (const auto & stop_factor : stop_reason.stop_factors) { + if (stop_factor.dist_to_stop_pose < thresh_dist_to_stop_pose_) { + // append only near stop factor + near_stop_reason.stop_factors.emplace_back(stop_factor); + } + } + return near_stop_reason; +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp b/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp new file mode 100644 index 0000000..174ee63 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp @@ -0,0 +1,104 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" + +#include +#include + +namespace autoware_api +{ +namespace +{ +std::string createKey(const Command & command) { return command.type + "-" + command.id; } + +std::string createKey(const State & state) { return state.type + "-" + state.id; } +} // namespace + +AutowareIvV2XAggregator::AutowareIvV2XAggregator(rclcpp::Node & node) +: logger_(node.get_logger().get_child("awapi_awiv_v2x_aggregator")), clock_(node.get_clock()) +{ +} + +CommandArray::ConstSharedPtr AutowareIvV2XAggregator::updateV2XCommand( + const CommandArray::ConstSharedPtr & msg) +{ + // Update data + for (const auto & command : msg->commands) { + const auto key = createKey(command); + command_map_[key] = command; + } + + // Pick valid data + auto output = std::make_shared(); + output->stamp = clock_->now(); + for (const auto & [key, command] : command_map_) { + // Calculate time diff + const auto delay = (clock_->now() - command.stamp).seconds(); + + // Ignore future data considering clock's error + if (delay < -max_clock_error_sec_) { + RCLCPP_DEBUG( + logger_, "future command: delay=%f, max_clock_error=%f", delay, max_clock_error_sec_); + continue; + } + + // Ignore old data + if (delay > max_delay_sec_) { + RCLCPP_DEBUG(logger_, "old command: delay=%f, max_delay_sec=%f", delay, max_delay_sec_); + continue; + } + + output->commands.push_back(command); + } + + return output; +} + +StateArray::ConstSharedPtr AutowareIvV2XAggregator::updateV2XState( + const StateArray::ConstSharedPtr & msg) +{ + // Update data + for (const auto & state : msg->states) { + const auto key = createKey(state); + state_map_[key] = state; + } + + // Pick valid data + auto output = std::make_shared(); + output->stamp = clock_->now(); + for (const auto & [key, state] : state_map_) { + // Calculate time diff + const auto delay = (clock_->now() - state.stamp).seconds(); + + // Ignore future data considering clock's error + if (delay < -max_clock_error_sec_) { + RCLCPP_DEBUG( + logger_, "future state: delay=%f, max_clock_error=%f", delay, max_clock_error_sec_); + continue; + } + + // Ignore old data + if (delay > max_delay_sec_) { + RCLCPP_DEBUG(logger_, "old state: delay=%f, max_delay_sec=%f", delay, max_delay_sec_); + continue; + } + + output->states.push_back(state); + } + + return output; +} + +} // namespace autoware_api diff --git a/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp new file mode 100644 index 0000000..0de8626 --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -0,0 +1,226 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" + +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" + +#include +#include + +namespace autoware_api +{ +AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher(rclcpp::Node & node) +: logger_(node.get_logger().get_child("awapi_awiv_vehicle_state_publisher")), + clock_(node.get_clock()), + prev_accel_(0.0) +{ + // publisher + pub_state_ = + node.create_publisher("output/vehicle_status", 1); +} + +void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + tier4_api_msgs::msg::AwapiVehicleStatus status = initVehicleStatus(); + + // input header + status.header.frame_id = "base_link"; + status.header.stamp = clock_->now(); + + // get all info + getPoseInfo(aw_info.current_pose_ptr, &status); + getSteerInfo(aw_info.steer_ptr, &status); + getVehicleCmdInfo(aw_info.vehicle_cmd_ptr, &status); + getTurnSignalInfo(aw_info.turn_indicators_ptr, aw_info.hazard_lights_ptr, &status); + getTwistInfo(aw_info.odometry_ptr, &status); + getGearInfo(aw_info.gear_ptr, &status); + getBatteryInfo(aw_info.battery_ptr, &status); + getGpsInfo(aw_info.nav_sat_ptr, &status); + + // publish info + pub_state_->publish(status); +} + +tier4_api_msgs::msg::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() +{ + tier4_api_msgs::msg::AwapiVehicleStatus status; + // set default value + if (std::numeric_limits::has_quiet_NaN) { + status.energy_level = std::numeric_limits::quiet_NaN(); + } + return status; +} + +void AutowareIvVehicleStatePublisher::getPoseInfo( + const std::shared_ptr & pose_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!pose_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "current pose is nullptr"); + return; + } + + // get pose + status->pose = pose_ptr->pose; + + // convert quaternion to euler + double yaw, pitch, roll; + tf2::getEulerYPR(pose_ptr->pose.orientation, yaw, pitch, roll); + status->eulerangle.yaw = yaw; + status->eulerangle.pitch = pitch; + status->eulerangle.roll = roll; +} + +void AutowareIvVehicleStatePublisher::getSteerInfo( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!steer_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "steer is nullptr"); + return; + } + + // get steer + using tier4_auto_msgs_converter::convert; + status->steering = convert(*steer_ptr).data; + + // get steer vel + if (previous_steer_ptr_) { + // calculate steer vel from steer + const double ds = steer_ptr->steering_tire_angle - previous_steer_ptr_->steering_tire_angle; + const double dt = std::max( + (rclcpp::Time(steer_ptr->stamp) - rclcpp::Time(previous_steer_ptr_->stamp)).seconds(), 1e-03); + const double steer_vel = ds / dt; + + // apply lowpass filter + const double lowpass_steer = + lowpass_filter(steer_vel, prev_steer_vel_, steer_vel_lowpass_gain_); + prev_steer_vel_ = lowpass_steer; + status->steering_velocity = lowpass_steer; + } + previous_steer_ptr_ = steer_ptr; +} +void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & vehicle_cmd_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!vehicle_cmd_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "vehicle cmd is nullptr"); + return; + } + + // get command + status->target_acceleration = vehicle_cmd_ptr->longitudinal.acceleration; + status->target_velocity = vehicle_cmd_ptr->longitudinal.speed; + status->target_steering = vehicle_cmd_ptr->lateral.steering_tire_angle; + status->target_steering_velocity = vehicle_cmd_ptr->lateral.steering_tire_rotation_rate; +} + +void AutowareIvVehicleStatePublisher::getTurnSignalInfo( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & turn_indicators_ptr, + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & hazard_lights_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!turn_indicators_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "turn indicators is nullptr"); + return; + } + + if (!hazard_lights_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "hazard lights is nullptr"); + return; + } + + // get turn signal + using tier4_auto_msgs_converter::convert; + status->turn_signal = convert(*turn_indicators_ptr, *hazard_lights_ptr).data; +} + +void AutowareIvVehicleStatePublisher::getTwistInfo( + const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!odometry_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "odometry is nullptr"); + return; + } + + // get twist + status->velocity = odometry_ptr->twist.twist.linear.x; + status->angular_velocity = odometry_ptr->twist.twist.angular.z; + + // get accel + if (previous_odometry_ptr_) { + // calculate acceleration from velocity + const double dv = + odometry_ptr->twist.twist.linear.x - previous_odometry_ptr_->twist.twist.linear.x; + const double dt = std::max( + (rclcpp::Time(odometry_ptr->header.stamp) - + rclcpp::Time(previous_odometry_ptr_->header.stamp)) + .seconds(), + 1e-03); + const double accel = dv / dt; + + // apply lowpass filter + const double lowpass_accel = lowpass_filter(accel, prev_accel_, accel_lowpass_gain_); + prev_accel_ = lowpass_accel; + status->acceleration = lowpass_accel; + } + previous_odometry_ptr_ = odometry_ptr; +} + +void AutowareIvVehicleStatePublisher::getGearInfo( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!gear_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gear is nullptr"); + return; + } + + // get gear (shift) + using tier4_auto_msgs_converter::convert; + status->gear = convert(*gear_ptr).shift.data; +} + +void AutowareIvVehicleStatePublisher::getBatteryInfo( + const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!battery_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "battery is nullptr"); + return; + } + + // get battery + status->energy_level = battery_ptr->energy_level; +} + +void AutowareIvVehicleStatePublisher::getGpsInfo( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, + tier4_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!nav_sat_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "nav_sat(gps) is nullptr"); + return; + } + + // get geo_point + status->geo_point.latitude = nav_sat_ptr->latitude; + status->geo_point.longitude = nav_sat_ptr->longitude; + status->geo_point.altitude = nav_sat_ptr->altitude; +} + +} // namespace autoware_api