Port lane change planner (autowarefoundation#60)

* port CMakeLists.txt Signed-off-by: mitsudome-r <[email protected]> * port headers Signed-off-by: mitsudome-r <[email protected]> * port loggers Signed-off-by: mitsudome-r <[email protected]> * port time, duration, clock Signed-off-by: mitsudome-r <[email protected]> * fix warnings Signed-off-by: mitsudome-r <[email protected]> * port publishers and subscribers Signed-off-by: mitsudome-r <[email protected]> * port parameters Signed-off-by: mitsudome-r <[email protected]> * port tf Signed-off-by: mitsudome-r <[email protected]> * other Signed-off-by: mitsudome-r <[email protected]>
yukkysaito · Oct 30, 2020 · e5c8591 · e5c8591
1 parent e7d5cbe
commit e5c8591
Show file tree

Hide file tree

Showing 30 changed files with 708 additions and 663 deletions.
diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/CMakeLists.txt b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/CMakeLists.txt
@@ -1,31 +1,24 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(lane_change_planner)
 
-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)
+  add_compile_options(-Wno-unused-parameter)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  autoware_perception_msgs
-  autoware_planning_msgs
-  geometry_msgs
-  lanelet2_extension
-  roscpp
-  sensor_msgs
-  tf2
-  tf2_geometry_msgs
-  tf2_ros
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
 find_package(OpenCV REQUIRED)
 
-catkin_package()
-
 include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
 
