Merge pull request #1 from autowarefoundation/tier4/proposal

chore: sync upstream

control/pure_pursuit/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.5)
+project(pure_pursuit)
+
+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 -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Eigen3 REQUIRED)
+
+
+include_directories(
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+# pure_pursuit_core
+ament_auto_add_library(pure_pursuit_core SHARED
+  src/pure_pursuit_core/planning_utils.cpp
+  src/pure_pursuit_core/pure_pursuit.cpp
+  src/pure_pursuit_core/interpolate.cpp
+)
+
+# pure_pursuit
+ament_auto_add_library(pure_pursuit_node SHARED
+  src/pure_pursuit/pure_pursuit_node.cpp
+  src/pure_pursuit/pure_pursuit_viz.cpp
+)
+
+target_link_libraries(pure_pursuit_node
+  pure_pursuit_core
+)
+
+rclcpp_components_register_node(pure_pursuit_node
+  PLUGIN "pure_pursuit::PurePursuitNode"
+  EXECUTABLE pure_pursuit_node_exe
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

control/pure_pursuit/config/pure_pursuit.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    # -- system --
+    control_period: 0.033
+
+    # -- algorithm --
+    lookahead_distance_ratio: 2.2
+    min_lookahead_distance: 2.5
+    reverse_min_lookahead_distance: 7.0

control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp

@@ -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.
+/*
+ * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_HPP_
+#define PURE_PURSUIT__PURE_PURSUIT_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#define EIGEN_MPL2_ONLY
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace pure_pursuit
+{
+class PurePursuit
+{
+public:
+  PurePursuit() : lookahead_distance_(0.0), clst_thr_dist_(3.0), clst_thr_ang_(M_PI / 4) {}
+  ~PurePursuit() = default;
+
+  rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit");
+  // setter
+  void setCurrentPose(const geometry_msgs::msg::Pose & msg);
+  void setWaypoints(const std::vector<geometry_msgs::msg::Pose> & msg);
+  void setLookaheadDistance(double ld) { lookahead_distance_ = ld; }
+  void setClosestThreshold(double clst_thr_dist, double clst_thr_ang)
+  {
+    clst_thr_dist_ = clst_thr_dist;
+    clst_thr_ang_ = clst_thr_ang;
+  }
+
+  // getter
+  geometry_msgs::msg::Point getLocationOfNextWaypoint() const { return loc_next_wp_; }
+  geometry_msgs::msg::Point getLocationOfNextTarget() const { return loc_next_tgt_; }
+
+  bool isDataReady();
+  std::pair<bool, double> run();  // calculate curvature
+
+private:
+  // variables for debug
+  geometry_msgs::msg::Point loc_next_wp_;
+  geometry_msgs::msg::Point loc_next_tgt_;
+
+  // variables got from outside
+  double lookahead_distance_, clst_thr_dist_, clst_thr_ang_;
+  std::shared_ptr<std::vector<geometry_msgs::msg::Pose>> curr_wps_ptr_;
+  std::shared_ptr<geometry_msgs::msg::Pose> curr_pose_ptr_;
+
+  // functions
+  int32_t findNextPointIdx(int32_t search_start_idx);
+  std::pair<bool, geometry_msgs::msg::Point> lerpNextTarget(int32_t next_wp_idx);
+};
+
+}  // namespace pure_pursuit
+
+#endif  // PURE_PURSUIT__PURE_PURSUIT_HPP_

control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp

@@ -0,0 +1,128 @@
+// 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.
+/*
+ * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_
+#define PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_
+
+#include "pure_pursuit/pure_pursuit.hpp"
+#include "pure_pursuit/pure_pursuit_viz.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/self_pose_listener.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <boost/optional.hpp>  // To be replaced by std::optional in C++17
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <vector>
+
+namespace pure_pursuit
+{
+struct Param
+{
+  // Global Parameters
+  double wheel_base;
+
+  // Node Parameters
+  double ctrl_period;
+
+  // Algorithm Parameters
+  double lookahead_distance_ratio;
+  double min_lookahead_distance;
+  double reverse_min_lookahead_distance;  // min_lookahead_distance in reverse gear
+};
+
+struct DebugData
+{
+  geometry_msgs::msg::Point next_target;
+};
+
+class PurePursuitNode : public rclcpp::Node
+{
+public:
+  explicit PurePursuitNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Subscriber
+  tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_current_odometry_;
+
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_;
+  nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_;
+
+  bool isDataReady();
+
+  void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+  void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+
+  // TF
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+
+  // Publisher
+  rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannLateralCommand>::SharedPtr
+    pub_ctrl_cmd_;
+
+  void publishCommand(const double target_curvature);
+
+  // Debug Publisher
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_marker_;
+
+  void publishDebugMarker() const;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+  void onTimer();
+
+  // Parameter
+  Param param_;
+
+  // Algorithm
+  std::unique_ptr<PurePursuit> pure_pursuit_;
+
+  boost::optional<double> calcTargetCurvature();
+  boost::optional<autoware_auto_planning_msgs::msg::TrajectoryPoint> calcTargetPoint() const;
+
+  // Debug
+  mutable DebugData debug_data_;
+};
+
+}  // namespace pure_pursuit
+
+#endif  // PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_

control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp

@@ -0,0 +1,47 @@
+// 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.
+/*
+ * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_
+#define PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <vector>
+namespace pure_pursuit
+{
+visualization_msgs::msg::Marker createNextTargetMarker(
+  const geometry_msgs::msg::Point & next_target);
+
+visualization_msgs::msg::Marker createTrajectoryCircleMarker(
+  const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose);
+}  // namespace pure_pursuit
+
+#endif  // PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_

control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp

@@ -0,0 +1,55 @@
+// Copyright 2018-2019 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 PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
+#define PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+namespace pure_pursuit
+{
+class LinearInterpolate
+{
+public:
+  LinearInterpolate() {}
+  ~LinearInterpolate() {}
+  static bool interpolate(
+    const std::vector<double> & base_index, const std::vector<double> & base_value,
+    const std::vector<double> & return_index, std::vector<double> & return_value);
+};
+
+class SplineInterpolate
+{
+  bool initialized_ = false;
+  std::vector<double> a_;  //!< @brief temporal vector for calculation
+  std::vector<double> b_;  //!< @brief temporal vector for calculation
+  std::vector<double> c_;  //!< @brief temporal vector for calculation
+  std::vector<double> d_;  //!< @brief temporal vector for calculation
+
+public:
+  SplineInterpolate();
+  explicit SplineInterpolate(const std::vector<double> & x);
+  ~SplineInterpolate();
+  void generateSpline(const std::vector<double> & x);
+  double getValue(const double & s);
+  bool interpolate(
+    const std::vector<double> & base_index, const std::vector<double> & base_value,
+    const std::vector<double> & return_index, std::vector<double> & return_value);
+  void getValueVector(const std::vector<double> & s_v, std::vector<double> & value_v);
+};
+}  // namespace pure_pursuit
+
+#endif  // PURE_PURSUIT__UTIL__INTERPOLATE_HPP_