Skip to content

Commit 3b3214f

Browse files
committed
refactor(behavior_velocity_planner_common): move VelocityFactorInterface to motion_utils (autowarefoundation#7007)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent afc4fd4 commit 3b3214f

File tree

7 files changed

+27
-34
lines changed

7 files changed

+27
-34
lines changed

common/motion_utils/CMakeLists.txt

+1-9
Original file line numberDiff line numberDiff line change
@@ -7,15 +7,7 @@ autoware_package()
77
find_package(Boost REQUIRED)
88

99
ament_auto_add_library(motion_utils SHARED
10-
src/distance/distance.cpp
11-
src/marker/marker_helper.cpp
12-
src/marker/virtual_wall_marker_creator.cpp
13-
src/resample/resample.cpp
14-
src/trajectory/trajectory.cpp
15-
src/trajectory/interpolation.cpp
16-
src/trajectory/path_with_lane_id.cpp
17-
src/trajectory/tmp_conversion.cpp
18-
src/vehicle/vehicle_state_checker.cpp
10+
DIRECTORY src
1911
)
2012

2113
if(BUILD_TESTING)
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11

2-
// Copyright 2022 TIER IV, Inc.
2+
// Copyright 2022-2024 TIER IV, Inc.
33
//
44
// Licensed under the Apache License, Version 2.0 (the "License");
55
// you may not use this file except in compliance with the License.
@@ -13,8 +13,8 @@
1313
// See the License for the specific language governing permissions and
1414
// limitations under the License.
1515

16-
#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
17-
#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
16+
#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
17+
#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
1818

1919
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
2020
#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
@@ -25,35 +25,31 @@
2525
#include <string>
2626
#include <vector>
2727

28-
namespace behavior_velocity_planner
28+
namespace motion_utils
2929
{
30-
3130
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
3231
using autoware_adapi_v1_msgs::msg::VelocityFactor;
33-
using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
34-
using geometry_msgs::msg::Pose;
3532
using VelocityFactorBehavior = VelocityFactor::_behavior_type;
3633
using VelocityFactorStatus = VelocityFactor::_status_type;
34+
using geometry_msgs::msg::Pose;
3735

3836
class VelocityFactorInterface
3937
{
4038
public:
41-
VelocityFactorInterface() { behavior_ = VelocityFactor::UNKNOWN; }
42-
43-
VelocityFactor get() const { return velocity_factor_; }
44-
void init(const VelocityFactorBehavior behavior) { behavior_ = behavior; }
39+
[[nodiscard]] VelocityFactor get() const { return velocity_factor_; }
40+
void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
4541
void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
4642

4743
void set(
4844
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
4945
const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
50-
const std::string detail = "");
46+
const std::string & detail = "");
5147

5248
private:
53-
VelocityFactorBehavior behavior_;
54-
VelocityFactor velocity_factor_;
49+
VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
50+
VelocityFactor velocity_factor_{};
5551
};
5652

57-
} // namespace behavior_velocity_planner
53+
} // namespace motion_utils
5854

59-
#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
55+
#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_

common/motion_utils/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
<buildtool_depend>ament_cmake_auto</buildtool_depend>
2222
<buildtool_depend>autoware_cmake</buildtool_depend>
2323

24+
<depend>autoware_adapi_v1_msgs</depend>
2425
<depend>autoware_auto_planning_msgs</depend>
2526
<depend>autoware_auto_vehicle_msgs</depend>
2627
<depend>builtin_interfaces</depend>
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 TIER IV, Inc.
1+
// Copyright 2023-2024 TIER IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,23 +12,24 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
15+
#include <motion_utils/factor/velocity_factor_interface.hpp>
1616
#include <motion_utils/trajectory/trajectory.hpp>
1717

18-
namespace behavior_velocity_planner
18+
namespace motion_utils
1919
{
2020
void VelocityFactorInterface::set(
2121
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
2222
const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
23-
const std::string detail)
23+
const std::string & detail)
2424
{
2525
const auto & curr_point = curr_pose.position;
2626
const auto & stop_point = stop_pose.position;
2727
velocity_factor_.behavior = behavior_;
2828
velocity_factor_.pose = stop_pose;
29-
velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
29+
velocity_factor_.distance =
30+
static_cast<float>(motion_utils::calcSignedArcLength(points, curr_point, stop_point));
3031
velocity_factor_.status = status;
3132
velocity_factor_.detail = detail;
3233
}
3334

34-
} // namespace behavior_velocity_planner
35+
} // namespace motion_utils

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
2020
#include <behavior_velocity_planner_common/utilization/util.hpp>
2121
#include <lanelet2_extension/utility/utilities.hpp>
22+
#include <motion_utils/factor/velocity_factor_interface.hpp>
2223
#include <motion_utils/trajectory/trajectory.hpp>
2324
#include <opencv2/imgproc.hpp>
2425
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
@@ -91,6 +92,7 @@ static bool isTargetCollisionVehicleType(
9192
}
9293
return false;
9394
}
95+
using motion_utils::VelocityFactorInterface;
9496

9597
IntersectionModule::IntersectionModule(
9698
const int64_t module_id, const int64_t lane_id,

planning/behavior_velocity_planner_common/CMakeLists.txt

-1
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@ autoware_package()
66

77
ament_auto_add_library(${PROJECT_NAME} SHARED
88
src/scene_module_interface.cpp
9-
src/velocity_factor_interface.cpp
109
src/utilization/path_utilization.cpp
1110
src/utilization/trajectory_utils.cpp
1211
src/utilization/arc_lane_util.cpp

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@
1616
#define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_
1717

1818
#include <behavior_velocity_planner_common/planner_data.hpp>
19-
#include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
2019
#include <builtin_interfaces/msg/time.hpp>
20+
#include <motion_utils/factor/velocity_factor_interface.hpp>
2121
#include <motion_utils/marker/virtual_wall_marker_creator.hpp>
2222
#include <objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
2323
#include <rtc_interface/rtc_interface.hpp>
@@ -48,6 +48,8 @@ namespace behavior_velocity_planner
4848

4949
using autoware_auto_planning_msgs::msg::PathWithLaneId;
5050
using builtin_interfaces::msg::Time;
51+
using motion_utils::PlanningBehavior;
52+
using motion_utils::VelocityFactor;
5153
using objects_of_interest_marker_interface::ColorName;
5254
using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
5355
using rtc_interface::RTCInterface;
@@ -124,7 +126,7 @@ class SceneModuleInterface
124126
std::shared_ptr<const PlannerData> planner_data_;
125127
std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
126128
std::optional<int> first_stop_path_point_index_;
127-
VelocityFactorInterface velocity_factor_;
129+
motion_utils::VelocityFactorInterface velocity_factor_;
128130
std::vector<ObjectOfInterest> objects_of_interest_;
129131

130132
void setSafe(const bool safe)

0 commit comments

Comments
 (0)