Skip to content

Commit

Permalink
Update some member functions of FieldOperatorApplication to be non-…
Browse files Browse the repository at this point in the history
…virtual

Signed-off-by: yamacir-kit <[email protected]>
  • Loading branch information
yamacir-kit committed Dec 13, 2024
1 parent be05813 commit a9d81db
Show file tree
Hide file tree
Showing 5 changed files with 11 additions and 54 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ struct FieldOperatorApplication : public rclcpp::Node,
SubscriberWrapper<autoware_adapi_v1_msgs::msg::MrmState> getMrmState;
SubscriberWrapper<tier4_planning_msgs::msg::PathWithLaneId> getPathWithLaneId;
SubscriberWrapper<tier4_planning_msgs::msg::Trajectory> getTrajectory;
SubscriberWrapper<autoware_vehicle_msgs::msg::TurnIndicatorsCommand> getTurnIndicatorsCommandImpl;
SubscriberWrapper<autoware_vehicle_msgs::msg::TurnIndicatorsCommand> getTurnIndicatorsCommand;

ServiceWithValidation<autoware_adapi_v1_msgs::srv::ClearRoute> requestClearRoute;
ServiceWithValidation<tier4_rtc_msgs::srv::CooperateCommands> requestCooperateCommands;
Expand Down Expand Up @@ -202,23 +202,20 @@ struct FieldOperatorApplication : public rclcpp::Node,

virtual auto clearRoute() -> void = 0;

virtual auto getAutowareStateName() const -> std::string = 0;
auto getAutowareStateName() const { return autoware_state; }

virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0;
auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; }

virtual auto getMinimumRiskManeuverStateName() const -> std::string = 0;
auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; }

virtual auto getEmergencyStateName() const -> std::string = 0;
auto getEmergencyStateName() const { return minimum_risk_maneuver_state; }

virtual auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray = 0;

/* */ auto initialized() const noexcept { return initialize_was_called; }

virtual auto requestAutoModeForCooperation(const std::string &, bool) -> void = 0;

virtual auto getTurnIndicatorsCommand() const
-> autoware_vehicle_msgs::msg::TurnIndicatorsCommand = 0;

virtual auto rethrow() const noexcept(false) -> void;

virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,31 +22,16 @@ namespace concealer
template <>
struct FieldOperatorApplicationFor<AutowareUniverse> : public FieldOperatorApplication
{
template <typename... Ts>
CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs)
: FieldOperatorApplication(std::forward<decltype(xs)>(xs)...)
{
}
using FieldOperatorApplication::FieldOperatorApplication;

auto engage() -> void override;

auto engageable() const -> bool override;

auto engaged() const -> bool override;

auto getAutowareStateName() const -> std::string override;

auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override;

auto getTurnIndicatorsCommand() const
-> autoware_vehicle_msgs::msg::TurnIndicatorsCommand override;

auto getEmergencyStateName() const -> std::string override;

auto getMinimumRiskManeuverBehaviorName() const -> std::string override;

auto getMinimumRiskManeuverStateName() const -> std::string override;

auto initialize(const geometry_msgs::msg::Pose &) -> void override;

auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ class ServiceWithValidation
{
public:
template <typename... Ts>
explicit TimeoutError(Ts &&... xs) : common::scenario_simulator_exception::Error(std::forward<decltype(xs)>(xs)...)
explicit TimeoutError(Ts &&... xs)
: common::scenario_simulator_exception::Error(std::forward<decltype(xs)>(xs)...)
{
}
};
Expand Down
4 changes: 3 additions & 1 deletion external/concealer/src/field_operator_application.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ auto toAutowareStateString(std::uint8_t state) -> char const *
#undef CASE
}

// clang-format off
FieldOperatorApplication::FieldOperatorApplication(const pid_t pid)
: rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)),
process_id(pid),
Expand Down Expand Up @@ -113,7 +114,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid)
}),
getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this),
getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this),
getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this),
getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this),
requestClearRoute("/api/routing/clear_route", *this),
requestCooperateCommands("/api/external/set/rtc_commands", *this),
requestEngage("/api/external/set/engage", *this),
Expand All @@ -125,6 +126,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid)
requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this)
{
}
// clang-format on

FieldOperatorApplication::~FieldOperatorApplication()
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -350,34 +350,6 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::getWaypoints() const
return waypoints;
}

auto FieldOperatorApplicationFor<AutowareUniverse>::getTurnIndicatorsCommand() const
-> autoware_vehicle_msgs::msg::TurnIndicatorsCommand
{
return getTurnIndicatorsCommandImpl();
}

auto FieldOperatorApplicationFor<AutowareUniverse>::getAutowareStateName() const -> std::string
{
return autoware_state;
}

auto FieldOperatorApplicationFor<AutowareUniverse>::getEmergencyStateName() const -> std::string
{
return minimum_risk_maneuver_state;
}

auto FieldOperatorApplicationFor<AutowareUniverse>::getMinimumRiskManeuverBehaviorName() const
-> std::string
{
return minimum_risk_maneuver_behavior;
}

auto FieldOperatorApplicationFor<AutowareUniverse>::getMinimumRiskManeuverStateName() const
-> std::string
{
return minimum_risk_maneuver_state;
}

auto FieldOperatorApplicationFor<AutowareUniverse>::setVelocityLimit(double velocity_limit) -> void
{
task_queue.delay([this, velocity_limit]() {
Expand Down

0 comments on commit a9d81db

Please sign in to comment.