diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt index 991d12097cc08..3993674c89e8c 100644 --- a/planning/static_centerline_generator/CMakeLists.txt +++ b/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() diff --git a/planning/static_centerline_generator/README.md b/planning/static_centerline_generator/README.md index 00572b754645c..844d0af15f2a5 100644 --- a/planning/static_centerline_generator/README.md +++ b/planning/static_centerline_generator/README.md @@ -34,7 +34,7 @@ We can run with the following command by designating `` ```sh -ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:= +ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml 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 - `` ```sh -ros2 launch static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= ``` The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index c1d92c06a8e05..e8e5308d1655a 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/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 @@ -34,11 +34,12 @@ #include #include -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 plan_path(const std::vector & route_lane_ids); + static std::vector 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_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 2dcb9cbbd099f..d04117dda756b 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/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_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index c8cf8f8b90590..90b08c63e8715 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -16,7 +16,7 @@ #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 @@ -24,9 +24,7 @@ #include #include -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 & 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_ diff --git a/planning/static_centerline_generator/launch/run_planning_server.launch.xml b/planning/static_centerline_generator/launch/run_planning_server.launch.xml index 1493078317458..6ce0054343acc 100644 --- a/planning/static_centerline_generator/launch/run_planning_server.launch.xml +++ b/planning/static_centerline_generator/launch/run_planning_server.launch.xml @@ -2,11 +2,11 @@ - + - + diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index ae71b0ae6e925..8c05f9a960d0f 100644 --- a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -55,7 +55,7 @@ - + @@ -79,11 +79,11 @@ - + - + diff --git a/planning/static_centerline_generator/package.xml b/planning/static_centerline_generator/package.xml index 6573f6e439c43..96e17595d49ee 100644 --- a/planning/static_centerline_generator/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -1,9 +1,9 @@ - static_centerline_generator + autoware_static_centerline_generator 0.1.0 - The static_centerline_generator package + The autoware_static_centerline_generator package Takayuki Murooka Kosuke Takeuchi Apache License 2.0 diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/static_centerline_generator/scripts/app.py index c1cb0473da040..08bff8b80dcb7 100755 --- a/planning/static_centerline_generator/scripts/app.py +++ b/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") diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 981cf54fc9274..fb8199f353b67 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/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(node_options); + std::make_shared( + node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index ec7d6278e346d..f603cfecdb8c7 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -12,8 +12,11 @@ // 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" @@ -21,10 +24,12 @@ #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 #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { +using ::tier4_autoware_utils::Point2d; + namespace { std::vector 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 diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index ddfd3e11036ce..04c8f469200ce 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/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 #include -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 diff --git a/planning/static_centerline_generator/srv/PlanPath.srv b/planning/static_centerline_generator/srv/PlanPath.srv index 7d823b4eccbf9..3a8d0321ffb9a 100644 --- a/planning/static_centerline_generator/srv/PlanPath.srv +++ b/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 diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 29ee49a11e3b3..2ecba10d994d7 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,11 +28,12 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("static_centerline_generator"), "test/data/lanelet2_map.osm" + get_package_share_directory("autoware_static_centerline_generator"), + "test/data/lanelet2_map.osm" ) static_centerline_generator_node = Node( - package="static_centerline_generator", + package="autoware_static_centerline_generator", executable="main", output="screen", parameters=[ @@ -50,7 +51,7 @@ def generate_test_description(): "mission_planner.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/static_centerline_generator.param.yaml", ), os.path.join( @@ -74,15 +75,15 @@ def generate_test_description(): "config/lanelet2_map_loader.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/common.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/nearest_search.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/vehicle_info.param.yaml", ), ],