Skip to content

Commit

Permalink
fix(behavior_path_planner): move module output interface definition
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
satoshi-ota committed Feb 21, 2023
1 parent 873666f commit be019e5
Show file tree
Hide file tree
Showing 3 changed files with 49 additions and 46 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,17 @@

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>

#include <lanelet2_core/geometry/Lanelet.h>

#include <limits>
#include <memory>
#include <string>
#include <vector>
Expand All @@ -38,11 +42,16 @@ namespace behavior_path_planner
{
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
using autoware_planning_msgs::msg::PoseWithUuidStamped;
using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::PoseStamped;
using nav_msgs::msg::OccupancyGrid;
using nav_msgs::msg::Odometry;
using route_handler::RouteHandler;
using PlanResult = PathWithLaneId::SharedPtr;

struct BoolStamped
{
explicit BoolStamped(bool in_data) : data(in_data) {}
Expand All @@ -63,6 +72,46 @@ struct DrivableLanes
lanelet::ConstLanelets middle_lanes;
};

struct TurnSignalInfo
{
TurnSignalInfo()
{
turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
hazard_signal.command = HazardLightsCommand::NO_COMMAND;
}

// desired turn signal
TurnIndicatorsCommand turn_signal;
HazardLightsCommand hazard_signal;

geometry_msgs::msg::Pose desired_start_point;
geometry_msgs::msg::Pose desired_end_point;
geometry_msgs::msg::Pose required_start_point;
geometry_msgs::msg::Pose required_end_point;
};

struct BehaviorModuleOutput
{
BehaviorModuleOutput() = default;

// path planed by module
PlanResult path{};

TurnSignalInfo turn_signal_info{};

std::optional<PoseWithUuidStamped> modified_goal{};
};

struct CandidateOutput
{
CandidateOutput() = default;
explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {}
PathWithLaneId path_candidate{};
double lateral_shift{0.0};
double start_distance_to_path_change{std::numeric_limits<double>::lowest()};
double finish_distance_to_path_change{std::numeric_limits<double>::lowest()};
};

struct PlannerData
{
Odometry::ConstSharedPtr self_odometry{};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,6 @@

#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
#include <unique_identifier_msgs/msg/uuid.hpp>

Expand All @@ -47,38 +44,13 @@ namespace behavior_path_planner
{
using autoware_adapi_v1_msgs::msg::SteeringFactor;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
using autoware_planning_msgs::msg::PoseWithUuidStamped;
using rtc_interface::RTCInterface;
using steering_factor_interface::SteeringFactorInterface;
using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
using unique_identifier_msgs::msg::UUID;
using visualization_msgs::msg::MarkerArray;
using PlanResult = PathWithLaneId::SharedPtr;

struct BehaviorModuleOutput
{
BehaviorModuleOutput() = default;

// path planed by module
PlanResult path{};

TurnSignalInfo turn_signal_info{};

std::optional<PoseWithUuidStamped> modified_goal{};
};

struct CandidateOutput
{
CandidateOutput() = default;
explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {}
PathWithLaneId path_candidate{};
double lateral_shift{0.0};
double start_distance_to_path_change{std::numeric_limits<double>::lowest()};
double finish_distance_to_path_change{std::numeric_limits<double>::lowest()};
};

class SceneModuleInterface
{
public:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,24 +39,6 @@ using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using route_handler::RouteHandler;

struct TurnSignalInfo
{
TurnSignalInfo()
{
turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
hazard_signal.command = HazardLightsCommand::NO_COMMAND;
}

// desired turn signal
TurnIndicatorsCommand turn_signal;
HazardLightsCommand hazard_signal;

geometry_msgs::msg::Pose desired_start_point;
geometry_msgs::msg::Pose desired_end_point;
geometry_msgs::msg::Pose required_start_point;
geometry_msgs::msg::Pose required_end_point;
};

const std::map<std::string, uint8_t> signal_map = {
{"left", TurnIndicatorsCommand::ENABLE_LEFT},
{"right", TurnIndicatorsCommand::ENABLE_RIGHT},
Expand Down

0 comments on commit be019e5

Please sign in to comment.