behavior_planner.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__BEHAVIOR_PLANNER_HPP_
#define BEHAVIOR_PLANNER__BEHAVIOR_PLANNER_HPP_

#include <behavior_planner/visibility_control.hpp>
#include <behavior_planner/trajectory_manager.hpp>

// Autoware packages
#include <common/types.hpp>
#include <autoware_auto_msgs/msg/route.hpp>
#include <autoware_auto_msgs/msg/trajectory.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_kinematic_state.hpp>

// lanelet headers
#include <lanelet2_core/LaneletMap.h>

// others
#include <iostream>
#include <vector>

namespace autoware
{
namespace behavior_planner
{

// TODO(mitsudome-r): extract this into common library to be used
// both in behavior planner and global planner
namespace PrimitiveType
{
constexpr const char ParkingSpot[] = "parking_spot";
constexpr const char DrivableArea[] = "drivable_area";
constexpr const char Lane[] = "lane";
}  // namespace PrimitiveType


using autoware_auto_msgs::msg::Route;
using autoware_auto_msgs::msg::MapPrimitive;
using autoware_auto_msgs::msg::TrajectoryPoint;
using State = autoware_auto_msgs::msg::VehicleKinematicState;
using autoware_auto_msgs::msg::VehicleStateCommand;

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

enum PlannerType
{
  LANE,
  PARKING,
  UNKNOWN
};

enum class ParkingDirection
{
  HEAD_IN,  
  TOE_IN    
};

struct RouteWithType
{
  Route route;
  PlannerType planner_type;
};

class BEHAVIOR_PLANNER_PUBLIC BehaviorPlanner
{
public:
  explicit BehaviorPlanner(const PlannerConfig & config);

  void set_route(const Route & route, const lanelet::LaneletMapPtr & lanelet_map_ptr);
  void clear_route();
  void set_next_subroute();

  bool8_t is_route_ready();

  bool8_t needs_new_trajectory(const State & state);
  TrajectoryPoint get_sub_goal();
  bool8_t has_arrived_goal(const State & state);
  bool8_t has_arrived_subroute_goal(const State & state);

  bool8_t is_vehicle_stopped(const State & state);

  RouteWithType get_current_subroute(const State & ego_state);
  PlannerType get_planner_type();
  uchar8_t get_desired_gear(const State & state);
  std::vector<RouteWithType> get_subroutes();

  // relay to trajectory_manager
  bool8_t is_trajectory_ready();
  void set_trajectory(const Trajectory & trajectory);
  Trajectory get_trajectory(const State & state);
  size_t get_remaining_length(const State & state);

private:
  RouteWithType get_current_subroute();

  ParkingDirection get_parking_direction(
    const TrajectoryPoint & parking_point,
    const TrajectoryPoint & closest_lane_point);
  std::vector<RouteWithType> m_subroutes;
  std::size_t m_current_subroute;

  // parameters
  PlannerConfig m_config;

  // TODO(mitsudome-r): remove the manager once splitting of trajectory gets unnecessary
  TrajectoryManager m_trajectory_manager;

  bool8_t m_is_trajectory_complete;
};
}  // namespace behavior_planner
}  // namespace autoware

#endif  // BEHAVIOR_PLANNER__BEHAVIOR_PLANNER_HPP_