build(static_centerline_optimizer): prefix package and namespace with autoware_

planning/static_centerline_generator/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(static_centerline_generator)
+project(autoware_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(
-  static_centerline_generator
+  autoware_static_centerline_generator
   "srv/LoadMap.srv"
   "srv/PlanRoute.srv"
   "srv/PlanPath.srv"
@@ -29,10 +29,10 @@ ament_auto_add_executable(main
 
 if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
     rosidl_target_interfaces(main
-    static_centerline_generator "rosidl_typesupport_cpp")
+    autoware_static_centerline_generator "rosidl_typesupport_cpp")
 else()
     rosidl_get_typesupport_target(
-            cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp")
+            cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp")
     target_link_libraries(main "${cpp_typesupport_target}")
 endif()
 

planning/static_centerline_generator/README.md

@@ -34,7 +34,7 @@ We can run
 with the following command by designating `<vehicle_model>`
 
 ```sh
-ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:=<vehicle-model>
+ros2 launch autoware_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 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 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>
 ```
 
 The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`.

planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp

@@ -16,11 +16,11 @@
 #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_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 "autoware_static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.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 "autoware_static_centerline_generator/type_alias.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include <geography_utils/lanelet2_projector.hpp>
@@ -34,11 +34,12 @@
 #include <utility>
 #include <vector>
 
-namespace static_centerline_generator
+namespace autoware::static_centerline_generator
 {
-using static_centerline_generator::srv::LoadMap;
-using static_centerline_generator::srv::PlanPath;
-using static_centerline_generator::srv::PlanRoute;
+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;
 
 struct CenterlineWithRoute
 {
@@ -66,6 +67,9 @@ 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);
 
@@ -113,7 +117,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 static_centerline_generator
+}  // namespace autoware::static_centerline_generator
 #endif  // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_

planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp

@@ -11,8 +11,8 @@
 // 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 STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
-#define STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
+#ifndef AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
+#define AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
 
 #include "route_handler/route_handler.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
@@ -26,7 +26,7 @@
 #include "autoware_planning_msgs/msg/lanelet_route.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
-namespace static_centerline_generator
+namespace autoware::static_centerline_generator
 {
 using autoware_auto_mapping_msgs::msg::HADMapBin;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -36,11 +36,9 @@ 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 static_centerline_generator
+}  // namespace autoware::static_centerline_generator
 
-#endif  // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
+#endif  // AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_

planning/static_centerline_generator/include/static_centerline_generator/utils.hpp

@@ -16,17 +16,15 @@
 #define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_
 
 #include "route_handler/route_handler.hpp"
-#include "static_centerline_generator/type_alias.hpp"
+#include "autoware_static_centerline_generator/type_alias.hpp"
 
 #include <rclcpp/time.hpp>
 
 #include <memory>
 #include <string>
 #include <vector>
 
-namespace static_centerline_generator
-{
-namespace utils
+namespace autoware::static_centerline_generator::utils
 {
 rclcpp::QoS create_transient_local_qos();
 
@@ -37,8 +35,8 @@ 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 nearest_ego_dist_threshold,
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets,
+  const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold,
   const double nearest_ego_yaw_threshold);
 
 void update_centerline(
@@ -52,7 +50,6 @@ 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 utils
-}  // namespace static_centerline_generator
+}  // namespace autoware::static_centerline_generator::utils
 
 #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 static_centerline_generator)/launch/static_centerline_generator.launch.xml">
+  <include file="$(find-pkg-share autoware_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="static_centerline_generator" exec="app.py" name="static_centerline_generator_http_server" output="screen"/>
+  <node pkg="autoware_static_centerline_generator" exec="app.py" name="autoware_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="static_centerline_generator" exec="main" name="static_centerline_generator">
+  <node pkg="autoware_static_centerline_generator" exec="main" name="autoware_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 static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
+    <param from="$(find-pkg-share autoware_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 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 autoware_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>static_centerline_generator</name>
+  <name>autoware_static_centerline_generator</name>
   <version>0.1.0</version>
-  <description>The static_centerline_generator package</description>
+  <description>The autoware_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/main.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "static_centerline_generator/static_centerline_generator_node.hpp"
+#include "autoware_static_centerline_generator/static_centerline_generator_node.hpp"
 
 int main(int argc, char * argv[])
 {
@@ -21,7 +21,8 @@ int main(int argc, char * argv[])
   // initialize node
   rclcpp::NodeOptions node_options;
   auto node =
-    std::make_shared<static_centerline_generator::StaticCenterlineGeneratorNode>(node_options);
+    std::make_shared<autoware::static_centerline_generator::StaticCenterlineGeneratorNode>(
+      node_options);
 
   // get ros parameter
   const bool run_background = node->declare_parameter<bool>("run_background");

planning/static_centerline_generator/src/static_centerline_generator_node.cpp

@@ -12,19 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "static_centerline_generator/static_centerline_generator_node.hpp"
+#include "autoware_static_centerline_generator/static_centerline_generator_node.hpp"
 
+#include "autoware_static_centerline_optimizer/msg/points_with_lane_id.hpp"
+#include "autoware_static_centerline_optimizer/type_alias.hpp"
+#include "autoware_static_centerline_optimizer/utils.hpp"
 #include "lanelet2_extension/utility/message_conversion.hpp"
 #include "lanelet2_extension/utility/query.hpp"
 #include "lanelet2_extension/utility/utilities.hpp"
 #include "map_loader/lanelet2_map_loader_node.hpp"
 #include "map_projection_loader/load_info_from_lanelet2_map.hpp"
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/conversion.hpp"
-#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp"
-#include "static_centerline_generator/msg/points_with_lane_id.hpp"
-#include "static_centerline_generator/type_alias.hpp"
-#include "static_centerline_generator/utils.hpp"
+#include "autoware_static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp"
+#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp"
+#include "autoware_static_centerline_generator/type_alias.hpp"
+#include "autoware_static_centerline_generator/utils.hpp"
+#include "obstacle_avoidance_planner/node.hpp"
+#include "path_smoother/elastic_band_smoother.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/ros/parameter.hpp"
 
@@ -53,8 +58,10 @@
 #include <string>
 #include <vector>
 
-namespace static_centerline_generator
+namespace autoware::static_centerline_generator
 {
+using ::tier4_autoware_utils::Point2d;
+
 namespace
 {
 std::vector<lanelet::Id> get_lane_ids_from_route(const LaneletRoute & route)
@@ -533,7 +540,7 @@ void StaticCenterlineGeneratorNode::on_plan_path(
 
     if (!current_lanelet_points.empty()) {
       // register points with lane_id
-      static_centerline_generator::msg::PointsWithLaneId points_with_lane_id;
+      autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id;
       points_with_lane_id.lane_id = lanelet.id();
       points_with_lane_id.points = current_lanelet_points;
       response->points_with_lane_ids.push_back(points_with_lane_id);
@@ -636,4 +643,4 @@ void StaticCenterlineGeneratorNode::save_map(
   lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_);
   RCLCPP_INFO(get_logger(), "Saved map.");
 }
-}  // namespace static_centerline_generator
+}  // namespace autoware::static_centerline_generator

planning/static_centerline_generator/src/utils.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "static_centerline_generator/utils.hpp"
+#include "autoware_static_centerline_generator/utils.hpp"
 
 #include "behavior_path_planner_common/data_manager.hpp"
 #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
@@ -21,7 +21,7 @@
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
-namespace static_centerline_generator
+namespace autoware::static_centerline_generator
 {
 namespace
 {
@@ -32,7 +32,7 @@ nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs:
   return odometry_ptr;
 }
 
-lanelet::Point3d createPoint3d(const double x, const double y, const double z = 19.0)
+lanelet::Point3d create_point3d(const double x, const double y, const double z = 19.0)
 {
   lanelet::Point3d point(lanelet::utils::getId());
   point.setAttribute("local_x", x);
@@ -89,7 +89,7 @@ geometry_msgs::msg::Pose get_center_pose(
 }
 
 PathWithLaneId get_path_with_lane_id(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets,
+  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)
 {
@@ -141,7 +141,7 @@ void update_centerline(
       const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y);
       const bool is_inside = lanelet::geometry::inside(lanelet_ref, point);
       if (is_inside) {
-        const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z);
+        const auto center_point = create_point3d(traj_pos.x, traj_pos.y, traj_pos.z);
 
         // set center point
         centerline.push_back(center_point);
@@ -228,4 +228,4 @@ MarkerArray create_distance_text_marker(
   return marker_array;
 }
 }  // namespace utils
-}  // namespace static_centerline_generator
+}  // namespace autoware::static_centerline_generator

planning/static_centerline_generator/srv/PlanPath.srv

@@ -1,6 +1,6 @@
 uint32 map_id
 int64[] route
 ---
-static_centerline_generator/PointsWithLaneId[] points_with_lane_ids
+autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids
 string message
 int64[] unconnected_lane_ids