Skip to content

Commit

Permalink
refactor(static_centerline_generator): change the package name from s…
Browse files Browse the repository at this point in the history
…tatic_centerline_optimizer (#6795)

static_centerline_optimizer -> static_centerline_generator

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
takayuki5168 authored Apr 12, 2024
1 parent 682a814 commit fd032e8
Show file tree
Hide file tree
Showing 43 changed files with 133 additions and 133 deletions.
2 changes: 1 addition & 1 deletion .github/CODEOWNERS
Validating CODEOWNERS rules …
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,7 @@ planning/sampling_based_planner/frenet_planner/** [email protected]
planning/sampling_based_planner/path_sampler/** [email protected]
planning/sampling_based_planner/sampler_common/** [email protected]
planning/scenario_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
planning/static_centerline_optimizer/** [email protected] [email protected]
planning/static_centerline_generator/** [email protected] [email protected]
planning/surround_obstacle_checker/** [email protected]
sensing/gnss_poser/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
sensing/image_diagnostics/** [email protected]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ def generate_test_description():
LaunchDescription(
[
map_projection_loader_node,
# Start test after 1s - gives time for the static_centerline_optimizer to finish initialization
# Start test after 1s - gives time for the static_centerline_generator to finish initialization
launch.actions.TimerAction(
period=1.0, actions=[launch_testing.actions.ReadyToTest()]
),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ def generate_test_description():
LaunchDescription(
[
map_projection_loader_node,
# Start test after 1s - gives time for the static_centerline_optimizer to finish initialization
# Start test after 1s - gives time for the static_centerline_generator to finish initialization
launch.actions.TimerAction(
period=1.0, actions=[launch_testing.actions.ReadyToTest()]
),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ def generate_test_description():
LaunchDescription(
[
map_projection_loader_node,
# Start test after 1s - gives time for the static_centerline_optimizer to finish initialization
# Start test after 1s - gives time for the static_centerline_generator to finish initialization
launch.actions.TimerAction(
period=1.0, actions=[launch_testing.actions.ReadyToTest()]
),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ def generate_test_description():
LaunchDescription(
[
map_projection_loader_node,
# Start test after 1s - gives time for the static_centerline_optimizer to finish initialization
# Start test after 1s - gives time for the static_centerline_generator to finish initialization
launch.actions.TimerAction(
period=1.0, actions=[launch_testing.actions.ReadyToTest()]
),
Expand Down
2 changes: 1 addition & 1 deletion planning/.pages
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ nav:
- 'About Motion Velocity Smoother': planning/motion_velocity_smoother
- 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja
- 'Scenario Selector': planning/scenario_selector
- 'Static Centerline Optimizer': planning/static_centerline_optimizer
- 'Static Centerline Generator': planning/static_centerline_generator
- 'API and Library':
- 'Costmap Generator': planning/costmap_generator
- 'External Velocity Limit Selector': planning/external_velocity_limit_selector
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,12 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
public:
explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options);

// NOTE: This is for the static_centerline_optimizer package which utilizes the following
// NOTE: This is for the static_centerline_generator package which utilizes the following
// instance.
std::shared_ptr<MPTOptimizer> getMPTOptimizer() const { return mpt_optimizer_ptr_; }

// private:
protected: // for the static_centerline_optimizer package
protected: // for the static_centerline_generator package
// TODO(murooka) move this node to common
class DrivingDirectionChecker
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ class ElasticBandSmoother : public rclcpp::Node
public:
explicit ElasticBandSmoother(const rclcpp::NodeOptions & node_options);

// NOTE: This is for the static_centerline_optimizer package which utilizes the following
// NOTE: This is for the static_centerline_generator package which utilizes the following
// instance.
std::shared_ptr<EBPathSmoother> getElasticBandSmoother() const { return eb_path_smoother_ptr_; }

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class PathSampler : public rclcpp::Node
public:
explicit PathSampler(const rclcpp::NodeOptions & node_options);

protected: // for the static_centerline_optimizer package
protected: // for the static_centerline_generator package
// argument variables
vehicle_info_util::VehicleInfo vehicle_info_{};
mutable DebugData debug_data_{};
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.14)
project(static_centerline_optimizer)
project(static_centerline_generator)

find_package(autoware_cmake REQUIRED)

Expand All @@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)

rosidl_generate_interfaces(
static_centerline_optimizer
static_centerline_generator
"srv/LoadMap.srv"
"srv/PlanRoute.srv"
"srv/PlanPath.srv"
Expand All @@ -21,18 +21,18 @@ autoware_package()

ament_auto_add_executable(main
src/main.cpp
src/static_centerline_optimizer_node.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
)

if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
rosidl_target_interfaces(main
static_centerline_optimizer "rosidl_typesupport_cpp")
static_centerline_generator "rosidl_typesupport_cpp")
else()
rosidl_get_typesupport_target(
cpp_typesupport_target static_centerline_optimizer "rosidl_typesupport_cpp")
cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp")
target_link_libraries(main "${cpp_typesupport_target}")
endif()

Expand All @@ -45,7 +45,7 @@ ament_auto_package(

if(BUILD_TESTING)
add_launch_test(
test/test_static_centerline_optimizer.test.py
test/test_static_centerline_generator.test.py
TIMEOUT "30"
)
install(DIRECTORY
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# Static Centerline Optimizer
# Static Centerline Generator

## Purpose

Expand Down Expand Up @@ -34,7 +34,7 @@ We can run
with the following command by designating `<vehicle_model>`

```sh
ros2 launch static_centerline_optimizer 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.
Expand All @@ -50,7 +50,7 @@ The optimized centerline can be generated from the command line interface by des
- `<vehicle-model>`

```sh
ros2 launch static_centerline_optimizer static_centerline_optimizer.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`.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
#define STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_

#include "rclcpp/rclcpp.hpp"
#include "static_centerline_optimizer/type_alias.hpp"
#include "static_centerline_generator/type_alias.hpp"

#include <vector>

namespace static_centerline_optimizer
namespace static_centerline_generator
{
std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node);
} // namespace static_centerline_optimizer
#endif // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
} // namespace static_centerline_generator
#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT
#define STATIC_CENTERLINE_OPTIMIZER__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 "static_centerline_optimizer/type_alias.hpp"
#include "static_centerline_generator/type_alias.hpp"

#include <vector>

namespace static_centerline_optimizer
namespace static_centerline_generator
{
class OptimizationTrajectoryBasedCenterline
{
Expand All @@ -37,7 +37,7 @@ class OptimizationTrajectoryBasedCenterline
rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
};
} // namespace static_centerline_optimizer
} // namespace static_centerline_generator
// clang-format off
#endif // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT
#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT
// clang-format on
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
#define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
#ifndef STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_
#define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_

#include "rclcpp/rclcpp.hpp"
#include "static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp"
#include "static_centerline_optimizer/srv/load_map.hpp"
#include "static_centerline_optimizer/srv/plan_path.hpp"
#include "static_centerline_optimizer/srv/plan_route.hpp"
#include "static_centerline_optimizer/type_alias.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"

#include <geography_utils/lanelet2_projector.hpp>
Expand All @@ -34,22 +34,22 @@
#include <utility>
#include <vector>

namespace static_centerline_optimizer
namespace static_centerline_generator
{
using static_centerline_optimizer::srv::LoadMap;
using static_centerline_optimizer::srv::PlanPath;
using static_centerline_optimizer::srv::PlanRoute;
using static_centerline_generator::srv::LoadMap;
using static_centerline_generator::srv::PlanPath;
using static_centerline_generator::srv::PlanRoute;

struct CenterlineWithRoute
{
std::vector<TrajectoryPoint> centerline{};
std::vector<lanelet::Id> route_lane_ids{};
};

class StaticCenterlineOptimizerNode : public rclcpp::Node
class StaticCenterlineGeneratorNode : public rclcpp::Node
{
public:
explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options);
explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options);
void run();

private:
Expand Down Expand Up @@ -115,5 +115,5 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
// vehicle info
vehicle_info_util::VehicleInfo vehicle_info_;
};
} // namespace static_centerline_optimizer
#endif // STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
} // namespace static_centerline_generator
#endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -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_OPTIMIZER__TYPE_ALIAS_HPP_
#define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_
#ifndef STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
#define STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_

#include "route_handler/route_handler.hpp"
#include "tier4_autoware_utils/geometry/geometry.hpp"
Expand All @@ -26,7 +26,7 @@
#include "autoware_planning_msgs/msg/lanelet_route.hpp"
#include "visualization_msgs/msg/marker_array.hpp"

namespace static_centerline_optimizer
namespace static_centerline_generator
{
using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_perception_msgs::msg::PredictedObjects;
Expand All @@ -41,6 +41,6 @@ using tier4_autoware_utils::LinearRing2d;
using tier4_autoware_utils::LineString2d;
using tier4_autoware_utils::Point2d;
using visualization_msgs::msg::MarkerArray;
} // namespace static_centerline_optimizer
} // namespace static_centerline_generator

#endif // STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_
#endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,19 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_
#define STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_
#ifndef STATIC_CENTERLINE_GENERATOR__UTILS_HPP_
#define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_

#include "route_handler/route_handler.hpp"
#include "static_centerline_optimizer/type_alias.hpp"
#include "static_centerline_generator/type_alias.hpp"

#include <rclcpp/time.hpp>

#include <memory>
#include <string>
#include <vector>

namespace static_centerline_optimizer
namespace static_centerline_generator
{
namespace utils
{
Expand Down Expand Up @@ -53,6 +53,6 @@ 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_optimizer
} // namespace static_centerline_generator

#endif // STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_
#endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,11 @@
<!-- mandatory arguments for planning-->
<arg name="vehicle_model"/>

<include file="$(find-pkg-share static_centerline_optimizer)/launch/static_centerline_optimizer.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="static_centerline_optimizer" exec="app.py" name="static_centerline_optimizer_http_server" output="screen"/>
<node pkg="static_centerline_generator" exec="app.py" name="static_centerline_generator_http_server" output="screen"/>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@
</node>

<!-- optimize path -->
<node pkg="static_centerline_optimizer" exec="main" name="static_centerline_optimizer">
<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"/>
Expand All @@ -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_optimizer)/config/static_centerline_optimizer.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 static_centerline_optimizer)/rviz/static_centerline_optimizer.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>
Original file line number Diff line number Diff line change
@@ -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_optimizer</name>
<name>static_centerline_generator</name>
<version>0.1.0</version>
<description>The static_centerline_optimizer 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>
Expand Down
Loading

0 comments on commit fd032e8

Please sign in to comment.