Skip to content

Commit

Permalink
feat: apply autoware_ prefix for predicted_path_checker (#9985)
Browse files Browse the repository at this point in the history
* feat(predicted_path_checker): apply `autoware_` prefix (see below):

  Note:
    * In this commit, I did not organize a folder structure.
      The folder structure will be organized in the next some commits.
    * The changes will follow the Autoware's guideline as below:
        - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder

Signed-off-by: Junya Sasaki <[email protected]>

* rename(predicted_path_checker): move headers under `include/autoware`

  * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit

Signed-off-by: Junya Sasaki <[email protected]>

* fix(predicted_path_checker): fix include header paths

  * To follow the previous commit

Signed-off-by: Junya Sasaki <[email protected]>

* rename: `predicted_path_checker` => `autoware_predicted_path_checker`

Signed-off-by: Junya Sasaki <[email protected]>

* style(pre-commit): autofix

* bug(autoware_predicted_path_checker): fix inconsistent namespacings

Signed-off-by: Junya Sasaki <[email protected]>

* bug(autoware_predicted_path_checker): do not change node name

  * This might contaminate topic name

Signed-off-by: Junya Sasaki <[email protected]>

* style(pre-commit): autofix

* bug(tier4_control_launch): fix wrong package/plugin names

Signed-off-by: Junya Sasaki <[email protected]>

---------

Signed-off-by: Junya Sasaki <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
sasakisasaki and pre-commit-ci[bot] authored Jan 23, 2025
1 parent bca9030 commit 47d55f4
Show file tree
Hide file tree
Showing 18 changed files with 64 additions and 65 deletions.
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.14)
project(predicted_path_checker)
project(autoware_predicted_path_checker)

find_package(autoware_cmake REQUIRED)
autoware_package()
Expand All @@ -12,17 +12,17 @@ include_directories(
${Eigen3_INCLUDE_DIRS}
)

ament_auto_add_library(predicted_path_checker SHARED
ament_auto_add_library(${PROJECT_NAME} SHARED
src/predicted_path_checker_node/predicted_path_checker_node.cpp
src/predicted_path_checker_node/collision_checker.cpp
src/predicted_path_checker_node/utils.cpp
src/predicted_path_checker_node/debug_marker.cpp

)

rclcpp_components_register_node(predicted_path_checker
PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode"
EXECUTABLE predicted_path_checker_node
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "autoware::predicted_path_checker::PredictedPathCheckerNode"
EXECUTABLE ${PROJECT_NAME}_node
)

if(BUILD_TESTING)
Expand Down
File renamed without changes.
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
#define AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_

#include <autoware/motion_utils/trajectory/conversion.hpp>
#include <autoware/motion_utils/trajectory/interpolation.hpp>
#include <autoware/predicted_path_checker/debug_marker.hpp>
#include <autoware/predicted_path_checker/utils.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/debug_publisher.hpp>
#include <autoware/universe_utils/ros/transform_listener.hpp>
#include <autoware_vehicle_info_utils/vehicle_info.hpp>
#include <predicted_path_checker/debug_marker.hpp>
#include <predicted_path_checker/utils.hpp>
#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>
Expand All @@ -40,7 +40,7 @@
#include <utility>
#include <vector>

namespace autoware::motion::control::predicted_path_checker
namespace autoware::predicted_path_checker
{
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;
Expand Down Expand Up @@ -123,6 +123,6 @@ class CollisionChecker
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
std::vector<PredictedObjectWithDetectionTime> predicted_object_history_{};
};
} // namespace autoware::motion::control::predicted_path_checker
} // namespace autoware::predicted_path_checker

#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
#endif // AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
#define AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_

#include <rclcpp/rclcpp.hpp>

Expand All @@ -35,7 +35,7 @@
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

namespace autoware::motion::control::predicted_path_checker
namespace autoware::predicted_path_checker
{

enum class PolygonType : int8_t { Vehicle = 0, Collision };
Expand Down Expand Up @@ -87,6 +87,6 @@ class PredictedPathCheckerDebugNode
std::vector<std::vector<Eigen::Vector3d>> collision_polyhedrons_;
};

} // namespace autoware::motion::control::predicted_path_checker
} // namespace autoware::predicted_path_checker

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

#ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
#define AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_

#include <autoware/component_interface_specs_universe/control.hpp>
#include <autoware/component_interface_utils/rclcpp.hpp>
#include <autoware/motion_utils/trajectory/conversion.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/predicted_path_checker/collision_checker.hpp>
#include <autoware/predicted_path_checker/utils.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/self_pose_listener.hpp>
#include <autoware_vehicle_info_utils/vehicle_info.hpp>
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <predicted_path_checker/collision_checker.hpp>
#include <predicted_path_checker/utils.hpp>

#include <autoware_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_planning_msgs/msg/trajectory.hpp>
Expand All @@ -45,7 +45,7 @@
#include <utility>
#include <vector>