-add_executable(lane_change_planner
+ament_auto_add_executable(lane_change_planner
   src/lane_change_planner_node.cpp
   src/lane_changer.cpp
   src/data_manager.cpp
@@ -42,16 +35,9 @@ add_executable(lane_change_planner
   src/state/stopping_lane_change.cpp
   src/state/common_functions.cpp
 )
-add_dependencies(lane_change_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(lane_change_planner
-  ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
 )
 
-install(
-  TARGETS
-    lane_change_planner
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ament_auto_package(
 )
diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/COLCON_IGNORE b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/COLCON_IGNORE
diff --git a/..._driving/behavior_planning/lane_change_planner/include/lane_change_planner/data_manager.h b/..._driving/behavior_planning/lane_change_planner/include/lane_change_planner/data_manager.h
@@ -18,18 +18,18 @@
 #define LANE_CHANGE_PLANNER_DATA_MANAGER_H
 
 // ROS
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/TwistStamped.h>
-#include <ros/ros.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <std_msgs/Bool.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/bool.hpp>
 #include <tf2_ros/transform_listener.h>
 
 // Autoware
-#include <autoware_lanelet2_msgs/MapBin.h>
-#include <autoware_perception_msgs/DynamicObjectArray.h>
-#include <autoware_planning_msgs/PathWithLaneId.h>
-#include <autoware_planning_msgs/Route.h>
+#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
+#include <autoware_perception_msgs/msg/dynamic_object_array.hpp>
+#include <autoware_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_planning_msgs/msg/route.hpp>
 #include <lane_change_planner/parameters.h>
 
 // lanelet
@@ -45,20 +45,22 @@ namespace lane_change_planner
 class SelfPoseLinstener
 {
 public:
-  SelfPoseLinstener();
-  bool getSelfPose(geometry_msgs::PoseStamped & self_pose);
+  SelfPoseLinstener(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock);
+  bool getSelfPose(geometry_msgs::msg::PoseStamped & self_pose);
   bool isSelfPoseReady();
 
 private:
-  tf2_ros::Buffer tf_buffer_;
+  tf2::BufferCore tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 struct BoolStamped
 {
   explicit BoolStamped(bool in_data) : data(in_data) {}
   bool data = false;
-  ros::Time stamp;
+  rclcpp::Time stamp;
 };
 
 class DataManager
@@ -67,40 +69,45 @@ class DataManager
   /*
    * Cache
    */
-  autoware_perception_msgs::DynamicObjectArray::ConstPtr perception_ptr_;
-  geometry_msgs::TwistStamped::ConstPtr vehicle_velocity_ptr_;
+  autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr perception_ptr_;
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr vehicle_velocity_ptr_;
   BoolStamped lane_change_approval_;
   BoolStamped force_lane_change_;
-  geometry_msgs::PoseStamped self_pose_;
+  geometry_msgs::msg::PoseStamped self_pose_;
 
   // ROS parameters
   LaneChangerParameters parameters_;
   bool is_parameter_set_;
 
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
   /*
    * SelfPoseLinstener
    */
   std::shared_ptr<SelfPoseLinstener> self_pose_listener_ptr_;
 
 public:
-  DataManager();
+  DataManager(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock);
   ~DataManager() = default;
 
   // callbacks
   void perceptionCallback(
-    const autoware_perception_msgs::DynamicObjectArray::ConstPtr & input_perception_msg);
-  void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr & input_twist_msg);
+    const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_perception_msg);
+  void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_twist_msg);
   void setLaneChangerParameters(const LaneChangerParameters & parameters);
-  void laneChangeApprovalCallback(const std_msgs::Bool & input_approval_msg);
-  void forceLaneChangeSignalCallback(const std_msgs::Bool & input_approval_msg);
+  void laneChangeApprovalCallback(const std_msgs::msg::Bool::ConstSharedPtr input_approval_msg);
+  void forceLaneChangeSignalCallback(const std_msgs::msg::Bool::ConstSharedPtr input_approval_msg);
 
   // getters
-  autoware_perception_msgs::DynamicObjectArray::ConstPtr getDynamicObjects();
-  geometry_msgs::PoseStamped getCurrentSelfPose();
-  geometry_msgs::TwistStamped::ConstPtr getCurrentSelfVelocity();
+  autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr getDynamicObjects();
+  geometry_msgs::msg::PoseStamped getCurrentSelfPose();
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr getCurrentSelfVelocity();
   LaneChangerParameters getLaneChangerParameters();
   bool getLaneChangeApproval();
   bool getForceLaneChangeSignal();
+  rclcpp::Logger & getLogger();
+  rclcpp::Clock::SharedPtr getClock();
 
   bool isDataReady();
 };

diff --git a/..._driving/behavior_planning/lane_change_planner/include/lane_change_planner/lane_changer.h b/..._driving/behavior_planning/lane_change_planner/include/lane_change_planner/lane_changer.h
@@ -18,16 +18,18 @@
 #define LANE_CHANGE_PLANNER_LANE_CHANGER_H
 
 // ROS
-#include <ros/ros.h>
-#include <std_msgs/Bool.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
 #include <tf2_ros/transform_listener.h>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 // Autoware
-#include <autoware_lanelet2_msgs/MapBin.h>
-#include <autoware_perception_msgs/DynamicObjectArray.h>
-#include <autoware_planning_msgs/PathWithLaneId.h>
-#include <autoware_planning_msgs/Route.h>
-#include <autoware_planning_msgs/StopReasonArray.h>
+#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
+#include <autoware_perception_msgs/msg/dynamic_object_array.hpp>
+#include <autoware_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_planning_msgs/msg/path.hpp>
+#include <autoware_planning_msgs/msg/route.hpp>
+#include <autoware_planning_msgs/msg/stop_reason_array.hpp>
 
 #include <lane_change_planner/data_manager.h>
 #include <lane_change_planner/route_handler.h>
@@ -42,42 +44,39 @@
 
 namespace lane_change_planner
 {
-class LaneChanger
+class LaneChanger : public rclcpp::Node
 {
 private:
-  ros::Timer timer_;
+  rclcpp::TimerBase::SharedPtr timer_;
 
-  ros::Publisher path_publisher_;
-  ros::Publisher candidate_path_publisher_;
-  ros::Publisher path_marker_publisher_;
-  ros::Publisher stop_reason_publisher_;
-  ros::Publisher drivable_area_publisher_;
-  ros::Publisher lane_change_ready_publisher_;
-  ros::Publisher lane_change_available_publisher_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::PathWithLaneId>::SharedPtr path_publisher_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::Path>::SharedPtr candidate_path_publisher_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_marker_publisher_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reason_publisher_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr drivable_area_publisher_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr lane_change_ready_publisher_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr lane_change_available_publisher_;
 
-  ros::NodeHandle pnh_;
+  rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr perception_subscriber_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_subscriber_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr lane_change_approval_subscriber_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr force_lane_change_subscriber_;
 
-  ros::Subscriber points_subscriber_;
-  ros::Subscriber perception_subscriber_;
-  ros::Subscriber velocity_subscriber_;
-  ros::Subscriber lane_change_approval_subscriber_;
-  ros::Subscriber force_lane_change_subscriber_;
-
-  ros::Subscriber vector_map_subscriber_;
-  ros::Subscriber route_subscriber_;
-  ros::Subscriber route_init_subscriber_;
+  rclcpp::Subscription<autoware_lanelet2_msgs::msg::MapBin>::SharedPtr vector_map_subscriber_;
+  rclcpp::Subscription<autoware_planning_msgs::msg::Route>::SharedPtr route_subscriber_;
+  rclcpp::Subscription<autoware_planning_msgs::msg::Route>::SharedPtr route_init_subscriber_;
 
   std::shared_ptr<DataManager> data_manager_ptr_;
   std::shared_ptr<StateMachine> state_machine_ptr_;
   std::shared_ptr<RouteHandler> route_handler_ptr_;
   // PathExtender path_extender_;
 
-  void run(const ros::TimerEvent & event);
+  void run();
   void publishDebugMarkers();
-  void publishDrivableArea(const autoware_planning_msgs::PathWithLaneId & path);
-  autoware_planning_msgs::StopReasonArray makeStopReasonArray(
+  void publishDrivableArea(const autoware_planning_msgs::msg::PathWithLaneId & path);
+  autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray(
     const DebugData & debug_data, const State & state);
-  std::vector<autoware_planning_msgs::StopReason> makeEmptyStopReasons();
+  std::vector<autoware_planning_msgs::msg::StopReason> makeEmptyStopReasons();
   void waitForData();
 
 public: