Skip to content

Commit 821cf29

Browse files
committed
use the double type instead of the structure
Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
1 parent 43258a2 commit 821cf29

File tree

3 files changed

+9
-14
lines changed

3 files changed

+9
-14
lines changed

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

+2-7
Original file line numberDiff line numberDiff line change
@@ -84,19 +84,14 @@ struct Input
8484
std::vector<std::string> boundary_types_to_detect{};
8585
};
8686

87-
struct DerivativeDeviation
88-
{
89-
double longitudinal_vel{0.0};
90-
};
91-
9287
struct Output
9388
{
9489
std::map<std::string, double> processing_time_map{};
9590
bool will_leave_lane{};
9691
bool is_out_of_lane{};
9792
bool will_cross_boundary{};
9893
PoseDeviation trajectory_deviation{};
99-
DerivativeDeviation trajectory_der_deviation{};
94+
double deviation_longitudinal_vel{0.0};
10095
lanelet::ConstLanelets candidate_lanelets{};
10196
TrajectoryPoints resampled_trajectory{};
10297
std::vector<LinearRing2d> vehicle_footprints{};
@@ -144,7 +139,7 @@ class LaneDepartureChecker
144139
Param param_;
145140
std::shared_ptr<vehicle_info_util::VehicleInfo> vehicle_info_ptr_;
146141

147-
static DerivativeDeviation calcLongitudinalDeviationDerivatives(
142+
static double calcLongitudinalDeviationDerivatives(
148143
const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold,
149144
const Input & input);
150145

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ Output LaneDepartureChecker::update(const Input & input)
104104

105105
tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
106106

107-
output.trajectory_der_deviation = calcLongitudinalDeviationDerivatives(
107+
output.deviation_longitudinal_vel = calcLongitudinalDeviationDerivatives(
108108
*input.predicted_trajectory, param_.ego_nearest_dist_threshold,
109109
param_.ego_nearest_yaw_threshold, input);
110110

@@ -169,21 +169,21 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
169169
return willLeaveLane(candidate_lanelets, vehicle_footprints);
170170
}
171171

172-
DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives(
172+
double LaneDepartureChecker::calcLongitudinalDeviationDerivatives(
173173
const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold,
174174
const Input & input)
175175
{
176176
const geometry_msgs::msg::Pose & pose = input.current_odom->pose.pose;
177177
const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
178178
trajectory.points, pose, dist_threshold, yaw_threshold);
179-
DerivativeDeviation der_deviation;
179+
double deviation_longitudinal_vel;
180180
const auto & ref_point = trajectory.points.at(nearest_idx);
181181

182182
const auto current_vel = input.current_odom->twist.twist.linear.x;
183183
const auto & ref_vel = ref_point.longitudinal_velocity_mps;
184-
der_deviation.longitudinal_vel = current_vel - ref_vel;
184+
deviation_longitudinal_vel = current_vel - ref_vel;
185185

186-
return der_deviation;
186+
return deviation_longitudinal_vel;
187187
}
188188

189189
PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -358,7 +358,7 @@ void LaneDepartureCheckerNode::onTimer()
358358

359359
{
360360
const auto & deviation = output_.trajectory_deviation;
361-
const auto & der_deviation = output_.trajectory_der_deviation;
361+
const double deviation_vel_longitudinal = output_.deviation_longitudinal_vel;
362362
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
363363
"deviation/lateral", deviation.lateral);
364364
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>("deviation/yaw", deviation.yaw);
@@ -367,7 +367,7 @@ void LaneDepartureCheckerNode::onTimer()
367367
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
368368
"deviation/longitudinal", deviation.longitudinal);
369369
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
370-
"deviation/longitudinal_velocity", der_deviation.longitudinal_vel);
370+
"deviation/longitudinal_velocity", deviation_vel_longitudinal);
371371
}
372372
processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true);
373373

0 commit comments

Comments
 (0)