Port pure pursuit (autowarefoundation#134)

* porting pure_pursuit in progress

* ported pure_pursuit from ROS1 to ROS2

control/pure_pursuit/CMakeLists.txt

@@ -1,92 +1,41 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(pure_pursuit)
 
-add_compile_options(-std=c++14)
+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 -Wno-unused-parameter)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  autoware_control_msgs
-  autoware_planning_msgs
-  geometry_msgs
-  roscpp
-  sensor_msgs
-  std_msgs
-  tf2
-  tf2_eigen
-  tf2_ros
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
 find_package(Eigen3 REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS
-    include
-  CATKIN_DEPENDS
-    autoware_control_msgs
-    autoware_planning_msgs
-    geometry_msgs
-    sensor_msgs
-    std_msgs
-    tf2
-    tf2_eigen
-    tf2_ros
-)
 
 include_directories(
-  include
   ${EIGEN3_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
 )
 
 # pure_pursuit_core
-add_library(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
 )
 
-target_link_libraries(pure_pursuit_core
-  ${catkin_LIBRARIES}
-)
-
-add_dependencies(pure_pursuit_core
-  ${catkin_EXPORTED_TARGETS}
-)
-
 # pure_pursuit
-add_executable(pure_pursuit
+ament_auto_add_executable(pure_pursuit
   src/pure_pursuit/main.cpp
   src/pure_pursuit/pure_pursuit_node.cpp
   src/pure_pursuit/pure_pursuit_viz.cpp
 )
 
 target_link_libraries(pure_pursuit
   pure_pursuit_core
-  ${catkin_LIBRARIES}
-)
-
-add_dependencies(pure_pursuit
-  ${catkin_EXPORTED_TARGETS}
-)
-
-# Install executables and/or libraries
-install(
-  TARGETS
-    pure_pursuit_core
-    pure_pursuit
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-# Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
-# Install files
-install(
-  DIRECTORY
-    config
-    launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ament_auto_package(INSTALL_TO_SHARE
+  launch
 )

control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp

@@ -31,12 +31,14 @@
 
 #include <memory>
 #include <vector>
+#include <rclcpp/rclcpp.hpp>
 
 #define EIGEN_MPL2_ONLY
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 
-#include "geometry_msgs/Pose.h"
+#include <geometry_msgs/msg/pose.hpp>
+#define PURE_PURSUIT_LOGGER "pure_pursuit"
 
 namespace planning_utils
 {
@@ -46,9 +48,11 @@ class PurePursuit
   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_LOGGER);
   // setter
-  void setCurrentPose(const geometry_msgs::Pose & msg);
-  void setWaypoints(const std::vector<geometry_msgs::Pose> & msg);
+  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)
   {
@@ -57,25 +61,25 @@ class PurePursuit
   }
 
   // getter
-  geometry_msgs::Point getLocationOfNextWaypoint() const { return loc_next_wp_; }
-  geometry_msgs::Point getLocationOfNextTarget() const { return loc_next_tgt_; }
+  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::Point loc_next_wp_;
-  geometry_msgs::Point loc_next_tgt_;
+  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::Pose>> curr_wps_ptr_;
-  std::shared_ptr<geometry_msgs::Pose> curr_pose_ptr_;
+  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::Point> lerpNextTarget(int32_t next_wp_idx);
+  std::pair<bool, geometry_msgs::msg::Point> lerpNextTarget(int32_t next_wp_idx);
 };
 
 }  // namespace planning_utils

control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp

@@ -27,22 +27,19 @@
  * limitations under the License.
  */
 
-#pragma once
-
 #include <memory>
 #include <vector>
 
 #include "boost/optional.hpp"  // To be replaced by std::optional in C++17
-
-#include "ros/ros.h"
+#include <rclcpp/rclcpp.hpp>
 
 #include "tf2_ros/transform_listener.h"
 
-#include "autoware_control_msgs/ControlCommandStamped.h"
-#include "autoware_planning_msgs/Trajectory.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "geometry_msgs/TwistStamped.h"
-
+#include <autoware_control_msgs/msg/control_command_stamped.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include "pure_pursuit/pure_pursuit_viz.hpp"
 #include "pure_pursuit/pure_pursuit.hpp"
 
 struct Param
@@ -68,49 +65,46 @@ struct TargetValues
 
 struct DebugData
 {
-  geometry_msgs::Point next_target;
+  geometry_msgs::msg::Point next_target;
 };
 
-class PurePursuitNode
+class PurePursuitNode : public rclcpp::Node
 {
 public:
   PurePursuitNode();
 
 private:
-  // Node Handle
-  ros::NodeHandle nh_;
-  ros::NodeHandle private_nh_;
 
   // Subscriber
-  ros::Subscriber sub_trajectory_;
-  ros::Subscriber sub_current_velocity_;
+  rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_current_velocity_;
 
-  autoware_planning_msgs::Trajectory::ConstPtr trajectory_;
-  geometry_msgs::TwistStamped::ConstPtr current_velocity_;
+  autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_;
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_;
 
-  bool isDataReady() const;
+  bool isDataReady();
 
-  void onTrajectory(const autoware_planning_msgs::Trajectory::ConstPtr & msg);
-  void onCurrentVelocity(const geometry_msgs::TwistStamped::ConstPtr & msg);
+  void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+  void onCurrentVelocity(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg);
 
   // TF
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
-  boost::optional<geometry_msgs::PoseStamped> current_pose_;
+  boost::optional<geometry_msgs::msg::PoseStamped> current_pose_;
 
   // Publisher
-  ros::Publisher pub_ctrl_cmd_;
+  rclcpp::Publisher<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr pub_ctrl_cmd_;
 
-  void publishCommand(const TargetValues & targets) const;
+  void publishCommand(const TargetValues & targets);
 
   // Debug Publisher
-  ros::Publisher pub_debug_marker_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_marker_;
 
   void publishDebugMarker() const;
 
   // Timer
-  ros::Timer timer_;
-  void onTimer(const ros::TimerEvent & event);
+  rclcpp::TimerBase::SharedPtr timer_;
+  void onTimer();
 
   // Parameter
   Param param_;
@@ -119,8 +113,8 @@ class PurePursuitNode
   std::unique_ptr<planning_utils::PurePursuit> pure_pursuit_;
   TargetValues target_values_;
 
-  boost::optional<TargetValues> calcTargetValues() const;
-  boost::optional<autoware_planning_msgs::TrajectoryPoint> calcTargetPoint() const;
+  boost::optional<TargetValues> calcTargetValues();
+  boost::optional<autoware_planning_msgs::msg::TrajectoryPoint> calcTargetPoint() const;
 
   // Debug
   mutable DebugData debug_data_;

control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp

@@ -27,15 +27,12 @@
  * limitations under the License.
  */
 
-#pragma once
-
 #include <vector>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
-#include "geometry_msgs/PoseStamped.h"
-#include "visualization_msgs/Marker.h"
-#include "visualization_msgs/MarkerArray.h"
-
-visualization_msgs::Marker createNextTargetMarker(const geometry_msgs::Point & next_target);
+visualization_msgs::msg::Marker createNextTargetMarker(const geometry_msgs::msg::Point & next_target);
 
-visualization_msgs::Marker createTrajectoryCircleMarker(
-  const geometry_msgs::Point & target, const geometry_msgs::Pose & current_pose);
+visualization_msgs::msg::Marker createTrajectoryCircleMarker(
+  const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose);

control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp

@@ -14,12 +14,12 @@
 #pragma once
 
 #include <string>
+#include <rclcpp/rclcpp.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
-#include "visualization_msgs/MarkerArray.h"
-
-inline geometry_msgs::Point createMarkerPosition(double x, double y, double z)
+inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z)
 {
-  geometry_msgs::Point point;
+  geometry_msgs::msg::Point point;
 
   point.x = x;
   point.y = y;
@@ -28,9 +28,9 @@ inline geometry_msgs::Point createMarkerPosition(double x, double y, double z)
   return point;
 }
 
-inline geometry_msgs::Quaternion createMarkerOrientation(double x, double y, double z, double w)
+inline geometry_msgs::msg::Quaternion createMarkerOrientation(double x, double y, double z, double w)
 {
-  geometry_msgs::Quaternion quaternion;
+  geometry_msgs::msg::Quaternion quaternion;
 
   quaternion.x = x;
   quaternion.y = y;
@@ -40,9 +40,9 @@ inline geometry_msgs::Quaternion createMarkerOrientation(double x, double y, dou
   return quaternion;
 }
 
-inline geometry_msgs::Vector3 createMarkerScale(double x, double y, double z)
+inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z)
 {
-  geometry_msgs::Vector3 scale;
+  geometry_msgs::msg::Vector3 scale;
 
   scale.x = x;
   scale.y = y;
@@ -51,9 +51,9 @@ inline geometry_msgs::Vector3 createMarkerScale(double x, double y, double z)
   return scale;
 }
 
-inline std_msgs::ColorRGBA createMarkerColor(float r, float g, float b, float a)
+inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a)
 {
-  std_msgs::ColorRGBA color;
+  std_msgs::msg::ColorRGBA color;
 
   color.r = r;
   color.g = g;
@@ -63,19 +63,20 @@ inline std_msgs::ColorRGBA createMarkerColor(float r, float g, float b, float a)
   return color;
 }
 
