diff --git a/localization/initial_pose_button_panel/CMakeLists.txt b/localization/initial_pose_button_panel/CMakeLists.txt new file mode 100644 index 0000000000000..6549261a0025d --- /dev/null +++ b/localization/initial_pose_button_panel/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) +project(initial_pose_button_panel) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +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() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +add_definitions(-DQT_NO_KEYWORDS -g) +set(CMAKE_AUTOMOC ON) + +ament_auto_add_library(initial_pose_button_panel SHARED + src/initial_pose_button_panel.cpp) +target_link_libraries(initial_pose_button_panel + ${QT_LIBRARIES}) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/localization/initial_pose_button_panel/README.md b/localization/initial_pose_button_panel/README.md new file mode 100644 index 0000000000000..cdb4824ada0e2 --- /dev/null +++ b/localization/initial_pose_button_panel/README.md @@ -0,0 +1,18 @@ +# initial_pose_button_panel + +## Role + +`initial_pose_button_panel` is the package to send a request to the localization module to calculate the current ego pose. + +It starts calculating the current ego pose by pushing the button on Rviz, implemented as an Rviz plugin. +You can see the button on the right bottom of Rviz. + +![initialize_button](./media/initialize_button.png) + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ---------------------------------------------- | --------------------------------------------- | -------------------------------------------------------------- | +| `/sensing/gnss/pose_with_covariance` (default) | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose with covariance to calculate the current ego pose | diff --git a/localization/initial_pose_button_panel/media/initialize_button.png b/localization/initial_pose_button_panel/media/initialize_button.png new file mode 100644 index 0000000000000..f7bfe8aa652ac Binary files /dev/null and b/localization/initial_pose_button_panel/media/initialize_button.png differ diff --git a/localization/initial_pose_button_panel/package.xml b/localization/initial_pose_button_panel/package.xml new file mode 100644 index 0000000000000..b65621bb16180 --- /dev/null +++ b/localization/initial_pose_button_panel/package.xml @@ -0,0 +1,27 @@ + + + initial_pose_button_panel + 0.1.0 + The initial_pose_button_panel package + + Yamato ANDO + Yamato ANDO + + Apache License 2.0 + + ament_cmake_auto + autoware_localization_msgs + geometry_msgs + libqt5-core + libqt5-widgets + qtbase5-dev + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/localization/initial_pose_button_panel/plugins/plugin_description.xml b/localization/initial_pose_button_panel/plugins/plugin_description.xml new file mode 100644 index 0000000000000..f872e68d4f50b --- /dev/null +++ b/localization/initial_pose_button_panel/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + initial button. + + + diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp new file mode 100644 index 0000000000000..1b09d3f965145 --- /dev/null +++ b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp @@ -0,0 +1,127 @@ +// 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 "initial_pose_button_panel.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware_localization_rviz_plugin +{ +InitialPoseButtonPanel::InitialPoseButtonPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + topic_label_ = new QLabel("PoseWithCovarianceStamped "); + topic_label_->setAlignment(Qt::AlignCenter); + + topic_edit_ = new QLineEdit("/sensing/gnss/pose_with_covariance"); + connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); + + initialize_button_ = new QPushButton("Wait for subscribe topic"); + initialize_button_->setEnabled(false); + connect(initialize_button_, SIGNAL(clicked(bool)), SLOT(pushInitializeButton())); + + status_label_ = new QLabel("Not Initialize"); + status_label_->setAlignment(Qt::AlignCenter); + status_label_->setStyleSheet("QLabel { background-color : gray;}"); + + QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding); + initialize_button_->setSizePolicy(q_size_policy); + + auto * topic_layout = new QHBoxLayout; + topic_layout->addWidget(topic_label_); + topic_layout->addWidget(topic_edit_); + + auto * v_layout = new QVBoxLayout; + v_layout->addLayout(topic_layout); + v_layout->addWidget(initialize_button_); + v_layout->addWidget(status_label_); + + setLayout(v_layout); +} +void InitialPoseButtonPanel::onInitialize() +{ + rclcpp::Node::SharedPtr raw_node = + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + pose_cov_sub_ = raw_node->create_subscription( + topic_edit_->text().toStdString(), 10, + std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); + + client_ = raw_node->create_client( + "/localization/util/initialize_pose"); +} + +void InitialPoseButtonPanel::callbackPoseCov( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + pose_cov_msg_ = *msg; + initialize_button_->setText("Pose Initializer Let's GO!"); + initialize_button_->setEnabled(true); +} + +void InitialPoseButtonPanel::editTopic() +{ + pose_cov_sub_.reset(); + rclcpp::Node::SharedPtr raw_node = + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + pose_cov_sub_ = raw_node->create_subscription( + topic_edit_->text().toStdString(), 10, + std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); + initialize_button_->setText("Wait for subscribe topic"); + initialize_button_->setEnabled(false); +} + +void InitialPoseButtonPanel::pushInitializeButton() +{ + // lock button + initialize_button_->setEnabled(false); + + status_label_->setStyleSheet("QLabel { background-color : dodgerblue;}"); + status_label_->setText("Initializing..."); + + std::thread thread([this] { + auto req = + std::make_shared(); + req->pose_with_covariance = pose_cov_msg_; + + client_->async_send_request( + req, + [this]( + rclcpp::Client::SharedFuture + result) { + status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); + status_label_->setText("OK!!!"); + + // unlock button + initialize_button_->setEnabled(true); + }); + }); + + thread.detach(); +} + +} // end namespace autoware_localization_rviz_plugin + +PLUGINLIB_EXPORT_CLASS( + autoware_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel) diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp new file mode 100644 index 0000000000000..2583e7d096dbb --- /dev/null +++ b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp @@ -0,0 +1,65 @@ +// 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. + +#pragma once + +#ifndef INITIAL_POSE_BUTTON_PANEL_HPP_ +#define INITIAL_POSE_BUTTON_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#ifndef Q_MOC_RUN + +#include +#include +#include +#endif +#include +#include + +namespace autoware_localization_rviz_plugin +{ +class InitialPoseButtonPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit InitialPoseButtonPanel(QWidget * parent = nullptr); + void onInitialize() override; + void callbackPoseCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + +public Q_SLOTS: + void editTopic(); + void pushInitializeButton(); + +protected: + rclcpp::Subscription::SharedPtr pose_cov_sub_; + + rclcpp::Client::SharedPtr client_; + + QLabel * topic_label_; + QLineEdit * topic_edit_; + QPushButton * initialize_button_; + QLabel * status_label_; + + geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_msg_; +}; + +} // end namespace autoware_localization_rviz_plugin + +#endif // INITIAL_POSE_BUTTON_PANEL_HPP_ diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt new file mode 100644 index 0000000000000..adc48cde3b681 --- /dev/null +++ b/localization/pose_initializer/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(pose_initializer) + +### Compile options +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() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(PCL REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(pose_initializer + src/pose_initializer_node.cpp + src/pose_initializer_core.cpp +) +target_link_libraries(pose_initializer ${PCL_LIBRARIES}) + +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/localization/pose_initializer/README.md b/localization/pose_initializer/README.md new file mode 100644 index 0000000000000..8fceb433efa58 --- /dev/null +++ b/localization/pose_initializer/README.md @@ -0,0 +1,24 @@ +# pose_initializer + +## Purpose + +`pose_initializer` is the package to send an initial pose to `ekf_localizer`. +It receives roughly estimated initial pose from GNSS/user. +Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service. +Finally, it publishes the initial pose to `ekf_localizer`. + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ------------------------------------ | --------------------------------------------- | ---------------------- | +| `/initialpose` | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose from rviz | +| `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss | +| `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map | + +### Output topics + +| Name | Type | Description | +| ---------------- | --------------------------------------------- | --------------------------- | +| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | diff --git a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp new file mode 100644 index 0000000000000..4771be3a01164 --- /dev/null +++ b/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp @@ -0,0 +1,96 @@ +// Copyright 2020 Autoware Foundation +// +// 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 POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ +#define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +class PoseInitializer : public rclcpp::Node +{ +public: + PoseInitializer(); + ~PoseInitializer(); + +private: + void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void serviceInitializePose( + const std::shared_ptr req, + std::shared_ptr res); + void serviceInitializePoseAuto( + const std::shared_ptr req, + std::shared_ptr res); + void callbackInitialPose( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr); + void callbackGNSSPoseCov( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr); + void callbackPoseInitializationRequest( + const autoware_localization_msgs::msg::PoseInitializationRequest::ConstSharedPtr + request_msg_ptr); // NOLINT + + bool getHeight( + const geometry_msgs::msg::PoseWithCovarianceStamped & input_pose_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr output_pose_msg_ptr); + bool callAlignServiceAndPublishResult( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + rclcpp::Subscription::SharedPtr gnss_pose_sub_; + rclcpp::Subscription::SharedPtr map_points_sub_; + + // TODO(Takagi, Isamu): deprecated + rclcpp::Subscription::SharedPtr initial_pose_sub_; + rclcpp::Subscription::SharedPtr + pose_initialization_request_sub_; + + rclcpp::Publisher::SharedPtr initial_pose_pub_; + + rclcpp::Client::SharedPtr ndt_client_; + + rclcpp::Service::SharedPtr + initialize_pose_service_; + rclcpp::Service::SharedPtr + initialize_pose_auto_service_; + + tf2::BufferCore tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + + pcl::PointCloud::Ptr map_ptr_; + std::string map_frame_; + + // With the currently available facilities for calling a service, there is no + // easy way of detecting whether an answer was received within a reasonable + // amount of time. So, as a sanity check, we check whether a response for the + // previous request was received when a new request is sent. + uint32_t request_id_ = 0; + uint32_t response_id_ = 0; + + bool enable_gnss_callback_; +}; + +#endif // POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml new file mode 100644 index 0000000000000..55f4a622fffa8 --- /dev/null +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml new file mode 100644 index 0000000000000..04eb6549c3777 --- /dev/null +++ b/localization/pose_initializer/package.xml @@ -0,0 +1,31 @@ + + + + pose_initializer + 0.1.0 + The pose_initializer package + Yamato Ando + Apache License 2.0 + ament_cmake + + autoware_api_utils + autoware_external_api_msgs + autoware_localization_msgs + geometry_msgs + libpcl-all-dev + pcl_conversions + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + + ament_cmake_cppcheck + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/pose_initializer/src/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer_core.cpp new file mode 100644 index 0000000000000..c0b7039c32404 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer_core.cpp @@ -0,0 +1,244 @@ +// Copyright 2020 Autoware Foundation +// +// 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 "pose_initializer/pose_initializer_core.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +double getGroundHeight(const pcl::PointCloud::Ptr pcdmap, const tf2::Vector3 & point) +{ + constexpr double radius = 1.0 * 1.0; + const double x = point.getX(); + const double y = point.getY(); + + double height = INFINITY; + for (const auto & p : pcdmap->points) { + const double dx = x - p.x; + const double dy = y - p.y; + const double sd = (dx * dx) + (dy * dy); + if (sd < radius) { + height = std::min(height, static_cast(p.z)); + } + } + return std::isfinite(height) ? height : point.getZ(); +} + +PoseInitializer::PoseInitializer() +: Node("pose_initializer"), tf2_listener_(tf2_buffer_), map_frame_("map") +{ + enable_gnss_callback_ = this->declare_parameter("enable_gnss_callback", true); + + // We can't use _1 because pcl leaks an alias to boost::placeholders::_1, so it would be ambiguous + initial_pose_sub_ = this->create_subscription( + "initialpose", 10, + std::bind(&PoseInitializer::callbackInitialPose, this, std::placeholders::_1)); + map_points_sub_ = this->create_subscription( + "pointcloud_map", rclcpp::QoS{1}.transient_local(), + std::bind(&PoseInitializer::callbackMapPoints, this, std::placeholders::_1)); + gnss_pose_sub_ = this->create_subscription( + "gnss_pose_cov", 1, + std::bind(&PoseInitializer::callbackGNSSPoseCov, this, std::placeholders::_1)); + pose_initialization_request_sub_ = + this->create_subscription( + "pose_initialization_request", rclcpp::QoS{1}.transient_local(), + std::bind(&PoseInitializer::callbackPoseInitializationRequest, this, std::placeholders::_1)); + + initial_pose_pub_ = + this->create_publisher("initialpose3d", 10); + + ndt_client_ = this->create_client( + "ndt_align_srv"); + while (!ndt_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO(get_logger(), "Waiting for service..."); + } + + initialize_pose_service_ = + this->create_service( + "service/initialize_pose", std::bind( + &PoseInitializer::serviceInitializePose, this, + std::placeholders::_1, std::placeholders::_2)); + + initialize_pose_auto_service_ = + this->create_service( + "service/initialize_pose_auto", std::bind( + &PoseInitializer::serviceInitializePoseAuto, this, + std::placeholders::_1, std::placeholders::_2)); +} + +PoseInitializer::~PoseInitializer() {} + +void PoseInitializer::callbackMapPoints( + sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) +{ + std::string map_frame_ = map_points_msg_ptr->header.frame_id; + map_ptr_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + pcl::fromROSMsg(*map_points_msg_ptr, *map_ptr_); +} + +void PoseInitializer::serviceInitializePose( + const std::shared_ptr req, + std::shared_ptr res) +{ + enable_gnss_callback_ = false; // get only first topic + + auto add_height_pose_msg_ptr = std::make_shared(); + getHeight(req->pose_with_covariance, add_height_pose_msg_ptr); + + // TODO(YamatoAndo) + add_height_pose_msg_ptr->pose.covariance[0] = 1.0; + add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 1.0; + add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01; + add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01; + add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01; + add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 1.0; + + res->success = callAlignServiceAndPublishResult(add_height_pose_msg_ptr); +} + +void PoseInitializer::callbackInitialPose( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr) +{ + enable_gnss_callback_ = false; // get only first topic + + auto add_height_pose_msg_ptr = std::make_shared(); + getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr); + + // TODO(YamatoAndo) + add_height_pose_msg_ptr->pose.covariance[0] = 2.0; + add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 2.0; + add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01; + add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01; + add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01; + add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 0.3; + + callAlignServiceAndPublishResult(add_height_pose_msg_ptr); +} + +// NOTE Still not usable callback +void PoseInitializer::callbackGNSSPoseCov( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr) +{ + if (!enable_gnss_callback_) { + return; + } + + // TODO(YamatoAndo) check service is available + + auto add_height_pose_msg_ptr = std::make_shared(); + getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr); + + // TODO(YamatoAndo) + add_height_pose_msg_ptr->pose.covariance[0] = 1.0; + add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 1.0; + add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01; + add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01; + add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01; + add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 3.14; + + callAlignServiceAndPublishResult(add_height_pose_msg_ptr); +} + +void PoseInitializer::serviceInitializePoseAuto( + const std::shared_ptr req, + std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Called Pose Initialize Service"); + enable_gnss_callback_ = true; + res->status = autoware_api_utils::response_success(); +} + +void PoseInitializer::callbackPoseInitializationRequest( + const autoware_localization_msgs::msg::PoseInitializationRequest::ConstSharedPtr request_msg_ptr) +{ + RCLCPP_INFO(this->get_logger(), "Called Pose Initialize"); + enable_gnss_callback_ = request_msg_ptr->data; +} + +bool PoseInitializer::getHeight( + const geometry_msgs::msg::PoseWithCovarianceStamped & input_pose_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr output_pose_msg_ptr) +{ + std::string fixed_frame = input_pose_msg.header.frame_id; + tf2::Vector3 point( + input_pose_msg.pose.pose.position.x, input_pose_msg.pose.pose.position.y, + input_pose_msg.pose.pose.position.z); + + if (map_ptr_) { + tf2::Transform transform; + try { + const auto stamped = tf2_buffer_.lookupTransform(map_frame_, fixed_frame, tf2::TimePointZero); + tf2::fromMsg(stamped.transform, transform); + } catch (tf2::TransformException & exception) { + RCLCPP_WARN_STREAM(get_logger(), "failed to lookup transform: " << exception.what()); + } + + point = transform * point; + point.setZ(getGroundHeight(map_ptr_, point)); + point = transform.inverse() * point; + } + + *output_pose_msg_ptr = input_pose_msg; + output_pose_msg_ptr->pose.pose.position.x = point.getX(); + output_pose_msg_ptr->pose.pose.position.y = point.getY(); + output_pose_msg_ptr->pose.pose.position.z = point.getZ(); + + return true; +} + +bool PoseInitializer::callAlignServiceAndPublishResult( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr input_pose_msg) +{ + if (request_id_ != response_id_) { + RCLCPP_ERROR(get_logger(), "Did not receive response for previous NDT Align Server call"); + return false; + } + auto req = + std::make_shared(); + req->pose_with_covariance = *input_pose_msg; + req->seq = ++request_id_; + + RCLCPP_INFO(get_logger(), "call NDT Align Server"); + + ndt_client_->async_send_request( + req, + [this](rclcpp::Client::SharedFuture + result) { + if (result.get()->success) { + RCLCPP_INFO(get_logger(), "called NDT Align Server"); + response_id_ = result.get()->seq; + // NOTE temporary cov + geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_covariance = + result.get()->pose_with_covariance; + pose_with_covariance.pose.covariance[0] = 1.0; + pose_with_covariance.pose.covariance[1 * 6 + 1] = 1.0; + pose_with_covariance.pose.covariance[2 * 6 + 2] = 0.01; + pose_with_covariance.pose.covariance[3 * 6 + 3] = 0.01; + pose_with_covariance.pose.covariance[4 * 6 + 4] = 0.01; + pose_with_covariance.pose.covariance[5 * 6 + 5] = 0.2; + initial_pose_pub_->publish(pose_with_covariance); + enable_gnss_callback_ = false; + } else { + RCLCPP_INFO(get_logger(), "failed NDT Align Server"); + response_id_ = result.get()->seq; + } + }); + return true; +} diff --git a/localization/pose_initializer/src/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer_node.cpp new file mode 100644 index 0000000000000..b01297bbd3acd --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer_node.cpp @@ -0,0 +1,27 @@ +// Copyright 2020 Autoware Foundation +// +// 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 "pose_initializer/pose_initializer_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}