namespace autoware::motion::control::predicted_path_checker
namespace autoware::predicted_path_checker
{
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;
Expand Down Expand Up @@ -178,6 +178,6 @@ class PredictedPathCheckerNode : public rclcpp::Node
// Diagnostic Updater
diagnostic_updater::Updater updater_;
};
} // namespace autoware::motion::control::predicted_path_checker
} // namespace autoware::predicted_path_checker

#endif // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
#endif // AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_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 PREDICTED_PATH_CHECKER__UTILS_HPP_
#define PREDICTED_PATH_CHECKER__UTILS_HPP_
#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_
#define AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_

#include <autoware/interpolation/linear_interpolation.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
Expand All @@ -36,7 +36,7 @@
#include <utility>
#include <vector>

namespace utils
namespace autoware::predicted_path_checker
{

using autoware::universe_utils::Point2d;
Expand Down Expand Up @@ -99,6 +99,6 @@ void getCurrentObjectPose(
const rclcpp::Time & current_time);

bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos);
} // namespace utils
} // namespace autoware::predicted_path_checker

#endif // PREDICTED_PATH_CHECKER__UTILS_HPP_
#endif // AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,11 @@
<arg name="input/odometry" default="/localization/kinematic_state"/>
<arg name="input/current_accel" default="/localization/acceleration"/>

<arg name="config_file" default="$(find-pkg-share predicted_path_checker)/config/predicted_path_checker.param.yaml"/>
<arg name="config_file" default="$(find-pkg-share autoware_predicted_path_checker)/config/predicted_path_checker.param.yaml"/>
<!-- vehicle info -->
<arg name="vehicle_info_param_file" default="$(find-pkg-share autoware_vehicle_info_utils)/config/vehicle_info.param.yaml"/>

<node pkg="predicted_path_checker" exec="predicted_path_checker_node" name="predicted_path_checker_node" output="screen">
<node pkg="autoware_predicted_path_checker" exec="autoware_predicted_path_checker_node" name="predicted_path_checker_node" output="screen">
<param from="$(var config_file)"/>
<param from="$(var vehicle_info_param_file)"/>
<remap from="~/input/objects" to="$(var input/objects)"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
<?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>predicted_path_checker</name>
<name>autoware_predicted_path_checker</name>
<version>0.40.0</version>
<description>The predicted_path_checker package</description>

<maintainer email="[email protected]">Berkay Karaman</maintainer>
<maintainer email="[email protected]">Junya Sasaki</maintainer>
<license>Apache 2.0</license>
<author email="[email protected]">Berkay Karaman</author>

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

#include "predicted_path_checker/collision_checker.hpp"
#include "autoware/predicted_path_checker/collision_checker.hpp"

#include <autoware/universe_utils/ros/marker_helper.hpp>
#include <rclcpp/logging.hpp>
Expand All @@ -22,7 +22,7 @@
#include <utility>
#include <vector>

