Skip to content

Commit

Permalink
refactor(mission_planner): node refactoring
Browse files Browse the repository at this point in the history 
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed Sep 22, 2022
1 parent 93cc9bd commit 7b5c9aa
Show file tree
Hide file tree
Showing 4 changed files with 92 additions and 84 deletions.
77 changes: 37 additions & 40 deletions planning/mission_planner/src/mission_planner/mission_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#endif
#include <visualization_msgs/msg/marker_array.h>

#include <memory>
#include <string>

namespace mission_planner
Expand All @@ -39,10 +40,13 @@ MissionPlanner::MissionPlanner(

using std::placeholders::_1;

odometry_subscriber_ = create_subscription<nav_msgs::msg::Odometry>(
"/localization/kinematic_state", rclcpp::QoS{1},
std::bind(&MissionPlanner::odometry_callback, this, _1));
goal_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"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::checkpoint_callback, this, _1));
check_point_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"input/check_point", 10, std::bind(&MissionPlanner::check_point_callback, this, _1));

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

bool MissionPlanner::get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose)
void MissionPlanner::odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
{
geometry_msgs::msg::PoseStamped base_link_origin;
base_link_origin.header.frame_id = base_link_frame_;
base_link_origin.pose.position.x = 0;
base_link_origin.pose.position.y = 0;
base_link_origin.pose.position.z = 0;
base_link_origin.pose.orientation.x = 0;
base_link_origin.pose.orientation.y = 0;
base_link_origin.pose.orientation.z = 0;
base_link_origin.pose.orientation.w = 1;

// transform base_link frame origin to map_frame to get vehicle positions
return transform_pose(base_link_origin, ego_vehicle_pose, map_frame_);
ego_pose_ = std::make_shared<const geometry_msgs::msg::Pose>(msg->pose.pose);
}

bool MissionPlanner::transform_pose(
const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose,
const std::string & target_frame)
boost::optional<geometry_msgs::msg::PoseStamped> MissionPlanner::transform_pose(
const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame)
{
geometry_msgs::msg::TransformStamped transform;
try {
geometry_msgs::msg::PoseStamped output_pose;
transform =
tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, tf2::TimePointZero);
tf2::doTransform(input_pose, *output_pose, transform);
return true;
tf2::doTransform(input_pose, output_pose, transform);
return output_pose;
} catch (tf2::TransformException & ex) {
RCLCPP_WARN(get_logger(), "%s", ex.what());
return false;
}

return {};
}

