Skip to content

Commit

Permalink
apply clang format
Browse files Browse the repository at this point in the history
  • Loading branch information
takayuki5168 committed Jan 16, 2022
1 parent b17671d commit aaae68c
Show file tree
Hide file tree
Showing 8 changed files with 33 additions and 28 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,13 @@
#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
#include "behavior_path_planner/turn_signal_decider.hpp"
#include "planning_manager/msg/planning_data.hpp"
#include "planning_manager/srv/behavior_path_planner_plan.hpp"
#include "planning_manager/srv/behavior_path_planner_validate.hpp"

#include <route_handler/route_handler.hpp>
#include <tier4_autoware_utils/ros/self_pose_listener.hpp>

#include "planning_manager/msg/planning_data.hpp"
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -622,7 +622,8 @@ void BehaviorPathPlannerNode::setPlanningData([[maybe_unused]] const PlanningDat

planner_data_->route_handler->setMap(planning_data.had_map_bin);
planner_data_->self_odometry = std::make_shared<Odometry>(planning_data.ego_odom);
planner_data_->dynamic_object = std::make_shared<PredictedObjects>(planning_data.predicted_objects);
planner_data_->dynamic_object =
std::make_shared<PredictedObjects>(planning_data.predicted_objects);

// external approval
planner_data_->approval.is_approved.data = planning_data.external_approval.approval;
Expand All @@ -637,7 +638,8 @@ void BehaviorPathPlannerNode::setPlanningData([[maybe_unused]] const PlanningDat
return "NONE";
}
};
planner_data_->approval.is_force_approved.module_name = getModuleName(planning_data.force_approval.module);
planner_data_->approval.is_force_approved.module_name =
getModuleName(planning_data.force_approval.module);
planner_data_->approval.is_force_approved.stamp = planning_data.header.stamp;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,12 @@

#include "behavior_velocity_planner/planner_data.hpp"
#include "behavior_velocity_planner/planner_manager.hpp"
#include "planning_manager/msg/planning_data.hpp"
#include "planning_manager/srv/behavior_velocity_planner_plan.hpp"
#include "planning_manager/srv/behavior_velocity_planner_validate.hpp"

#include <rclcpp/rclcpp.hpp>

#include "planning_manager/msg/planning_data.hpp"
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/path.hpp>
Expand Down Expand Up @@ -100,8 +100,10 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr stop_reason_diag_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;

rclcpp::Service<planning_manager::srv::BehaviorVelocityPlannerPlan>::SharedPtr srv_planning_manager_plan_;
rclcpp::Service<planning_manager::srv::BehaviorVelocityPlannerValidate>::SharedPtr srv_planning_manager_validate_;
rclcpp::Service<planning_manager::srv::BehaviorVelocityPlannerPlan>::SharedPtr
srv_planning_manager_plan_;
rclcpp::Service<planning_manager::srv::BehaviorVelocityPlannerValidate>::SharedPtr
srv_planning_manager_validate_;

void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path);

Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>diagnostic_msgs</depend>
<depend>planning_manager</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>grid_map_ros</depend>
Expand All @@ -26,6 +25,7 @@
<depend>nav_msgs</depend>
<depend>nlohmann-json-dev</depend>
<depend>pcl_conversions</depend>
<depend>planning_manager</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
Expand Down
10 changes: 6 additions & 4 deletions planning/behavior_velocity_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,9 +159,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
srv_planning_manager_plan_ = create_service<planning_manager::srv::BehaviorVelocityPlannerPlan>(
"~/srv/planning_manager/plan",
std::bind(&BehaviorVelocityPlannerNode::onPlanService, this, _1, _2));
srv_planning_manager_validate_ = create_service<planning_manager::srv::BehaviorVelocityPlannerValidate>(
"~/srv/planning_manager/validate",
std::bind(&BehaviorVelocityPlannerNode::onValidateService, this, _1, _2));
srv_planning_manager_validate_ =
create_service<planning_manager::srv::BehaviorVelocityPlannerValidate>(
"~/srv/planning_manager/validate",
std::bind(&BehaviorVelocityPlannerNode::onValidateService, this, _1, _2));
}

bool BehaviorVelocityPlannerNode::isDataReady()
Expand Down Expand Up @@ -379,7 +380,8 @@ void BehaviorVelocityPlannerNode::onValidateService(
response->result.data = true;
}

