feat: add goal distance calculator package (autowarefoundation#36)

* Feature/goal distance calculator (autowarefoundation#311) * add initial package * half lint * pass cpplint * add pose deviation * add autoware utils * fix prefix * beautify * add ament copyright * fix ament format * fix param name * refactor param * refactor params * [goal_sistance_calculator]: Fix CMakeLists.txt Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> * ros2-v0.8.0-goal_distance_calulator (autowarefoundation#319) * update to component * refactor node name * Ros2 v0.8.0 lane departure checker (autowarefoundation#327) * Add lane departure checker (autowarefoundation#928) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix bug of lane_departure_checker (autowarefoundation#1011) * Fix bug of lane_departure_checker Since preceeding lanelets are missing, when vehicle is at the beginning of a lanelet, it's mistakenly considered as out of lane. Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix typo Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Reduce computational cost of lane_departure_checker (autowarefoundation#1026) Lanelet visualization will be too slow when big lanelets are visualized. Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix typo (autowarefoundation#1062) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * [lane_departure_checker]: Port to ROS2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [lane_departure_checker]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [autoware_utils]: Publish autoware debug msgs Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [lane_departure_checker]: Use autoware_debug_msgs instead of std_msgs Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [goal_distance_calculator]: Use autoware_debug_msgs instead of std_msgs Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [autoware_utils]: Add namespace to debug traits Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [lane_departure_checker]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Rename ROS-related .yaml to .param.yaml (autowarefoundation#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix overlooked parameters Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * add use_sim-time option (autowarefoundation#454) * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * 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> * goal_distance_calculator port to .auto (autowarefoundation#525) * goal_distance_calculator port to .auto * Fix formatting * Fix pre commit * [goal_distance_calculator]add readme (autowarefoundation#545) * add readme * Update common/util/goal_distance_calculator/Readme.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix format Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * [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: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
YoheiMishina · Dec 2, 2021 · b725734 · b725734
1 parent 6dd12d6
commit b725734
Show file tree

Hide file tree

Showing 9 changed files with 414 additions and 0 deletions.
diff --git a/common/goal_distance_calculator/CMakeLists.txt b/common/goal_distance_calculator/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.5)
+project(goal_distance_calculator)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+ament_auto_add_library(goal_distance_calculator SHARED
+  src/goal_distance_calculator_node.cpp
+  src/goal_distance_calculator.cpp
+)
+
+rclcpp_components_register_node(goal_distance_calculator
+  PLUGIN "goal_distance_calculator::GoalDistanceCalculatorNode"
+  EXECUTABLE goal_distance_calculator_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)
diff --git a/common/goal_distance_calculator/Readme.md b/common/goal_distance_calculator/Readme.md
@@ -0,0 +1,43 @@
+# goal_distance_calculator
+
+## Purpose
+
+This node publishes deviation of self-pose from goal pose.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name                               | Type                                      | Description           |
+| ---------------------------------- | ----------------------------------------- | --------------------- |
+| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::Route` | Used to get goal pose |
+| `/tf`                              | `tf2_msgs/TFMessage`                      | TF (self-pose)        |
+
+### Output
+
+| Name                     | Type                                       | Description                                                   |
+| ------------------------ | ------------------------------------------ | ------------------------------------------------------------- |
+| `deviation/lateral`      | `autoware_debug_msgs::msg::Float64Stamped` | publish lateral deviation of self-pose from goal pose[m]      |
+| `deviation/longitudinal` | `autoware_debug_msgs::msg::Float64Stamped` | publish longitudinal deviation of self-pose from goal pose[m] |
+| `deviation/yaw`          | `autoware_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[rad]        |
+| `deviation/yaw_deg`      | `autoware_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[deg]        |
+
+## Parameters
+
+### Node Parameters
+
+| Name          | Type   | Default Value | Explanation                 |
+| ------------- | ------ | ------------- | --------------------------- |
+| `update_rate` | double | 10.0          | Timer callback period. [Hz] |
+
+### Core Parameters
+
+| Name      | Type | Default Value | Explanation                                |
+| --------- | ---- | ------------- | ------------------------------------------ |
+| `oneshot` | bool | true          | publish deviations just once or repeatedly |
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
diff --git a/...on/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/...on/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp
@@ -0,0 +1,57 @@
+// 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.
+
+#ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_
+#define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_
+
+#include <autoware_utils/geometry/pose_deviation.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/route.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <boost/optional.hpp>
+
+namespace goal_distance_calculator
+{
+using autoware_utils::PoseDeviation;
+
+struct Param
+{
+};
+
+struct Input
+{
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;
+  autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route;
+};
+
+struct Output
+{
+  PoseDeviation goal_deviation;
+};
+
+class GoalDistanceCalculator
+{
+public:
+  Output update(const Input & input);
+
+  void setParam(const Param & param) { param_ = param; }
+
+private:
+  Param param_;
+};
+}  // namespace goal_distance_calculator
+
+#endif  // GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_
diff --git a/...al_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/...al_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp
@@ -0,0 +1,78 @@
+// 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.
+
+#ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_NODE_HPP_
+#define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_NODE_HPP_
+
+#include "goal_distance_calculator/goal_distance_calculator.hpp"
+
+#include <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/self_pose_listener.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/route.hpp>
+#include <autoware_debug_msgs/msg/float64_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+
+namespace goal_distance_calculator
+{
+struct NodeParam
+{
+  double update_rate;
+  bool oneshot;
+};
+
+class GoalDistanceCalculatorNode : public rclcpp::Node
+{
+public:
+  explicit GoalDistanceCalculatorNode(const rclcpp::NodeOptions & options);
+
+private:
+  // Subscriber
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_initial_pose_;
+  autoware_utils::SelfPoseListener self_pose_listener_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Route>::SharedPtr sub_route_;
+
+  // Data Buffer
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+  autoware_auto_planning_msgs::msg::Route::SharedPtr route_;
+
+  // Callback
+  void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg);
+
+  // Publisher
+  autoware_utils::DebugPublisher debug_publisher_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  bool isDataReady();
+  bool isDataTimeout();
+  void onTimer();
+
+  // Parameter
+  NodeParam node_param_;
+  Param param_;
+
+  // Core
+  Input input_;
+  Output output_;
+  std::unique_ptr<GoalDistanceCalculator> goal_distance_calculator_;
+};
+}  // namespace goal_distance_calculator
+#endif  // GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_NODE_HPP_
diff --git a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <arg name="oneshot" default="false" />
+  <arg name="config_file" default="$(find-pkg-share goal_distance_calculator)/config/goal_distance_calculator.param.yaml" />
+  <node pkg="goal_distance_calculator" exec="goal_distance_calculator_node" name="goal_distance_calculator" output="screen">
+    <param from="$(var config_file)"/>
+    <param name="oneshot" value="$(var oneshot)"/>
+  </node>
+</launch>
diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml
@@ -0,0 +1,25 @@
+<?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>goal_distance_calculator</name>
+  <version>0.0.0</version>
+  <description>The goal_distance_calculator package </description>
+  <maintainer email="ttatcoder@outlook.jp">Taiki Tanaka</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>geometry_msgs</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/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp
@@ -0,0 +1,29 @@
+// 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.
+
+#include "goal_distance_calculator/goal_distance_calculator.hpp"
+
+namespace goal_distance_calculator
+{
+Output GoalDistanceCalculator::update(const Input & input)
+{
+  Output output{};
+
+  output.goal_deviation =
+    autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose);
+
+  return output;
+}
+
+}  // namespace goal_distance_calculator