Skip to content

Commit d0c2ed2

Browse files
style(pre-commit): autofix
1 parent a058960 commit d0c2ed2

File tree

2 files changed

+15
-11
lines changed

2 files changed

+15
-11
lines changed

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@
3131
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
3232
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
3333
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
34-
#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
3534
#include <autoware_planning_msgs/msg/mission_remaining_distance_time.hpp>
35+
#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
3636
#include <geometry_msgs/msg/pose.hpp>
3737
#include <nav_msgs/msg/occupancy_grid.hpp>
3838
#include <nav_msgs/msg/odometry.hpp>
@@ -63,8 +63,8 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
6363
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
6464
using autoware_perception_msgs::msg::TrafficSignalArray;
6565
using autoware_planning_msgs::msg::LaneletRoute;
66-
using autoware_planning_msgs::msg::PoseWithUuidStamped;
6766
using autoware_planning_msgs::msg::MissionRemainingDistanceTime;
67+
using autoware_planning_msgs::msg::PoseWithUuidStamped;
6868
using geometry_msgs::msg::Pose;
6969
using nav_msgs::msg::OccupancyGrid;
7070
using nav_msgs::msg::Odometry;
@@ -111,7 +111,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
111111
rclcpp::Subscription<LateralOffset>::SharedPtr lateral_offset_subscriber_;
112112
rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
113113
rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
114-
rclcpp::Publisher<MissionRemainingDistanceTime>::SharedPtr mission_remaining_distance_time_publisher_;
114+
rclcpp::Publisher<MissionRemainingDistanceTime>::SharedPtr
115+
mission_remaining_distance_time_publisher_;
115116
rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
116117
rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
117118
rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
@@ -193,7 +194,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
193194
// debug
194195
rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
195196
rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;
196-
197+
197198
/**
198199
* @brief compute mission remaining distance and time
199200
*/

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+10-7
Original file line numberDiff line numberDiff line change
@@ -496,14 +496,16 @@ void BehaviorPathPlannerNode::computeTurnSignal(
496496
publish_steering_factor(planner_data, turn_signal);
497497
}
498498

499-
void BehaviorPathPlannerNode::computeMissionRemainingDistanceTime(const behavior_path_planner::PlanResult & path)
499+
void BehaviorPathPlannerNode::computeMissionRemainingDistanceTime(
500+
const behavior_path_planner::PlanResult & path)
500501
{
501-
remaining_distance_time_.remaining_distance =
502-
motion_utils::calcSignedArcLength(path->points, planner_data_->self_odometry->pose.pose.position, goal_pose_.position);
502+
remaining_distance_time_.remaining_distance = motion_utils::calcSignedArcLength(
503+
path->points, planner_data_->self_odometry->pose.pose.position, goal_pose_.position);
503504

504-
geometry_msgs::msg::Vector3 current_vehicle_velocity = planner_data_->self_odometry->twist.twist.linear;
505+
geometry_msgs::msg::Vector3 current_vehicle_velocity =
506+
planner_data_->self_odometry->twist.twist.linear;
505507

506-
double current_vehicle_velocity_norm = std::sqrt(
508+
double current_vehicle_velocity_norm = std::sqrt(
507509
current_vehicle_velocity.x * current_vehicle_velocity.x +
508510
current_vehicle_velocity.y * current_vehicle_velocity.y);
509511

@@ -516,14 +518,15 @@ void BehaviorPathPlannerNode::computeMissionRemainingDistanceTime(const behavior
516518
return;
517519
}
518520

519-
double remaining_time = remaining_distance_time_.remaining_distance / current_vehicle_velocity_norm;
521+
double remaining_time =
522+
remaining_distance_time_.remaining_distance / current_vehicle_velocity_norm;
520523
remaining_distance_time_.remaining_time = remaining_time;
521524

522525
remaining_distance_time_.hours = static_cast<uint32_t>(remaining_time / 3600.0);
523526
remaining_time = std::fmod(remaining_time, 3600);
524527
remaining_distance_time_.minutes = static_cast<uint32_t>(remaining_time / 60.0);
525528
remaining_distance_time_.seconds = static_cast<uint32_t>(std::fmod(remaining_time, 60));
526-
return ;
529+
return;
527530
}
528531

529532
void BehaviorPathPlannerNode::publish_steering_factor(

0 commit comments

Comments
 (0)