void MissionPlanner::goal_pose_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr)
{
// set 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");
if (!ego_pose_) {
RCLCPP_ERROR(get_logger(), "Ego pose has not been subscribed. Aborting mission planning");
return;
}

// set goal pose
if (!transform_pose(*goal_msg_ptr, &goal_pose_, map_frame_)) {
const auto opt_transformed_goal_pose = transform_pose(*goal_msg_ptr, map_frame_);
if (!opt_transformed_goal_pose) {
RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning");
return;
}
const auto transformed_goal_pose = opt_transformed_goal_pose.get();

RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints.");
checkpoints_.clear();
checkpoints_.push_back(start_pose_);
checkpoints_.push_back(goal_pose_);
RCLCPP_INFO(get_logger(), "New goal pose is set. Reset check_points.");
check_points_.clear();
check_points_.push_back(*ego_pose_);
check_points_.push_back(transformed_goal_pose.pose);

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

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

void MissionPlanner::checkpoint_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr)
void MissionPlanner::check_point_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr check_point_msg_ptr)
{
if (checkpoints_.size() < 2) {
if (check_points_.size() < 2) {
RCLCPP_ERROR(
get_logger(),
"You must set start and goal before setting checkpoints. Aborting mission planning");
"You must set start and goal before setting check_points. Aborting mission planning");
return;
}

geometry_msgs::msg::PoseStamped transformed_checkpoint;
if (!transform_pose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) {
const auto opt_transformed_check_point = transform_pose(*check_point_msg_ptr, map_frame_);
if (!opt_transformed_check_point) {
RCLCPP_ERROR(
get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning");
get_logger(), "Failed to get check_point pose in map frame. Aborting mission planning");
return;
}
const auto transformed_check_point = opt_transformed_check_point.get();

// insert checkpoint before goal
checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint);
// insert check_point before goal
check_points_.insert(check_points_.end() - 1, transformed_check_point.pose);

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

Expand Down
25 changes: 14 additions & 11 deletions planning/mission_planner/src/mission_planner/mission_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef MISSION_PLANNER__MISSION_PLANNER_HPP_
#define MISSION_PLANNER__MISSION_PLANNER_HPP_

#include <boost/optional.hpp>

#include <string>
#include <vector>

Expand All @@ -30,6 +32,7 @@
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>

namespace mission_planner
{
Expand All @@ -38,36 +41,36 @@ class MissionPlanner : public rclcpp::Node
protected:
MissionPlanner(const std::string & node_name, const rclcpp::NodeOptions & node_options);

geometry_msgs::msg::PoseStamped goal_pose_;
geometry_msgs::msg::PoseStamped start_pose_;
std::vector<geometry_msgs::msg::PoseStamped> checkpoints_;
geometry_msgs::msg::Pose::ConstSharedPtr ego_pose_;
std::vector<geometry_msgs::msg::Pose> check_points_;

std::string base_link_frame_;
std::string map_frame_;

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

virtual bool is_routing_graph_ready() const = 0;
virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route() = 0;
virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route(
const std::vector<geometry_msgs::msg::Pose> & check_points) = 0;
virtual void visualize_route(
const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0;
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_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_subscriber_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr checkpoint_subscriber_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr check_point_subscriber_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_subscriber_;

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

bool get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose);
void odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
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);
void check_point_callback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr check_point_msg_ptr);
boost::optional<geometry_msgs::msg::PoseStamped> transform_pose(
const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame);
};

} // namespace mission_planner
Expand Down
66 changes: 36 additions & 30 deletions planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,19 +192,17 @@ void MissionPlannerLanelet2::visualize_route(
marker_publisher_->publish(route_marker_array);
}

bool MissionPlannerLanelet2::is_goal_valid() const
bool MissionPlannerLanelet2::is_goal_valid(const geometry_msgs::msg::Pose & goal_pose) const
{
lanelet::Lanelet closest_lanelet;
if (!lanelet::utils::query::getClosestLanelet(
road_lanelets_, goal_pose_.pose, &closest_lanelet)) {
if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_pose, &closest_lanelet)) {
return false;
}
const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position);
const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose.position);

if (is_in_lane(closest_lanelet, goal_lanelet_pt)) {
const auto lane_yaw =
lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose_.pose.position);
const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation);
const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose.position);
const auto goal_yaw = tf2::getYaw(goal_pose.orientation);
const auto angle_diff = normalize_radian(lane_yaw - goal_yaw);

constexpr double th_angle = M_PI / 4;
Expand All @@ -229,14 +227,14 @@ bool MissionPlannerLanelet2::is_goal_valid() const
// check if goal is in shoulder lanelet
lanelet::Lanelet closest_shoulder_lanelet;
if (!lanelet::utils::query::getClosestLanelet(
shoulder_lanelets_, goal_pose_.pose, &closest_shoulder_lanelet)) {
shoulder_lanelets_, goal_pose, &closest_shoulder_lanelet)) {
return false;
}
// check if goal pose is in shoulder lane
if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) {
const auto lane_yaw =
lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose_.pose.position);
const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation);
lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose.position);
const auto goal_yaw = tf2::getYaw(goal_pose.orientation);
const auto angle_diff = normalize_radian(lane_yaw - goal_yaw);

constexpr double th_angle = M_PI / 4;
Expand All @@ -248,31 +246,34 @@ bool MissionPlannerLanelet2::is_goal_valid() const
return false;
}

autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route()
autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route(
const std::vector<geometry_msgs::msg::Pose> & check_points)
{
const auto & goal_pose = check_points.back();

std::stringstream log_ss;
for (const auto & checkpoint : checkpoints_) {
log_ss << "x: " << checkpoint.pose.position.x << " "
<< "y: " << checkpoint.pose.position.y << std::endl;
for (const auto & check_point : check_points) {
log_ss << "x: " << check_point.position.x << " "
<< "y: " << check_point.position.y << std::endl;
}
RCLCPP_INFO_STREAM(
get_logger(), "start planning route with checkpoints: " << std::endl
<< log_ss.str());
get_logger(), "start planning route with check_points: " << std::endl
<< log_ss.str());

autoware_auto_planning_msgs::msg::HADMapRoute route_msg;
RouteSections route_sections;

if (!is_goal_valid()) {
if (!is_goal_valid(goal_pose)) {
RCLCPP_WARN(get_logger(), "Goal is not valid! Please check position and angle of goal_pose");
return route_msg;
}

for (std::size_t i = 1; i < checkpoints_.size(); i++) {
const auto start_checkpoint = checkpoints_.at(i - 1);
const auto goal_checkpoint = checkpoints_.at(i);
for (std::size_t i = 1; i < check_points.size(); i++) {
const auto start_check_point = check_points.at(i - 1);
const auto goal_check_point = check_points.at(i);
lanelet::ConstLanelets path_lanelets;
if (!route_handler_.planPathLaneletsBetweenCheckpoints(
start_checkpoint.pose, goal_checkpoint.pose, &path_lanelets)) {
start_check_point, goal_check_point, &path_lanelets)) {
return route_msg;
}
// create local route sections
Expand All @@ -286,25 +287,30 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route
return route_msg;
}

refine_goal_height(route_sections);
const auto refined_goal_pose = refine_goal_height(route_sections, goal_pose);

route_msg.header.stamp = this->now();
route_msg.header.frame_id = map_frame_;
route_msg.segments = route_sections;
route_msg.goal_pose = goal_pose_.pose;
route_msg.goal_pose = refined_goal_pose;

RCLCPP_DEBUG(get_logger(), "Goal Pose Z : %lf", goal_pose_.pose.position.z);
RCLCPP_DEBUG(get_logger(), "Goal Pose Z : %lf", refined_goal_pose.position.z);
return route_msg;
}

void MissionPlannerLanelet2::refine_goal_height(const RouteSections & route_sections)
geometry_msgs::msg::Pose MissionPlannerLanelet2::refine_goal_height(
const RouteSections & route_sections, const geometry_msgs::msg::Pose & goal_pose) const
{
const auto goal_lane_id = route_sections.back().preferred_primitive_id;
lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id);
const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position);
double goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt);
goal_pose_.pose.position.z = goal_height;
checkpoints_.back().pose.position.z = goal_height;
const lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id);
const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose.position);

const double goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt);

geometry_msgs::msg::Pose refined_goal_pose = goal_pose;
refined_goal_pose.position.z = goal_height;

return refined_goal_pose;
}

} // namespace mission_planner
Expand Down
8 changes: 5 additions & 3 deletions planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,14 @@ class MissionPlannerLanelet2 : public MissionPlanner
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_subscriber_;

void map_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
bool is_goal_valid() const;
void refine_goal_height(const RouteSections & route_sections);
bool is_goal_valid(const geometry_msgs::msg::Pose & goal_pose) const;
geometry_msgs::msg::Pose refine_goal_height(
const RouteSections & route_sections, const geometry_msgs::msg::Pose & goal_pose) const;

// virtual functions
bool is_routing_graph_ready() const override;
autoware_auto_planning_msgs::msg::HADMapRoute plan_route() override;
autoware_auto_planning_msgs::msg::HADMapRoute plan_route(
const std::vector<geometry_msgs::msg::Pose> & check_points) override;
void visualize_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const override;
};
} // namespace mission_planner
Expand Down

0 comments on commit 7b5c9aa

Please sign in to comment.