diff --git a/launch/tier4_autoware_api_launch/README.md b/launch/tier4_autoware_api_launch/README.md index e2ea11b7de4c1..fe409160f8e51 100644 --- a/launch/tier4_autoware_api_launch/README.md +++ b/launch/tier4_autoware_api_launch/README.md @@ -10,13 +10,10 @@ Please see `` in `package.xml`. ## Usage -You can include as follows in `*.launch.xml` to use `control.launch.py`. +You can include as follows in `*.launch.xml` to use `autoware_api.launch.xml`. ```xml - - - - + ``` ## Notes diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py index 4b8513f544588..422c35abee354 100644 --- a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py @@ -13,7 +13,6 @@ # limitations under the License. import launch -from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -29,12 +28,7 @@ def _create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): - param_initial_pose = { - "init_simulator_pose": LaunchConfiguration("init_simulator_pose"), - "init_localization_pose": LaunchConfiguration("init_localization_pose"), - } components = [ - _create_api_node("initial_pose", "InitialPose", parameters=[param_initial_pose]), _create_api_node("iv_msgs", "IVMsgs"), _create_api_node("operator", "Operator"), _create_api_node("route", "Route"), diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml index 4723621f84d68..27c62523ab6bd 100644 --- a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml @@ -16,8 +16,5 @@ name="crosswalk_states" args="/api/autoware/set/crosswalk_states /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_crosswalk_states" /> - - - diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index bf083a97bb1bf..71525748d5f2d 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -13,6 +13,7 @@ + diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index e3f60904c24fa..45cdbf1464129 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,11 +6,13 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/interface.cpp + src/localization.cpp src/routing.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode" + "default_ad_api::LocalizationNode" "default_ad_api::RoutingNode" ) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 7e11805870c5b..d93f779427277 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -31,6 +31,7 @@ def _create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): components = [ _create_api_node("interface", "InterfaceNode"), + _create_api_node("localization", "LocalizationNode"), _create_api_node("routing", "RoutingNode"), ] container = ComposableNodeContainer( diff --git a/system/default_ad_api/src/localization.cpp b/system/default_ad_api/src/localization.cpp new file mode 100644 index 0000000000000..468e02b37644f --- /dev/null +++ b/system/default_ad_api/src/localization.cpp @@ -0,0 +1,32 @@ +// Copyright 2022 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 "localization.hpp" + +namespace default_ad_api +{ + +LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options) +: Node("localization", options) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.relay_message(pub_state_, sub_state_); + adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::LocalizationNode) diff --git a/system/default_ad_api/src/localization.hpp b/system/default_ad_api/src/localization.hpp new file mode 100644 index 0000000000000..077c60d86df01 --- /dev/null +++ b/system/default_ad_api/src/localization.hpp @@ -0,0 +1,44 @@ +// Copyright 2022 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 LOCALIZATION_HPP_ +#define LOCALIZATION_HPP_ + +#include +#include +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class LocalizationNode : public rclcpp::Node +{ +public: + explicit LocalizationNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + Srv srv_initialize_; + Pub pub_state_; + Cli cli_initialize_; + Sub sub_state_; +}; + +} // namespace default_ad_api + +#endif // LOCALIZATION_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt index 65028d9ba5cc5..35cb599995b31 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt +++ b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt @@ -4,8 +4,12 @@ project(ad_api_adaptors) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_executable(initial_pose_adaptor + src/initial_pose_adaptor.cpp +) + ament_auto_add_executable(routing_adaptor src/routing_adaptor.cpp ) -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index bb25903043bac..95cd40a2ec33b 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -1,5 +1,17 @@ # ad_api_adaptors +## initial_pose_adaptor + +This node makes it easy to use the localization AD API from RViz. +When a initial pose topic is received, call the localization initialize API. +This node depends on fitting to map height service. + +| Interface | Local Name | Global Name | Description | +| ------------ | -------------- | --------------------------------- | ----------------------------------------- | +| Subscription | initialpose | /initialpose | The pose for localization initialization. | +| Client | fit_map_height | /localization/util/fit_map_height | To fix initial pose to map height | +| Client | - | /api/localization/initialize | The localization initialize API. | + ## routing_adaptor This node makes it easy to use the routing AD API from RViz. diff --git a/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml b/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml new file mode 100644 index 0000000000000..46748a022f558 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + + # from initialpose (Rviz's 2DPoseEstimate) + initial_pose_particle_covariance: + [ + 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 4.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, + ] diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml index 976d0868fe20e..b3301a3c30f8e 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml @@ -1,6 +1,14 @@ - - - - + + + + + + + + + + + + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index b98d230a4c3cf..442a076a9c544 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -15,6 +15,7 @@ autoware_ad_api_specs component_interface_utils rclcpp + tier4_localization_msgs ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp new file mode 100644 index 0000000000000..137243baaf0d2 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp @@ -0,0 +1,72 @@ +// Copyright 2022 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_adaptor.hpp" + +#include +#include +#include + +namespace ad_api_adaptors +{ +template +using Future = typename rclcpp::Client::SharedFuture; + +std::array get_covariance_parameter(rclcpp::Node * node, const std::string & name) +{ + const auto vector = node->declare_parameter>(name); + if (vector.size() != 36) { + throw std::invalid_argument("The covariance parameter size is not 36."); + } + std::array array; + std::copy_n(vector.begin(), array.size(), array.begin()); + return array; +} + +InitialPoseAdaptor::InitialPoseAdaptor() : Node("initial_pose_adaptor") +{ + rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance"); + cli_map_fit_ = create_client("~/fit_map_height"); + sub_initial_pose_ = create_subscription( + "~/initialpose", rclcpp::QoS(1), + std::bind(&InitialPoseAdaptor::on_initial_pose, this, std::placeholders::_1)); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_cli(cli_initialize_); +} + +void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->pose_with_covariance = *msg; + cli_map_fit_->async_send_request(req, [this](Future future) { + const auto req = std::make_shared(); + req->pose.push_back(future.get()->pose_with_covariance); + req->pose.back().pose.covariance = rviz_particle_covariance_; + cli_initialize_->async_send_request(req); + }); +} + +} // namespace ad_api_adaptors + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp new file mode 100644 index 0000000000000..c23968acc4ed9 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 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_ADAPTOR_HPP_ +#define INITIAL_POSE_ADAPTOR_HPP_ + +#include +#include +#include + +#include +#include + +namespace ad_api_adaptors +{ + +class InitialPoseAdaptor : public rclcpp::Node +{ +public: + InitialPoseAdaptor(); + +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using Initialize = autoware_ad_api::localization::Initialize; + using RequestHeightFitting = tier4_localization_msgs::srv::PoseWithCovarianceStamped; + rclcpp::Subscription::SharedPtr sub_initial_pose_; + rclcpp::Client::SharedPtr cli_map_fit_; + component_interface_utils::Client::SharedPtr cli_initialize_; + std::array rviz_particle_covariance_; + + void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg); +}; + +} // namespace ad_api_adaptors + +#endif // INITIAL_POSE_ADAPTOR_HPP_ diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt b/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt new file mode 100644 index 0000000000000..ad65f04356f4d --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(automatic_pose_initializer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(automatic_pose_initializer + src/automatic_pose_initializer.cpp +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/README.md b/system/default_ad_api_helpers/automatic_pose_initializer/README.md new file mode 100644 index 0000000000000..4fda3706356d3 --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/README.md @@ -0,0 +1,11 @@ +# automatic_pose_initializer + +## automatic_pose_initializer + +This node calls localization initialize API when the localization initialization state is uninitialized. +Since the API uses GNSS pose when no pose is specified, initialization using GNSS can be performed automatically. + +| Interface | Local Name | Global Name | Description | +| ------------ | ---------- | -------------------------------------- | ------------------------------------------ | +| Subscription | - | /api/localization/initialization_state | The localization initialization state API. | +| Client | - | /api/localization/initialize | The localization initialize API. | diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml new file mode 100644 index 0000000000000..ac5e63e84092a --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml new file mode 100644 index 0000000000000..e6a351b392680 --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -0,0 +1,25 @@ + + + + automatic_pose_initializer + 0.1.0 + The automatic_pose_initializer package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_ad_api_msgs + autoware_ad_api_specs + component_interface_utils + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp new file mode 100644 index 0000000000000..302dfecdd4710 --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp @@ -0,0 +1,58 @@ +// Copyright 2022 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 "automatic_pose_initializer.hpp" + +#include + +namespace automatic_pose_initializer +{ + +AutomaticPoseInitializer::AutomaticPoseInitializer() : Node("automatic_pose_initializer") +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_initialize_, group_cli_); + adaptor.init_sub(sub_state_, [this](const State::Message::ConstSharedPtr msg) { state_ = *msg; }); + + const auto period = rclcpp::Rate(1.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); }); + + state_.stamp = now(); + state_.state = State::Message::UNKNOWN; +} + +void AutomaticPoseInitializer::on_timer() +{ + if (state_.state == State::Message::UNINITIALIZED) { + try { + const auto req = std::make_shared(); + cli_initialize_->call(req); + } catch (const component_interface_utils::ServiceException & error) { + } + } +} + +} // namespace automatic_pose_initializer + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp new file mode 100644 index 0000000000000..80fe78832026d --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp @@ -0,0 +1,43 @@ +// Copyright 2022 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 AUTOMATIC_POSE_INITIALIZER_HPP_ +#define AUTOMATIC_POSE_INITIALIZER_HPP_ + +#include +#include +#include + +namespace automatic_pose_initializer +{ + +class AutomaticPoseInitializer : public rclcpp::Node +{ +public: + AutomaticPoseInitializer(); + +private: + void on_timer(); + using Initialize = autoware_ad_api::localization::Initialize; + using State = autoware_ad_api::localization::InitializationState; + rclcpp::CallbackGroup::SharedPtr group_cli_; + rclcpp::TimerBase::SharedPtr timer_; + component_interface_utils::Client::SharedPtr cli_initialize_; + component_interface_utils::Subscription::SharedPtr sub_state_; + State::Message state_; +}; + +} // namespace automatic_pose_initializer + +#endif // AUTOMATIC_POSE_INITIALIZER_HPP_