behavior_planner_node.hpp

Go to the documentation of this file.

// Copyright 2020 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#ifndef BEHAVIOR_PLANNER_NODES__BEHAVIOR_PLANNER_NODE_HPP_
#define BEHAVIOR_PLANNER_NODES__BEHAVIOR_PLANNER_NODE_HPP_

#include <behavior_planner_nodes/behavior_planner_node.hpp>
#include <behavior_planner_nodes/visibility_control.hpp>

// rclcpp headers
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

// autoware packages
#include <common/types.hpp>
#include <autoware_auto_msgs/action/plan_trajectory.hpp>
#include <autoware_auto_msgs/msg/trajectory.hpp>
#include <autoware_auto_msgs/msg/trajectory_point.hpp>
#include <autoware_auto_msgs/msg/vehicle_kinematic_state.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_report.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_command.hpp>
#include <autoware_auto_msgs/srv/had_map_service.hpp>
#include <autoware_auto_msgs/srv/modify_trajectory.hpp>
#include <behavior_planner/behavior_planner.hpp>

//  Other ROS packages
#include <lanelet2_core/LaneletMap.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>

// others
#include <string>
#include <memory>

namespace autoware
{
namespace behavior_planner_nodes
{
using PlanTrajectoryAction = autoware_auto_msgs::action::PlanTrajectory;
using PlanTrajectoryGoalHandle = rclcpp_action::ClientGoalHandle<PlanTrajectoryAction>;
using HADMapService = autoware_auto_msgs::srv::HADMapService;
using autoware_auto_msgs::srv::ModifyTrajectory;
using autoware_auto_msgs::msg::TrajectoryPoint;
using autoware_auto_msgs::msg::Trajectory;
using autoware_auto_msgs::msg::Route;
using autoware_auto_msgs::msg::VehicleStateCommand;
using autoware_auto_msgs::msg::VehicleStateReport;
using State = autoware_auto_msgs::msg::VehicleKinematicState;
using autoware::behavior_planner::PlannerType;
using autoware::behavior_planner::RouteWithType;

using autoware::common::types::bool8_t;
using autoware::common::types::uchar8_t;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;

class BEHAVIOR_PLANNER_NODES_PUBLIC BehaviorPlannerNode : public rclcpp::Node
{
public:
  explicit BehaviorPlannerNode(const rclcpp::NodeOptions & options);

private:
  //  ROS Interface
  rclcpp_action::Client<PlanTrajectoryAction>::SharedPtr m_lane_planner_client;
  rclcpp_action::Client<PlanTrajectoryAction>::SharedPtr m_parking_planner_client;
  rclcpp::Client<HADMapService>::SharedPtr m_map_client;
  // May be nullptr if disabled
  rclcpp::Client<ModifyTrajectory>::SharedPtr m_modify_trajectory_client;
  rclcpp::Subscription<State>::SharedPtr m_ego_state_sub{};
  rclcpp::Subscription<Route>::SharedPtr m_route_sub{};
  rclcpp::Subscription<Trajectory>::SharedPtr m_lane_trajectory_sub{};
  rclcpp::Subscription<Trajectory>::SharedPtr m_parking_trajectory_sub{};
  rclcpp::Subscription<VehicleStateReport>::SharedPtr m_vehicle_state_report_sub{};
  rclcpp::Publisher<Trajectory>::SharedPtr m_trajectory_pub{};
  rclcpp::Publisher<Trajectory>::SharedPtr m_debug_trajectory_pub{};
  rclcpp::Publisher<Trajectory>::SharedPtr m_debug_checkpoints_pub{};
  rclcpp::Publisher<Route>::SharedPtr m_debug_subroute_pub{};
  rclcpp::Publisher<VehicleStateCommand>::SharedPtr m_vehicle_state_command_pub{};

  //  planner
  std::unique_ptr<behavior_planner::BehaviorPlanner> m_planner;

  // msg cache
  lanelet::LaneletMapPtr m_lanelet_map_ptr;
  Route::SharedPtr m_route;
  State m_ego_state;
  uchar8_t m_current_gear;

  // bools to manage states
  bool8_t m_requesting_trajectory;

  // transforms
  std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
  std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;

  // callbacks
  void on_ego_state(const State::SharedPtr & msg);
  void on_route(const Route::SharedPtr & msg);
  void on_lane_trajectory(const Trajectory::SharedPtr & msg);
  void on_parking_trajectory(const Trajectory::SharedPtr & msg);
  void on_vehicle_state_report(const VehicleStateReport::SharedPtr & msg);
  void map_response(rclcpp::Client<HADMapService>::SharedFuture future);
  void modify_trajectory_response(rclcpp::Client<ModifyTrajectory>::SharedFuture future);
  void clear_trajectory_cache();

  void goal_response_callback(std::shared_future<PlanTrajectoryGoalHandle::SharedPtr> future);
  void feedback_callback(
    PlanTrajectoryGoalHandle::SharedPtr goal_handle,
    const std::shared_ptr<const PlanTrajectoryAction::Feedback> feedback);
  void result_callback(const PlanTrajectoryGoalHandle::WrappedResult & result);

  // other functions
  void init();
  Trajectory refine_trajectory(const State & ego_state, const Trajectory & input);
  State transform_to_map(const State & state);
  void request_trajectory(const RouteWithType & route_with_type);
};
}  // namespace behavior_planner_nodes
}  // namespace autoware

#endif  // BEHAVIOR_PLANNER_NODES__BEHAVIOR_PLANNER_NODE_HPP_