Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Apr 23, 2024
1 parent a058960 commit d0c2ed2
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
#include <autoware_planning_msgs/msg/mission_remaining_distance_time.hpp>
#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
Expand Down Expand Up @@ -63,8 +63,8 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
using autoware_perception_msgs::msg::TrafficSignalArray;
using autoware_planning_msgs::msg::LaneletRoute;
using autoware_planning_msgs::msg::PoseWithUuidStamped;
using autoware_planning_msgs::msg::MissionRemainingDistanceTime;
using autoware_planning_msgs::msg::PoseWithUuidStamped;
using geometry_msgs::msg::Pose;
using nav_msgs::msg::OccupancyGrid;
using nav_msgs::msg::Odometry;
Expand Down Expand Up @@ -111,7 +111,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
rclcpp::Subscription<LateralOffset>::SharedPtr lateral_offset_subscriber_;
rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
rclcpp::Publisher<MissionRemainingDistanceTime>::SharedPtr mission_remaining_distance_time_publisher_;
rclcpp::Publisher<MissionRemainingDistanceTime>::SharedPtr
mission_remaining_distance_time_publisher_;
rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
Expand Down Expand Up @@ -193,7 +194,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
// debug
rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;

/**
* @brief compute mission remaining distance and time
*/
Expand Down
17 changes: 10 additions & 7 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -496,14 +496,16 @@ void BehaviorPathPlannerNode::computeTurnSignal(
publish_steering_factor(planner_data, turn_signal);
}

void BehaviorPathPlannerNode::computeMissionRemainingDistanceTime(const behavior_path_planner::PlanResult & path)
void BehaviorPathPlannerNode::computeMissionRemainingDistanceTime(
const behavior_path_planner::PlanResult & path)
{
remaining_distance_time_.remaining_distance =
motion_utils::calcSignedArcLength(path->points, planner_data_->self_odometry->pose.pose.position, goal_pose_.position);
remaining_distance_time_.remaining_distance = motion_utils::calcSignedArcLength(
path->points, planner_data_->self_odometry->pose.pose.position, goal_pose_.position);

geometry_msgs::msg::Vector3 current_vehicle_velocity = planner_data_->self_odometry->twist.twist.linear;
geometry_msgs::msg::Vector3 current_vehicle_velocity =
planner_data_->self_odometry->twist.twist.linear;

double current_vehicle_velocity_norm = std::sqrt(
double current_vehicle_velocity_norm = std::sqrt(
current_vehicle_velocity.x * current_vehicle_velocity.x +
current_vehicle_velocity.y * current_vehicle_velocity.y);

Expand All @@ -516,14 +518,15 @@ void BehaviorPathPlannerNode::computeMissionRemainingDistanceTime(const behavior
return;
}

double remaining_time = remaining_distance_time_.remaining_distance / current_vehicle_velocity_norm;
double remaining_time =
remaining_distance_time_.remaining_distance / current_vehicle_velocity_norm;
remaining_distance_time_.remaining_time = remaining_time;

remaining_distance_time_.hours = static_cast<uint32_t>(remaining_time / 3600.0);
remaining_time = std::fmod(remaining_time, 3600);
remaining_distance_time_.minutes = static_cast<uint32_t>(remaining_time / 60.0);
remaining_distance_time_.seconds = static_cast<uint32_t>(std::fmod(remaining_time, 60));
return ;
return;
}

void BehaviorPathPlannerNode::publish_steering_factor(
Expand Down

0 comments on commit d0c2ed2

Please sign in to comment.