feat: add path distance calculator package (autowarefoundation#37)

* Add path distance calculator (autowarefoundation#1520) * Add path distance calculator Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Use autoware_utils trajectory function Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add const Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * port path distance calculator (autowarefoundation#498) * port path distance calculator * remove ignore Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> Co-authored-by: tanaka3 <ttatcoder@outlook.jp> * [path_distance_calculator]add readme (autowarefoundation#547) * add readme of path_distance_calculator * update readme * fix readme * update description * ci(pre-commit): autofix Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
badai-nguyen · Dec 2, 2021 · a07ef97 · a07ef97
1 parent 57c3590
commit a07ef97
Show file tree

Hide file tree

Showing 6 changed files with 203 additions and 0 deletions.
diff --git a/common/path_distance_calculator/CMakeLists.txt b/common/path_distance_calculator/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.5)
+project(path_distance_calculator)
+
+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()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/path_distance_calculator.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "PathDistanceCalculator"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+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/common/path_distance_calculator/Readme.md b/common/path_distance_calculator/Readme.md
@@ -0,0 +1,37 @@
+# path_distance_calculator
+
+## Purpose
+
+This node publishes a distance from the closest path point from the self-position to the end point of the path.
+Note that the distance means the arc-length along the path, not the Euclidean distance between the two points.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                                              | Type                                     | Description    |
+| ----------------------------------------------------------------- | ---------------------------------------- | -------------- |
+| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_auto_planning_msgs::msg::Path` | Reference path |
+| `/tf`                                                             | `tf2_msgs/TFMessage`                     | TF (self-pose) |
+
+### Output
+
+| Name         | Type                                       | Description                                                                                           |
+| ------------ | ------------------------------------------ | ----------------------------------------------------------------------------------------------------- |
+| `~/distance` | `autoware_debug_msgs::msg::Float64Stamped` | Publish a distance from the closest path point from the self-position to the end point of the path[m] |
+
+## Parameters
+
+### Node Parameters
+
+None.
+
+### Core Parameters
+
+None.
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/common/path_distance_calculator/launch/path_distance_calculator.launch.xml b/common/path_distance_calculator/launch/path_distance_calculator.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <arg name="path" default="/planning/scenario_planning/lane_driving/behavior_planning/path" />
+  <arg name="distance" default="~/distance" />
+  <node pkg="path_distance_calculator" exec="path_distance_calculator_node" name="path_distance_calculator">
+    <remap from="~/input/path" to="$(var path)" />
+    <remap from="~/output/distance" to="$(var distance)" />
+  </node>
+</launch>
diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+
+  <name>path_distance_calculator</name>
+  <version>0.0.0</version>
+  <description>The path_distance_calculator package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_debug_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>
diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp
@@ -0,0 +1,66 @@
+// 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 "path_distance_calculator.hpp"
+
+#include <autoware_utils/autoware_utils.hpp>
+
+#include <algorithm>
+#include <chrono>
+#include <memory>
+#include <utility>
+#include <vector>
+
+PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options)
+: Node("path_distance_calculator", options), self_pose_listener_(this)
+{
+  sub_path_ = create_subscription<autoware_auto_planning_msgs::msg::Path>(
+    "~/input/path", rclcpp::QoS(1),
+    [this](const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; });
+  pub_dist_ =
+    create_publisher<autoware_debug_msgs::msg::Float64Stamped>("~/output/distance", rclcpp::QoS(1));
+
+  auto timer_callback = [this]() {
+    const auto path = path_;
+    const auto pose = self_pose_listener_.getCurrentPose();
+    if (!pose) {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "no pose");
+      return;
+    }
+    if (!path) {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "no path");
+      return;
+    }
+    if (path->points.empty()) {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "path empty");
+    }
+
+    const double distance = autoware_utils::calcSignedArcLength(
+      path->points, pose->pose.position, path->points.size() - 1);
+
+    autoware_debug_msgs::msg::Float64Stamped msg;
+    msg.stamp = pose->header.stamp;
+    msg.data = distance;
+    pub_dist_->publish(msg);
+  };
+
+  using namespace std::literals::chrono_literals;
+  timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
+    this->get_clock(), 1s, std::move(timer_callback),
+    this->get_node_base_interface()->get_context());
+  this->get_node_timers_interface()->add_timer(timer_, nullptr);
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(PathDistanceCalculator)
diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp
@@ -0,0 +1,37 @@
+// 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 PATH_DISTANCE_CALCULATOR_HPP_
+#define PATH_DISTANCE_CALCULATOR_HPP_
+
+#include <autoware_utils/ros/self_pose_listener.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path.hpp>
+#include <autoware_debug_msgs/msg/float64_stamped.hpp>
+
+class PathDistanceCalculator : public rclcpp::Node
+{
+public:
+  explicit PathDistanceCalculator(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Path>::SharedPtr sub_path_;
+  rclcpp::Publisher<autoware_debug_msgs::msg::Float64Stamped>::SharedPtr pub_dist_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  autoware_utils::SelfPoseListener self_pose_listener_;
+  autoware_auto_planning_msgs::msg::Path::SharedPtr path_;
+};
+
+#endif  // PATH_DISTANCE_CALCULATOR_HPP_