Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(path_smoother)!: prefix package and namespace with autoware #7381

Merged
Merged
Show file tree
Hide file tree
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
<group>
<group if="$(eval &quot;'$(var motion_path_smoother_type)' == 'elastic_band'&quot;)">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
<composable_node pkg="path_smoother" plugin="path_smoother::ElasticBandSmoother" name="elastic_band_smoother" namespace="">
<composable_node pkg="autoware_path_smoother" plugin="autoware::path_smoother::ElasticBandSmoother" name="elastic_band_smoother" namespace="">
<!-- topic remap -->
<remap from="~/input/path" to="$(var interface_input_topic)"/>
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,23 +1,23 @@
cmake_minimum_required(VERSION 3.14)
project(path_smoother)
project(autoware_path_smoother)

find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(Eigen3 REQUIRED)

ament_auto_add_library(path_smoother SHARED
ament_auto_add_library(autoware_path_smoother SHARED
DIRECTORY src
)

target_include_directories(path_smoother
target_include_directories(autoware_path_smoother
SYSTEM PUBLIC
${EIGEN3_INCLUDE_DIR}
)

# register node
rclcpp_components_register_node(path_smoother
PLUGIN "path_smoother::ElasticBandSmoother"
rclcpp_components_register_node(autoware_path_smoother
PLUGIN "autoware::path_smoother::ElasticBandSmoother"
EXECUTABLE elastic_band_smoother
)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PATH_SMOOTHER__COMMON_STRUCTS_HPP_
#define PATH_SMOOTHER__COMMON_STRUCTS_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_
#define AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_

#include "path_smoother/type_alias.hpp"
#include "autoware_path_smoother/type_alias.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"
Expand All @@ -25,7 +25,7 @@
#include <string>
#include <vector>

namespace path_smoother
namespace autoware::path_smoother
{
struct Bounds;

Expand Down Expand Up @@ -131,6 +131,6 @@ struct EgoNearestParam
double dist_threshold{0.0};
double yaw_threshold{0.0};
};
} // namespace path_smoother
} // namespace autoware::path_smoother

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

#ifndef PATH_SMOOTHER__ELASTIC_BAND_HPP_
#define PATH_SMOOTHER__ELASTIC_BAND_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_
#define AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_

#include "autoware_path_smoother/common_structs.hpp"
#include "autoware_path_smoother/type_alias.hpp"
#include "osqp_interface/osqp_interface.hpp"
#include "path_smoother/common_structs.hpp"
#include "path_smoother/type_alias.hpp"

#include <Eigen/Core>

Expand All @@ -27,7 +27,7 @@
#include <utility>
#include <vector>

namespace path_smoother
namespace autoware::path_smoother
{
class EBPathSmoother
{
Expand Down Expand Up @@ -128,6 +128,6 @@ class EBPathSmoother
const std::vector<double> & optimized_points, const std::vector<TrajectoryPoint> & traj_points,
const int pad_start_idx) const;
};
} // namespace path_smoother
} // namespace autoware::path_smoother

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

#ifndef PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_
#define PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_
#define AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_

#include "autoware_path_smoother/common_structs.hpp"
#include "autoware_path_smoother/elastic_band.hpp"
#include "autoware_path_smoother/replan_checker.hpp"
#include "autoware_path_smoother/type_alias.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
#include "path_smoother/common_structs.hpp"
#include "path_smoother/elastic_band.hpp"
#include "path_smoother/replan_checker.hpp"
#include "path_smoother/type_alias.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
Expand All @@ -32,7 +32,7 @@
#include <string>
#include <vector>

namespace path_smoother
namespace autoware::path_smoother
{
class ElasticBandSmoother : public rclcpp::Node
{
Expand All @@ -41,7 +41,10 @@ class ElasticBandSmoother : public rclcpp::Node

// 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_; }
std::shared_ptr<EBPathSmoother> getElasticBandSmoother() const
{
return eb_autoware_path_smoother_ptr_;
}

private:
class DrivingDirectionChecker
Expand All @@ -66,7 +69,7 @@ class ElasticBandSmoother : public rclcpp::Node
bool enable_debug_info_;

// algorithms
std::shared_ptr<EBPathSmoother> eb_path_smoother_ptr_{nullptr};
std::shared_ptr<EBPathSmoother> eb_autoware_path_smoother_ptr_{nullptr};
std::shared_ptr<ReplanChecker> replan_checker_ptr_{nullptr};

// parameters
Expand Down Expand Up @@ -116,6 +119,6 @@ class ElasticBandSmoother : public rclcpp::Node

std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
};
} // namespace path_smoother
} // namespace autoware::path_smoother

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

#ifndef PATH_SMOOTHER__REPLAN_CHECKER_HPP_
#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_
#define AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_

#include "path_smoother/common_structs.hpp"
#include "path_smoother/type_alias.hpp"
#include "autoware_path_smoother/common_structs.hpp"
#include "autoware_path_smoother/type_alias.hpp"

#include <rclcpp/rclcpp.hpp>

#include <memory>
#include <vector>

namespace path_smoother
namespace autoware::path_smoother
{
class ReplanChecker
{
Expand Down Expand Up @@ -66,6 +66,6 @@ class ReplanChecker
bool isPathGoalChanged(
const PlannerData & planner_data, const std::vector<TrajectoryPoint> & prev_traj_points) const;
};
} // namespace path_smoother
} // namespace autoware::path_smoother

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

#ifndef PATH_SMOOTHER__TYPE_ALIAS_HPP_
#define PATH_SMOOTHER__TYPE_ALIAS_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_
#define AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_

