Skip to content

Commit 067ee7a

Browse files
satoshi-otasatoshi-ota
authored
feat(motion_utils): add planning factor interface (#9676)
* feat(motion_utils): add planning factor interface Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix: use extern template Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix: define function in header Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 6ed5634 commit 067ee7a

File tree

2 files changed

+287
-0
lines changed

2 files changed

+287
-0
lines changed

‎common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp

+240
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,240 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_
16+
#define AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_
17+
18+
#include <autoware/motion_utils/trajectory/trajectory.hpp>
19+
#include <rclcpp/rclcpp.hpp>
20+
21+
#include <autoware_planning_msgs/msg/path_point.hpp>
22+
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
23+
#include <geometry_msgs/msg/pose.hpp>
24+
#include <tier4_planning_msgs/msg/control_point.hpp>
25+
#include <tier4_planning_msgs/msg/path_point_with_lane_id.hpp>
26+
#include <tier4_planning_msgs/msg/planning_factor.hpp>
27+
#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
28+
#include <tier4_planning_msgs/msg/safety_factor_array.hpp>
29+
30+
#include <string>
31+
#include <vector>
32+
33+
namespace autoware::motion_utils
34+
{
35+
36+
using geometry_msgs::msg::Pose;
37+
using tier4_planning_msgs::msg::ControlPoint;
38+
using tier4_planning_msgs::msg::PlanningFactor;
39+
using tier4_planning_msgs::msg::PlanningFactorArray;
40+
using tier4_planning_msgs::msg::SafetyFactorArray;
41+
42+
class PlanningFactorInterface
43+
{
44+
public:
45+
PlanningFactorInterface(rclcpp::Node * node, const std::string & name)
46+
: name_{name},
47+
pub_factors_{
48+
node->create_publisher<PlanningFactorArray>("/planning/planning_factors/" + name, 1)},
49+
clock_{node->get_clock()}
50+
{
51+
}
52+
53+
/**
54+
* @brief factor setter for single control point.
55+
*
56+
* @param path points.
57+
* @param ego current pose.
58+
* @param control point pose. (e.g. stop or slow down point pose)
59+
* @param behavior of this planning factor.
60+
* @param safety factor.
61+
* @param driving direction.
62+
* @param target velocity of the control point.
63+
* @param shift length of the control point.
64+
* @param detail information.
65+
*/
66+
template <class PointType>
67+
void add(
68+
const std::vector<PointType> & points, const Pose & ego_pose, const Pose & control_point_pose,
69+
const uint16_t behavior, const SafetyFactorArray & safety_factors,
70+
const bool is_driving_forward = true, const double velocity = 0.0,
71+
const double shift_length = 0.0, const std::string & detail = "")
72+
{
73+
const auto distance = static_cast<float>(autoware::motion_utils::calcSignedArcLength(
74+
points, ego_pose.position, control_point_pose.position));
75+
add(
76+
distance, control_point_pose, behavior, safety_factors, is_driving_forward, velocity,
77+
shift_length, detail);
78+
}
79+
80+
/**
81+
* @brief factor setter for two control points (section).
82+
*
83+
* @param path points.
84+
* @param ego current pose.
85+
* @param control section start pose. (e.g. lane change start point pose)
86+
* @param control section end pose. (e.g. lane change end point pose)
87+
* @param behavior of this planning factor.
88+
* @param safety factor.
89+
* @param driving direction.
90+
* @param target velocity of the control point.
91+
* @param shift length of the control point.
92+
* @param detail information.
93+
*/
94+
template <class PointType>
95+
void add(
96+
const std::vector<PointType> & points, const Pose & ego_pose, const Pose & start_pose,
97+
const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors,
98+
const bool is_driving_forward = true, const double velocity = 0.0,
99+
const double shift_length = 0.0, const std::string & detail = "")
100+
{
101+
const auto start_distance = static_cast<float>(
102+
autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, start_pose.position));
103+
const auto end_distance = static_cast<float>(
104+
autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, end_pose.position));
105+
add(
106+
start_distance, end_distance, start_pose, end_pose, behavior, safety_factors,
107+
is_driving_forward, velocity, shift_length, detail);
108+
}
109+
110+
/**
111+
* @brief factor setter for single control point.
112+
*
113+
* @param distance to control point.
114+
* @param control point pose. (e.g. stop point pose)
115+
* @param behavior of this planning factor.
116+
* @param safety factor.
117+
* @param driving direction.
118+
* @param target velocity of the control point.
119+
* @param shift length of the control point.
120+
* @param detail information.
121+
*/
122+
void add(
123+
const double distance, const Pose & control_point_pose, const uint16_t behavior,
124+
const SafetyFactorArray & safety_factors, const bool is_driving_forward = true,
125+
const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "")
126+
{
127+
const auto control_point = tier4_planning_msgs::build<ControlPoint>()
128+
.pose(control_point_pose)
129+
.velocity(velocity)
130+
.shift_length(shift_length)
131+
.distance(distance);
132+
133+
const auto factor = tier4_planning_msgs::build<PlanningFactor>()
134+
.module(name_)
135+
.is_driving_forward(is_driving_forward)
136+
.control_points({control_point})
137+
.behavior(behavior)
138+
.detail(detail)
139+
.safety_factors(safety_factors);
140+
141+
factors_.push_back(factor);
142+
}
143+
144+
/**
145+
* @brief factor setter for two control points (section).
146+
*
147+
* @param distance to control section start point.
148+
* @param distance to control section end point.
149+
* @param control section start pose. (e.g. lane change start point pose)
150+
* @param control section end pose. (e.g. lane change end point pose)
151+
* @param behavior of this planning factor.
152+
* @param safety factor.
153+
* @param driving direction.
154+
* @param target velocity of the control point.
155+
* @param shift length of the control point.
156+
* @param detail information.
157+
*/
158+
void add(
159+
const double start_distance, const double end_distance, const Pose & start_pose,
160+
const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors,
161+
const bool is_driving_forward = true, const double velocity = 0.0,
162+
const double shift_length = 0.0, const std::string & detail = "")
163+
{
164+
const auto control_start_point = tier4_planning_msgs::build<ControlPoint>()
165+
.pose(start_pose)
166+
.velocity(velocity)
167+
.shift_length(shift_length)
168+
.distance(start_distance);
169+
170+
const auto control_end_point = tier4_planning_msgs::build<ControlPoint>()
171+
.pose(end_pose)
172+
.velocity(velocity)
173+
.shift_length(shift_length)
174+
.distance(end_distance);
175+
176+
const auto factor = tier4_planning_msgs::build<PlanningFactor>()
177+
.module(name_)
178+
.is_driving_forward(is_driving_forward)
179+
.control_points({control_start_point, control_end_point})
180+
.behavior(behavior)
181+
.detail(detail)
182+
.safety_factors(safety_factors);
183+
184+
factors_.push_back(factor);
185+
}
186+
187+
/**
188+
* @brief publish planning factors.
189+
*/
190+
void publish()
191+
{
192+
PlanningFactorArray msg;
193+
msg.header.frame_id = "map";
194+
msg.header.stamp = clock_->now();
195+
msg.factors = factors_;
196+
197+
pub_factors_->publish(msg);
198+
199+
factors_.clear();
200+
}
201+
202+
private:
203+
std::string name_;
204+
205+
rclcpp::Publisher<PlanningFactorArray>::SharedPtr pub_factors_;
206+
207+
rclcpp::Clock::SharedPtr clock_;
208+
209+
std::vector<PlanningFactor> factors_;
210+
};
211+
212+
extern template void PlanningFactorInterface::add<tier4_planning_msgs::msg::PathPointWithLaneId>(
213+
const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
214+
const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
215+
const std::string &);
216+
extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPoint>(
217+
const std::vector<autoware_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
218+
const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
219+
const std::string &);
220+
extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::TrajectoryPoint>(
221+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &, const Pose &, const Pose &,
222+
const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
223+
const std::string &);
224+
225+
extern template void PlanningFactorInterface::add<tier4_planning_msgs::msg::PathPointWithLaneId>(
226+
const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
227+
const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double,
228+
const double, const std::string &);
229+
extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPoint>(
230+
const std::vector<autoware_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
231+
const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double,
232+
const double, const std::string &);
233+
extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::TrajectoryPoint>(
234+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &, const Pose &, const Pose &,
235+
const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double,
236+
const double, const std::string &);
237+
238+
} // namespace autoware::motion_utils
239+
240+
#endif // AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_

‎common/autoware_motion_utils/src/factor/planing_factor_interface.cpp

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
16+
17+
#include <string>
18+
#include <vector>
19+
20+
namespace autoware::motion_utils
21+
{
22+
template void PlanningFactorInterface::add<tier4_planning_msgs::msg::PathPointWithLaneId>(
23+
const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
24+
const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
25+
const std::string &);
26+
template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPoint>(
27+
const std::vector<autoware_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
28+
const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
29+
const std::string &);
30+
template void PlanningFactorInterface::add<autoware_planning_msgs::msg::TrajectoryPoint>(
31+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &, const Pose &, const Pose &,
32+
const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
33+
const std::string &);
34+
35+
template void PlanningFactorInterface::add<tier4_planning_msgs::msg::PathPointWithLaneId>(
36+
const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
37+
const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double,
38+
const double, const std::string &);
39+
template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPoint>(
40+
const std::vector<autoware_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
41+
const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double,
42+
const double, const std::string &);
43+
template void PlanningFactorInterface::add<autoware_planning_msgs::msg::TrajectoryPoint>(
44+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &, const Pose &, const Pose &,
45+
const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double,
46+
const double, const std::string &);
47+
} // namespace autoware::motion_utils

0 commit comments

Comments
 (0)