void BehaviorVelocityPlannerNode::setPlanningData([[maybe_unused]] const planning_manager::msg::PlanningData planning_data)
void BehaviorVelocityPlannerNode::setPlanningData(
[[maybe_unused]] const planning_manager::msg::PlanningData planning_data)
{
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_auto_planning_msgs/msg/planning_data.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <nav_msgs/msg/odometry.hpp>
Expand All @@ -36,21 +35,21 @@
namespace planning_manager
{
using autoware_auto_planning_msgs::msg::HADMapRoute;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_planning_msgs::msg::Trajectory;
using planning_manager::msg::PlanningData;
using planning_manager::srv::BehaviorPathPlannerPlan;
using planning_manager::srv::BehaviorPathPlannerValidate;
using planning_manager::srv::BehaviorVelocityPlannerPlan;
using planning_manager::srv::BehaviorVelocityPlannerValidate;
using planning_manager::msg::PlanningData;

// MEMO(murooka) beahvior path
using tier4_planning_msgs::msg::Approval;
using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::HADMapRoute;
using nav_msgs::msg::Odometry;
using tier4_planning_msgs::msg::Approval;
using tier4_planning_msgs::msg::PathChangeModule;

class PlanningManagerNode : public rclcpp::Node
Expand Down Expand Up @@ -87,12 +86,9 @@ class PlanningManagerNode : public rclcpp::Node
rclcpp::CallbackGroup::SharedPtr callback_group_services_;

// clients
rclcpp::Client<BehaviorPathPlannerPlan>::SharedPtr
client_behavior_path_planner_plan_;
rclcpp::Client<BehaviorPathPlannerValidate>::SharedPtr
client_behavior_path_planner_validate_;
rclcpp::Client<BehaviorVelocityPlannerPlan>::SharedPtr
client_behavior_velocity_planner_plan_;
rclcpp::Client<BehaviorPathPlannerPlan>::SharedPtr client_behavior_path_planner_plan_;
rclcpp::Client<BehaviorPathPlannerValidate>::SharedPtr client_behavior_path_planner_validate_;
rclcpp::Client<BehaviorVelocityPlannerPlan>::SharedPtr client_behavior_velocity_planner_plan_;
rclcpp::Client<BehaviorVelocityPlannerValidate>::SharedPtr
client_behavior_velocity_planner_validate_;

Expand Down
2 changes: 1 addition & 1 deletion planning/planning_manager/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,11 @@

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>tier4_autoware_utils</depend>
<depend>tier4_planning_msgs</depend>
<depend>nav_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
15 changes: 9 additions & 6 deletions planning/planning_manager/src/planning_manager_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,11 +73,12 @@ PlanningManagerNode::PlanningManagerNode(const rclcpp::NodeOptions & node_option
std::bind(&PlanningManagerNode::onMap, this, _1));
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
"~/input/odometry", 1, std::bind(&PlanningManagerNode::onOdometry, this, _1));
predicted_objects_sub_ = create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>(
"~/input/predicted_objects", 1, std::bind(&PlanningManagerNode::onPredictedObjects, this, _1));
predicted_objects_sub_ =
create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>(
"~/input/predicted_objects", 1,
std::bind(&PlanningManagerNode::onPredictedObjects, this, _1));
external_approval_sub_ = create_subscription<Approval>(
"~/input/external_approval", 1,
std::bind(&PlanningManagerNode::onExternalApproval, this, _1));
"~/input/external_approval", 1, std::bind(&PlanningManagerNode::onExternalApproval, this, _1));
force_approval_sub_ = create_subscription<PathChangeModule>(
"~/input/force_approval", 1, std::bind(&PlanningManagerNode::onForceApproval, this, _1));

Expand Down Expand Up @@ -256,10 +257,12 @@ void PlanningManagerNode::validateTrajectory([[maybe_unused]] const Trajectory &
client_behavior_velocity_planner_validate_, behavior_velocity_request);
}

void PlanningManagerNode::publishTrajectory(const Trajectory & traj) {
void PlanningManagerNode::publishTrajectory(const Trajectory & traj)
{
// TODO(murooka) use thread local for member variable?
RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start publishTrajectory");
traj_pub_->publish(traj); }
traj_pub_->publish(traj);
}
void PlanningManagerNode::publishDiagnostics() {}

} // namespace planning_manager
Expand Down

0 comments on commit aaae68c

Please sign in to comment.