revert changes for static_centerline_generator

Signed-off-by: Esteve Fernandez <[email protected]>

planning/static_centerline_generator/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(autoware_static_centerline_generator)
+project(static_centerline_generator)
 
 find_package(autoware_cmake REQUIRED)
 
@@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(
-  ${PROJECT_NAME}
+  static_centerline_generator
   "srv/LoadMap.srv"
   "srv/PlanRoute.srv"
   "srv/PlanPath.srv"
@@ -19,25 +19,21 @@ rosidl_generate_interfaces(
 
 autoware_package()
 
-ament_auto_add_library(${PROJECT_NAME}_node SHARED
+ament_auto_add_executable(main
+  src/main.cpp
   src/static_centerline_generator_node.cpp
   src/centerline_source/optimization_trajectory_based_centerline.cpp
   src/centerline_source/bag_ego_trajectory_based_centerline.cpp
   src/utils.cpp
 )
 
-rclcpp_components_register_node(${PROJECT_NAME}_node
-  PLUGIN "autoware::static_centerline_generator::StaticCenterlineGeneratorNode"
-  EXECUTABLE ${PROJECT_NAME}_exe
-)
-
 if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
-    rosidl_target_interfaces(${PROJECT_NAME}_node
-    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    rosidl_target_interfaces(main
+    static_centerline_generator "rosidl_typesupport_cpp")
 else()
     rosidl_get_typesupport_target(
-            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
-    target_link_libraries(${PROJECT_NAME}_node "${cpp_typesupport_target}")
+            cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp")
+    target_link_libraries(main "${cpp_typesupport_target}")
 endif()
 
 ament_auto_package(

planning/static_centerline_generator/README.md

@@ -34,7 +34,7 @@ We can run
 with the following command by designating `<vehicle_model>`
 
 ```sh
-ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:=<vehicle-model>
+ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:=<vehicle-model>
 ```
 
 FYI, port ID of the http server is 4010 by default.
@@ -50,7 +50,7 @@ The optimized centerline can be generated from the command line interface by des
 - `<vehicle-model>`
 
 ```sh
-ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:=<input-osm-path> lanelet2_output_file_path:=<output-osm-path> start_lanelet_id:=<start-lane-id> end_lanelet_id:=<end-lane-id> vehicle_model:=<vehicle-model>
+ros2 launch static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:=<input-osm-path> lanelet2_output_file_path:=<output-osm-path> start_lanelet_id:=<start-lane-id> end_lanelet_id:=<end-lane-id> vehicle_model:=<vehicle-model>
 ```
 
 The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`.

planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp

@@ -18,13 +18,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "static_centerline_generator/type_alias.hpp"
 
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
-
 #include <vector>
 
-namespace autoware::static_centerline_generator
+namespace static_centerline_generator
 {
-using ::autoware_auto_planning_msgs::msg::TrajectoryPoint;
 std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node);
-}  // namespace autoware::static_centerline_generator
+}  // namespace static_centerline_generator
 #endif  // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_

planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp

@@ -12,43 +12,32 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_
-STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
-#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_
-  STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
+#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
+#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
 
 #include "rclcpp/rclcpp.hpp"
-#include "route_handler/route_handler.hpp"
 #include "static_centerline_generator/type_alias.hpp"
 
-#include "autoware_auto_planning_msgs/msg/path.hpp"
-#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
-
 #include <vector>
 
-  namespace autoware::static_centerline_generator
+namespace static_centerline_generator
+{
+class OptimizationTrajectoryBasedCenterline
 {
-  using ::autoware_auto_planning_msgs::msg::Path;
-  using ::autoware_auto_planning_msgs::msg::PathWithLaneId;
-  using ::autoware_auto_planning_msgs::msg::TrajectoryPoint;
-  using ::route_handler::RouteHandler;
-  class OptimizationTrajectoryBasedCenterline
-  {
-  public:
-    OptimizationTrajectoryBasedCenterline() = default;
-    explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node);
-    std::vector<TrajectoryPoint> generate_centerline_with_optimization(
-      rclcpp::Node & node, const RouteHandler & route_handler,
-      const std::vector<lanelet::Id> & route_lane_ids);
+public:
+  OptimizationTrajectoryBasedCenterline() = default;
+  explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node);
+  std::vector<TrajectoryPoint> generate_centerline_with_optimization(
+    rclcpp::Node & node, const RouteHandler & route_handler,
+    const std::vector<lanelet::Id> & route_lane_ids);
 
-  private:
-    [[nodiscard]] static std::vector<TrajectoryPoint> optimize_trajectory(const Path & raw_path);
+private:
+  std::vector<TrajectoryPoint> optimize_trajectory(const Path & raw_path) const;
 
-    rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
-    rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
-  };
-}  // namespace autoware::static_centerline_generator
+  rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
+  rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
+};
+}  // namespace static_centerline_generator
 // clang-format off
 #endif  // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
 // clang-format on

planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp

@@ -15,11 +15,11 @@
 #ifndef STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_
 #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_
 
-#include "autoware_static_centerline_generator/srv/load_map.hpp"
-#include "autoware_static_centerline_generator/srv/plan_path.hpp"
-#include "autoware_static_centerline_generator/srv/plan_route.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp"
+#include "static_centerline_generator/srv/load_map.hpp"
+#include "static_centerline_generator/srv/plan_path.hpp"
+#include "static_centerline_generator/srv/plan_route.hpp"
 #include "static_centerline_generator/type_alias.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
@@ -34,12 +34,11 @@
 #include <utility>
 #include <vector>
 
-namespace autoware::static_centerline_generator
+namespace static_centerline_generator
 {
-using autoware_static_centerline_generator::srv::LoadMap;
-using autoware_static_centerline_generator::srv::PlanPath;
-using autoware_static_centerline_generator::srv::PlanRoute;
-using ::route_handler::RouteHandler;
+using static_centerline_generator::srv::LoadMap;
+using static_centerline_generator::srv::PlanPath;
+using static_centerline_generator::srv::PlanRoute;
 
 struct CenterlineWithRoute
 {
@@ -67,9 +66,6 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node
 
   // plan centerline
   CenterlineWithRoute generate_centerline_with_route();
-  // plan path
-  std::vector<TrajectoryPoint> plan_path(const std::vector<lanelet::Id> & route_lane_ids);
-  static std::vector<TrajectoryPoint> optimize_trajectory(const Path & raw_path);
   void on_plan_path(
     const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
 
@@ -117,7 +113,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr callback_group_;
 
   // vehicle info
-  vehicle_info_util::VehicleInfo vehicle_info_{};
+  vehicle_info_util::VehicleInfo vehicle_info_;
 };
-}  // namespace autoware::static_centerline_generator
+}  // namespace static_centerline_generator
 #endif  // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_

planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp

@@ -26,7 +26,7 @@
 #include "autoware_planning_msgs/msg/lanelet_route.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
-namespace autoware::static_centerline_generator
+namespace static_centerline_generator
 {
 using autoware_auto_mapping_msgs::msg::HADMapBin;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -36,9 +36,11 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using autoware_planning_msgs::msg::LaneletRoute;
+using route_handler::RouteHandler;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::LineString2d;
+using tier4_autoware_utils::Point2d;
 using visualization_msgs::msg::MarkerArray;
-}  // namespace autoware::static_centerline_generator
+}  // namespace static_centerline_generator
 
 #endif  // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_

planning/static_centerline_generator/include/static_centerline_generator/utils.hpp

@@ -24,10 +24,10 @@
 #include <string>
 #include <vector>
 
-namespace autoware::static_centerline_generator::utils
+namespace static_centerline_generator
+{
+namespace utils
 {
-using ::route_handler::RouteHandler;
-
 rclcpp::QoS create_transient_local_qos();
 
 lanelet::ConstLanelets get_lanelets_from_ids(
@@ -37,9 +37,9 @@ geometry_msgs::msg::Pose get_center_pose(
   const RouteHandler & route_handler, const size_t lanelet_id);
 
 PathWithLaneId get_path_with_lane_id(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets,
-  const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold,
-  const double ego_nearest_yaw_threshold);
+  const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets,
+  const geometry_msgs::msg::Pose & start_pose, const double nearest_ego_dist_threshold,
+  const double nearest_ego_yaw_threshold);
 
 void update_centerline(
   RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets,
@@ -52,6 +52,7 @@ MarkerArray create_footprint_marker(
 MarkerArray create_distance_text_marker(
   const geometry_msgs::msg::Pose & pose, const double dist,
   const std::array<double, 3> & marker_color, const rclcpp::Time & now, const size_t idx);
-}  // namespace autoware::static_centerline_generator::utils
+}  // namespace utils
+}  // namespace static_centerline_generator
 
 #endif  // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_

planning/static_centerline_generator/launch/run_planning_server.launch.xml

@@ -2,11 +2,11 @@
   <!-- mandatory arguments for planning-->
   <arg name="vehicle_model"/>
 
-  <include file="$(find-pkg-share autoware_static_centerline_generator)/launch/static_centerline_generator.launch.xml">
+  <include file="$(find-pkg-share static_centerline_generator)/launch/static_centerline_generator.launch.xml">
     <arg name="vehicle_model" value="$(var vehicle_model)"/>
     <arg name="run_background" value="true"/>
   </include>
 
   <!-- local server to connect path optimizer and cloud software -->
-  <node pkg="autoware_static_centerline_generator" exec="app.py" name="autoware_static_centerline_generator_http_server" output="screen"/>
+  <node pkg="static_centerline_generator" exec="app.py" name="static_centerline_generator_http_server" output="screen"/>
 </launch>

planning/static_centerline_generator/launch/static_centerline_generator.launch.xml

@@ -55,7 +55,7 @@
   </node>
 
   <!-- optimize path -->
-  <node pkg="autoware_static_centerline_generator" exec="autoware_static_centerline_generator_exe" name="autoware_static_centerline_generator">
+  <node pkg="static_centerline_generator" exec="main" name="static_centerline_generator">
     <remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
     <remap from="input_centerline" to="~/input_centerline"/>
     <remap from="output_whole_centerline" to="~/output_whole_centerline"/>
@@ -79,11 +79,11 @@
     <param from="$(var obstacle_avoidance_planner_param)"/>
     <param from="$(var mission_planner_param)"/>
     <!-- node param -->
-    <param from="$(find-pkg-share autoware_static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
+    <param from="$(find-pkg-share static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
     <param name="centerline_source" value="$(var centerline_source)"/>
     <param name="bag_filename" value="$(var bag_filename)"/>
   </node>
 
   <!-- rviz -->
-  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.rviz" if="$(var rviz)"/>
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share static_centerline_generator)/rviz/static_centerline_generator.rviz" if="$(var rviz)"/>
 </launch>

planning/static_centerline_generator/package.xml

@@ -1,9 +1,9 @@
 <?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>autoware_static_centerline_generator</name>
+  <name>static_centerline_generator</name>
   <version>0.1.0</version>
-  <description>The autoware_static_centerline_generator package</description>
+  <description>The static_centerline_generator package</description>
   <maintainer email="[email protected]">Takayuki Murooka</maintainer>
   <maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
   <license>Apache License 2.0</license>

planning/static_centerline_generator/scripts/app.py

@@ -17,15 +17,15 @@
 import json
 import uuid
 
-from autoware_static_centerline_generator.srv import LoadMap
-from autoware_static_centerline_generator.srv import PlanPath
-from autoware_static_centerline_generator.srv import PlanRoute
 from flask import Flask
 from flask import jsonify
 from flask import request
 from flask_cors import CORS
 import rclpy
 from rclpy.node import Node
+from static_centerline_generator.srv import LoadMap
+from static_centerline_generator.srv import PlanPath
+from static_centerline_generator.srv import PlanRoute
 
 rclpy.init()
 node = Node("static_centerline_generator_http_server")

planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp

@@ -20,7 +20,7 @@
 
 #include <nav_msgs/msg/odometry.hpp>
 
-namespace autoware::static_centerline_generator
+namespace static_centerline_generator
 {
 std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node)
 {
@@ -79,4 +79,4 @@ std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node)
 
   return centerline_traj_points;
 }
-}  // namespace autoware::static_centerline_generator
+}  // namespace static_centerline_generator