#include "autoware_planning_msgs/msg/path.hpp"
#include "autoware_planning_msgs/msg/path_point.hpp"
Expand All @@ -25,7 +25,7 @@
#include "tier4_debug_msgs/msg/float64_stamped.hpp"
#include "tier4_debug_msgs/msg/string_stamped.hpp"

namespace path_smoother
namespace autoware::path_smoother
{
// std_msgs
using std_msgs::msg::Header;
Expand All @@ -39,6 +39,6 @@ using nav_msgs::msg::Odometry;
// debug
using tier4_debug_msgs::msg::Float64Stamped;
using tier4_debug_msgs::msg::StringStamped;
} // namespace path_smoother
} // namespace autoware::path_smoother

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

#ifndef PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_
#define PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_
#define AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_

#include <tier4_autoware_utils/geometry/geometry.hpp>

namespace path_smoother
namespace autoware::path_smoother
{
namespace geometry_utils
{
Expand All @@ -31,5 +31,5 @@ bool isSamePoint(const T1 & t1, const T2 & t2)
return (std::abs(p1.x - p2.x) <= epsilon && std::abs(p1.y - p2.y) <= epsilon);
}
} // namespace geometry_utils
} // namespace path_smoother
#endif // PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_
} // namespace autoware::path_smoother
#endif // AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_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 PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_
#define PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_
#ifndef AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_
#define AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_

#include "autoware_path_smoother/common_structs.hpp"
#include "autoware_path_smoother/type_alias.hpp"
#include "interpolation/linear_interpolation.hpp"
#include "interpolation/spline_interpolation.hpp"
#include "interpolation/spline_interpolation_points_2d.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
#include "path_smoother/common_structs.hpp"
#include "path_smoother/type_alias.hpp"

#include <Eigen/Core>

Expand All @@ -35,7 +35,7 @@
#include <string>
#include <vector>

namespace path_smoother
namespace autoware::path_smoother
{
namespace trajectory_utils
{
Expand Down Expand Up @@ -137,7 +137,7 @@ std::optional<size_t> updateFrontPointForFix(
motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position);
if (0 < lon_offset_to_prev_front) {
RCLCPP_DEBUG(
rclcpp::get_logger("path_smoother.trajectory_utils"),
rclcpp::get_logger("autoware_path_smoother.trajectory_utils"),
"Fixed point will not be inserted due to the error during calculation.");
return std::nullopt;
}
Expand All @@ -148,7 +148,7 @@ std::optional<size_t> updateFrontPointForFix(
constexpr double max_lat_error = 3.0;
if (max_lat_error < dist) {
RCLCPP_DEBUG(
rclcpp::get_logger("path_smoother.trajectory_utils"),
rclcpp::get_logger("autoware_path_smoother.trajectory_utils"),
"New Fixed point is too far from points %f [m]", dist);
}

Expand All @@ -170,5 +170,5 @@ void insertStopPoint(
std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & input_stop_pose,
const size_t stop_seg_idx);
} // namespace trajectory_utils
} // namespace path_smoother
#endif // PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_
} // namespace autoware::path_smoother
#endif // AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,9 @@
<arg name="output_path_topic" default="/planning/scenario_planning/lane_driving/path_smoother/path"/>
<arg name="output_trajectory_topic" default="/planning/scenario_planning/lane_driving/path_smoother/trajectory"/>
<arg name="enable_debug_info" default="false"/>
<arg name="param_path" default="$(find-pkg-share path_smoother)/config/elastic_band_smoother.param.yaml"/>
<arg name="param_path" default="$(find-pkg-share autoware_path_smoother)/config/elastic_band_smoother.param.yaml"/>

<node pkg="path_smoother" exec="elastic_band_smoother" name="elastic_band_smoother" output="screen">
<node pkg="autoware_path_smoother" exec="elastic_band_smoother" name="elastic_band_smoother" output="screen">
<remap from="~/input/path" to="$(var input_path_topic)"/>
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
<remap from="~/output/traj" to="$(var output_trajectory_topic)"/>
Expand Down
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>path_smoother</name>
<name>autoware_path_smoother</name>
<version>0.1.0</version>
<description>The path_smoother package</description>
<description>The autoware_path_smoother package</description>
<maintainer email="[email protected]">Takayuki Murooka</maintainer>
<maintainer email="[email protected]">Maxime CLEMENT</maintainer>
<license>Apache License 2.0</license>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "path_smoother/elastic_band.hpp"
#include "autoware_path_smoother/elastic_band.hpp"

#include "autoware_path_smoother/type_alias.hpp"
#include "autoware_path_smoother/utils/geometry_utils.hpp"
#include "autoware_path_smoother/utils/trajectory_utils.hpp"
#include "motion_utils/trajectory/conversion.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
#include "path_smoother/type_alias.hpp"
#include "path_smoother/utils/geometry_utils.hpp"
#include "path_smoother/utils/trajectory_utils.hpp"
#include "tf2/utils.h"

#include <Eigen/Core>
Expand Down Expand Up @@ -84,7 +84,7 @@ std_msgs::msg::Header createHeader(const rclcpp::Time & now)
}
} // namespace

namespace path_smoother
namespace autoware::path_smoother
{
EBPathSmoother::EBParam::EBParam(rclcpp::Node * node)
{
Expand Down Expand Up @@ -453,4 +453,4 @@ std::optional<std::vector<TrajectoryPoint>> EBPathSmoother::convertOptimizedPoin
time_keeper_ptr_->toc(__func__, " ");
return eb_traj_points;
}
} // namespace path_smoother
} // namespace autoware::path_smoother
Loading
Loading