namespace autoware::motion::control::predicted_path_checker
namespace autoware::predicted_path_checker
{
CollisionChecker::CollisionChecker(
rclcpp::Node * node, std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr)
Expand Down Expand Up @@ -61,7 +61,7 @@ CollisionChecker::checkTrajectoryForCollision(

// create one-step polygon for vehicle
Polygon2d one_step_move_vehicle_polygon2d =
utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin);
createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin);
if (param_.enable_z_axis_obstacle_filtering) {
debug_ptr_->pushPolyhedron(
one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle);
Expand Down Expand Up @@ -126,7 +126,7 @@ CollisionChecker::checkObstacleHistory(
std::vector<std::pair<geometry_msgs::msg::Point, PredictedObject>> collision_points_in_history;
for (const auto & obj_history : predicted_object_history_) {
if (param_.enable_z_axis_obstacle_filtering) {
if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) {
if (!intersectsInZAxis(obj_history.object, z_min, z_max)) {
continue;
}
}
Expand Down Expand Up @@ -169,11 +169,11 @@ CollisionChecker::checkDynamicObjects(
for (size_t i = 0; i < dynamic_objects->objects.size(); ++i) {
const auto & obj = dynamic_objects->objects.at(i);
if (param_.enable_z_axis_obstacle_filtering) {
if (!utils::intersectsInZAxis(obj, z_min, z_max)) {
if (!intersectsInZAxis(obj, z_min, z_max)) {
continue;
}
}
const auto object_polygon = utils::convertObjToPolygon(obj);
const auto object_polygon = convertObjToPolygon(obj);
if (object_polygon.outer().empty()) {
// unsupported type
continue;
Expand Down Expand Up @@ -205,7 +205,7 @@ CollisionChecker::checkDynamicObjects(
}
geometry_msgs::msg::Point nearest_collision_point_tmp;

double norm = utils::getNearestPointAndDistanceForPredictedObject(
double norm = getNearestPointAndDistanceForPredictedObject(
collision_point_array, base_pose, &nearest_collision_point_tmp);
if (norm < min_norm_collision_norm || !is_init) {
min_norm_collision_norm = norm;
Expand All @@ -217,7 +217,7 @@ CollisionChecker::checkDynamicObjects(
}
if (is_init) {
const auto & obj = dynamic_objects->objects.at(nearest_collision_object_index);
const auto obstacle_polygon = utils::convertObjToPolygon(obj);
const auto obstacle_polygon = convertObjToPolygon(obj);
if (param_.enable_z_axis_obstacle_filtering) {
debug_ptr_->pushPolyhedron(obstacle_polygon, z_min, z_max, PolygonType::Collision);
} else {
Expand All @@ -229,4 +229,4 @@ CollisionChecker::checkDynamicObjects(
}
return boost::none;
}
} // namespace autoware::motion::control::predicted_path_checker
} // namespace autoware::predicted_path_checker
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "predicted_path_checker/debug_marker.hpp"
#include "autoware/predicted_path_checker/debug_marker.hpp"

#include <autoware/motion_utils/marker/marker_helper.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
Expand All @@ -38,7 +38,7 @@ using autoware::universe_utils::createMarkerOrientation;
using autoware::universe_utils::createMarkerScale;
using autoware::universe_utils::createPoint;

namespace autoware::motion::control::predicted_path_checker
namespace autoware::predicted_path_checker
{
PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode(
rclcpp::Node * node, const double base_link2front)
Expand Down Expand Up @@ -326,4 +326,4 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz
return msg;
}

} // namespace autoware::motion::control::predicted_path_checker
} // namespace autoware::predicted_path_checker
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "predicted_path_checker/predicted_path_checker_node.hpp"
#include "autoware/predicted_path_checker/predicted_path_checker_node.hpp"

#include <autoware/motion_utils/marker/marker_helper.hpp>
#include <autoware/motion_utils/resample/resample.hpp>
Expand All @@ -26,7 +26,7 @@
#include <utility>
#include <vector>

namespace autoware::motion::control::predicted_path_checker
namespace autoware::predicted_path_checker
{

PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options)
Expand Down Expand Up @@ -301,7 +301,7 @@ void PredictedPathCheckerNode::onTimer()

// Check if the vehicle is in the brake distance

const bool is_in_brake_distance = utils::isInBrakeDistance(
const bool is_in_brake_distance = isInBrakeDistance(
predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration,
node_param_.max_deceleration, node_param_.delay_time);

Expand Down Expand Up @@ -431,7 +431,7 @@ bool PredictedPathCheckerNode::isItDiscretePoint(
const auto nearest_segment =
autoware::motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose);

const auto nearest_point = utils::calcInterpolatedPoint(
const auto nearest_point = calcInterpolatedPoint(
reference_trajectory, collision_point.pose.position, *nearest_segment, false);

const auto distance = autoware::universe_utils::calcDistance2d(
Expand Down Expand Up @@ -495,7 +495,7 @@ void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & tr
// collision_point.
const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin;
const auto & goal_point = trajectory.back();
const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point);
const auto trajectory_point_extend = getExtendTrajectoryPoint(extend_distance, goal_point);
trajectory.push_back(trajectory_point_extend);
}

Expand All @@ -506,14 +506,14 @@ size_t PredictedPathCheckerNode::insertStopPoint(
autoware::motion_utils::findNearestSegmentIndex(trajectory, collision_point);

const auto nearest_collision_point =
utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true);
calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true);

const size_t collision_idx = nearest_collision_segment + 1;

trajectory.insert(trajectory.begin() + static_cast<int>(collision_idx), nearest_collision_point);

const auto stop_point =
utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_);
findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_);

const size_t stop_idx = stop_point.first + 1;
trajectory.insert(trajectory.begin() + static_cast<int>(stop_idx), stop_point.second);
Expand Down Expand Up @@ -550,13 +550,12 @@ void PredictedPathCheckerNode::filterObstacles(

for (auto & object : object_ptr_.get()->objects) {
// Check is it in front of ego vehicle
if (!utils::isFrontObstacle(
ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) {
if (!isFrontObstacle(ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) {
continue;
}

// Check is it near to trajectory
const double max_length = utils::calcObstacleMaxLength(object.shape);
const double max_length = calcObstacleMaxLength(object.shape);
const size_t seg_idx = autoware::motion_utils::findNearestSegmentIndex(
traj, object.kinematics.initial_pose_with_covariance.pose.position);
const auto p_front = autoware::universe_utils::getPoint(traj.at(seg_idx));
Expand All @@ -580,14 +579,13 @@ void PredictedPathCheckerNode::filterObstacles(
}
PredictedObject filtered_object = object;
if (use_prediction) {
utils::getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now());
getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now());
}
filtered_objects.objects.push_back(filtered_object);
}
}

} // namespace autoware::motion::control::predicted_path_checker
} // namespace autoware::predicted_path_checker

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(
autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode)
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::predicted_path_checker::PredictedPathCheckerNode)
Loading

0 comments on commit 47d55f4

Please sign in to comment.