Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

refactor(mission_planner): apply clang-tidy #1737

Merged
merged 2 commits into from
Aug 31, 2022
Merged

refactor(mission_planner): apply clang-tidy #1737

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4ca02e7
refactor(mission_planner): apply clang-tidy
isamu-takagi Aug 31, 2022
c5dc5ef
fix: fix build error
isamu-takagi Aug 31, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,12 @@ GoalPoseVisualizer::GoalPoseVisualizer(const rclcpp::NodeOptions & node_options)
{
sub_route_ = create_subscription<autoware_auto_planning_msgs::msg::HADMapRoute>(
"input/route", rclcpp::QoS{1}.transient_local(),
std::bind(&GoalPoseVisualizer::echoBackRouteCallback, this, std::placeholders::_1));
std::bind(&GoalPoseVisualizer::echo_back_route_callback, this, std::placeholders::_1));
pub_goal_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>(
"output/goal_pose", rclcpp::QoS{1}.transient_local());
}

void GoalPoseVisualizer::echoBackRouteCallback(
void GoalPoseVisualizer::echo_back_route_callback(
const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg)
{
geometry_msgs::msg::PoseStamped goal_pose;
Expand Down
2 changes: 1 addition & 1 deletion planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class GoalPoseVisualizer : public rclcpp::Node
rclcpp::Subscription<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr sub_route_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_goal_pose_;

void echoBackRouteCallback(
void echo_back_route_callback(
const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg);
};

Expand Down
37 changes: 19 additions & 18 deletions planning/mission_planner/src/mission_planner/mission_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ MissionPlanner::MissionPlanner(
using std::placeholders::_1;

goal_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"input/goal_pose", 10, std::bind(&MissionPlanner::goalPoseCallback, this, _1));
"input/goal_pose", 10, std::bind(&MissionPlanner::goal_pose_callback, this, _1));
checkpoint_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"input/checkpoint", 10, std::bind(&MissionPlanner::checkpointCallback, this, _1));
"input/checkpoint", 10, std::bind(&MissionPlanner::checkpoint_callback, this, _1));

rclcpp::QoS durable_qos{1};
durable_qos.transient_local();
Expand All @@ -52,7 +52,7 @@ MissionPlanner::MissionPlanner(
create_publisher<visualization_msgs::msg::MarkerArray>("debug/route_marker", durable_qos);
}

bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose)
bool MissionPlanner::get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose)
{
geometry_msgs::msg::PoseStamped base_link_origin;
base_link_origin.header.frame_id = base_link_frame_;
Expand All @@ -65,12 +65,12 @@ bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_veh
base_link_origin.pose.orientation.w = 1;

// transform base_link frame origin to map_frame to get vehicle positions
return transformPose(base_link_origin, ego_vehicle_pose, map_frame_);
return transform_pose(base_link_origin, ego_vehicle_pose, map_frame_);
}

bool MissionPlanner::transformPose(
bool MissionPlanner::transform_pose(
const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose,
const std::string target_frame)
const std::string & target_frame)
{
geometry_msgs::msg::TransformStamped transform;
try {
Expand All @@ -84,17 +84,17 @@ bool MissionPlanner::transformPose(
}
}

void MissionPlanner::goalPoseCallback(
void MissionPlanner::goal_pose_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr)
{
// set start pose
if (!getEgoVehiclePose(&start_pose_)) {
if (!get_ego_vehicle_pose(&start_pose_)) {
RCLCPP_ERROR(
get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning");
return;
}
// set goal pose
if (!transformPose(*goal_msg_ptr, &goal_pose_, map_frame_)) {
if (!transform_pose(*goal_msg_ptr, &goal_pose_, map_frame_)) {
RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning");
return;
}
Expand All @@ -104,16 +104,16 @@ void MissionPlanner::goalPoseCallback(
checkpoints_.push_back(start_pose_);
checkpoints_.push_back(goal_pose_);

if (!isRoutingGraphReady()) {
if (!is_routing_graph_ready()) {
RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning");
return;
}

autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute();
publishRoute(route);
autoware_auto_planning_msgs::msg::HADMapRoute route = plan_route();
publish_route(route);
} // namespace mission_planner

void MissionPlanner::checkpointCallback(
void MissionPlanner::checkpoint_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr)
{
if (checkpoints_.size() < 2) {
Expand All @@ -124,7 +124,7 @@ void MissionPlanner::checkpointCallback(
}

geometry_msgs::msg::PoseStamped transformed_checkpoint;
if (!transformPose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) {
if (!transform_pose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) {
RCLCPP_ERROR(
get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning");
return;
Expand All @@ -133,16 +133,17 @@ void MissionPlanner::checkpointCallback(
// insert checkpoint before goal
checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint);

autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute();
publishRoute(route);
autoware_auto_planning_msgs::msg::HADMapRoute route = plan_route();
publish_route(route);
}

void MissionPlanner::publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const
void MissionPlanner::publish_route(
const autoware_auto_planning_msgs::msg::HADMapRoute & route) const
{
if (!route.segments.empty()) {
RCLCPP_INFO(get_logger(), "Route successfully planned. Publishing...");
route_publisher_->publish(route);
visualizeRoute(route);
visualize_route(route);
} else {
RCLCPP_ERROR(get_logger(), "Calculated route is empty!");
}
Expand Down
19 changes: 10 additions & 9 deletions planning/mission_planner/src/mission_planner/mission_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,11 +47,11 @@ class MissionPlanner : public rclcpp::Node

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;

virtual bool isRoutingGraphReady() const = 0;
virtual autoware_auto_planning_msgs::msg::HADMapRoute planRoute() = 0;
virtual void visualizeRoute(
virtual bool is_routing_graph_ready() const = 0;
virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route() = 0;
virtual void visualize_route(
const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0;
virtual void publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const;
virtual void publish_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const;

private:
rclcpp::Publisher<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr route_publisher_;
Expand All @@ -61,12 +61,13 @@ class MissionPlanner : public rclcpp::Node
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose);
void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr);
void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr);
bool transformPose(
bool get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose);
void goal_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr);
void checkpoint_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr);
bool transform_pose(
const geometry_msgs::msg::PoseStamped & input_pose,
geometry_msgs::msg::PoseStamped * output_pose, const std::string target_frame);
geometry_msgs::msg::PoseStamped * output_pose, const std::string & target_frame);
};

} // namespace mission_planner
Expand Down
Loading