-inline visualization_msgs::Marker createDefaultMarker(
+inline visualization_msgs::msg::Marker createDefaultMarker(
   const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type,
-  const geometry_msgs::Vector3 & scale, const std_msgs::ColorRGBA & color)
+  const geometry_msgs::msg::Vector3 & scale, const std_msgs::msg::ColorRGBA & color)
 {
-  visualization_msgs::Marker marker;
+  visualization_msgs::msg::Marker marker;
 
   marker.header.frame_id = frame_id;
-  marker.header.stamp = ros::Time::now();
+//ToDo
+//  marker.header.stamp = rclcpp::Node::now();
   marker.ns = ns;
   marker.id = id;
   marker.type = type;
-  marker.action = visualization_msgs::Marker::ADD;
-  marker.lifetime = ros::Duration(0);
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.lifetime = rclcpp::Duration(0);
 
   marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0);
   marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0);
@@ -87,8 +88,8 @@ inline visualization_msgs::Marker createDefaultMarker(
 }
 
 inline void appendMarkerArray(
-  const visualization_msgs::MarkerArray & additional_marker_array,
-  visualization_msgs::MarkerArray * marker_array)
+  const visualization_msgs::msg::MarkerArray & additional_marker_array,
+  visualization_msgs::msg::MarkerArray * marker_array)
 {
   for (const auto & marker : additional_marker_array.markers) {
     marker_array->markers.push_back(marker);