scenario_simulator_v2 C++ API
ego_entity.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_
16 #define TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_
17 
18 #include <algorithm>
19 #include <boost/filesystem.hpp>
21 #include <get_parameter/get_parameter.hpp>
22 #include <memory>
23 #include <optional>
24 #include <string>
27 #include <traffic_simulator_msgs/msg/entity_type.hpp>
28 #include <vector>
29 
30 namespace traffic_simulator
31 {
32 namespace entity
33 {
35 {
36  bool is_controlled_by_simulator_{false};
37  std::optional<double> target_speed_;
38  traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_;
39  std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> polyline_trajectory_;
40 
41 public:
42  explicit EgoEntity() = delete;
43 
44  explicit EgoEntity(
46  const std::shared_ptr<hdmap_utils::HdMapUtils> &,
47  const traffic_simulator_msgs::msg::VehicleParameters &, const Configuration &,
48  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &);
49 
50  explicit EgoEntity(EgoEntity &&) = delete;
51 
52  explicit EgoEntity(const EgoEntity &) = delete;
53 
54  ~EgoEntity() override = default;
55 
56  auto operator=(EgoEntity &&) -> EgoEntity & = delete;
57 
58  auto operator=(const EgoEntity &) -> EgoEntity & = delete;
59 
60  auto getCurrentAction() const -> std::string override;
61 
62  auto getCurrentPose() const -> const geometry_msgs::msg::Pose &;
63 
64  auto getDefaultDynamicConstraints() const
65  -> const traffic_simulator_msgs::msg::DynamicConstraints & override;
66 
67  auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override;
68 
69  auto getEntityStatus(const double, const double) const -> const CanonicalizedEntityStatus;
70 
71  auto getEntityTypename() const -> const std::string & override;
72 
73  auto getObstacle() -> std::optional<traffic_simulator_msgs::msg::Obstacle> override;
74 
75  auto getRouteLanelets(double horizon = 100) -> lanelet::Ids override;
76 
77  auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
78 
79  auto updateFieldOperatorApplication() -> void;
80 
81  void onUpdate(double current_time, double step_time) override;
82 
83  void requestAcquirePosition(const CanonicalizedLaneletPose &) override;
84 
86  const CanonicalizedLaneletPose &, const traffic_simulator::RouteOption &) override;
87 
88  void requestAcquirePosition(const geometry_msgs::msg::Pose &) override;
89 
91  const geometry_msgs::msg::Pose &, const traffic_simulator::RouteOption &) override;
92 
93  void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override;
94 
95  void requestAssignRoute(
96  const std::vector<CanonicalizedLaneletPose> &, const traffic_simulator::RouteOption &) override;
97 
98  void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override;
99 
100  void requestAssignRoute(
101  const std::vector<geometry_msgs::msg::Pose> &, const traffic_simulator::RouteOption &) override;
102 
104  const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;
105 
106  auto requestLaneChange(const lanelet::Id) -> void override;
107 
108  auto requestLaneChange(const lane_change::Parameter &) -> void override;
109 
110  auto requestSpeedChange(
111  const double, const speed_change::Transition, const speed_change::Constraint,
112  const bool continuous) -> void override;
113 
114  auto requestSpeedChange(
115  const speed_change::RelativeTargetSpeed &, const speed_change::Transition,
116  const speed_change::Constraint, const bool continuous) -> void override;
117 
118  auto requestClearRoute() -> void;
119 
120  auto requestReplanRoute(
121  const std::vector<geometry_msgs::msg::PoseStamped> & route,
122  const bool allow_goal_modification = false) -> void;
123 
124  auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void;
125 
126  auto isControlledBySimulator() const -> bool override;
127 
128  auto setControlledBySimulator(bool state) -> void override
129  {
130  is_controlled_by_simulator_ = state;
131  }
132 
133  auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &)
134  -> void override;
135 
136  void requestSpeedChange(double, bool continuous) override;
137 
138  void requestSpeedChange(
139  const speed_change::RelativeTargetSpeed & target_speed, bool continuous) override;
140 
141  auto setVelocityLimit(double) -> void override;
142 
143  auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override;
144 
145  template <typename ParameterType>
146  auto setParameter(const std::string & name, const ParameterType & default_value = {})
147  -> ParameterType
148  {
149  return declare_parameter<ParameterType>(name, default_value);
150  }
151 
152  template <typename... Ts>
153  auto setStatus(Ts &&... xs) -> void
154  {
155  if (status_->getTime() > 0 && not isControlledBySimulator()) {
157  "You cannot set entity status to the ego vehicle named ", std::quoted(status_->getName()),
158  " after starting scenario.");
159  } else {
160  EntityBase::setStatus(std::forward<decltype(xs)>(xs)...);
161  }
162  }
163 
164  auto engage() -> void;
165  auto isEngaged() const -> bool;
166  auto isEngageable() const -> bool;
167  auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void;
168  auto getMinimumRiskManeuverBehaviorName() const -> std::string;
169  auto getMinimumRiskManeuverStateName() const -> std::string;
170  auto getEmergencyStateName() const -> std::string;
171  auto getTurnIndicatorsCommandName() const -> std::string;
172 };
173 } // namespace entity
174 } // namespace traffic_simulator
175 #endif // TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_
Definition: ego_entity.hpp:35
auto isEngageable() const -> bool
Definition: ego_entity.cpp:81
auto requestReplanRoute(const std::vector< geometry_msgs::msg::PoseStamped > &route, const bool allow_goal_modification=false) -> void
Definition: ego_entity.cpp:391
auto getObstacle() -> std::optional< traffic_simulator_msgs::msg::Obstacle > override
Definition: ego_entity.cpp:138
auto setMapPose(const geometry_msgs::msg::Pose &map_pose) -> void override
Definition: ego_entity.cpp:452
void requestAssignRoute(const std::vector< CanonicalizedLaneletPose > &) override
Definition: ego_entity.cpp:224
auto isEngaged() const -> bool
Definition: ego_entity.cpp:79
void requestAcquirePosition(const CanonicalizedLaneletPose &) override
Definition: ego_entity.cpp:198
auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override
Definition: ego_entity.cpp:159
auto getEntityStatus(const double, const double) const -> const CanonicalizedEntityStatus
auto getCurrentPose() const -> const geometry_msgs::msg::Pose &
Definition: ego_entity.cpp:154
auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints &override
Definition: ego_entity.cpp:417
auto requestClearRoute() -> void
Definition: ego_entity.cpp:389
auto setParameter(const std::string &name, const ParameterType &default_value={}) -> ParameterType
Definition: ego_entity.hpp:146
void onUpdate(double current_time, double step_time) override
Definition: ego_entity.cpp:171
auto setControlledBySimulator(bool state) -> void override
Definition: ego_entity.hpp:128
auto engage() -> void
Definition: ego_entity.cpp:77
auto getEntityTypename() const -> const std::string &override
Definition: ego_entity.cpp:132
auto requestFollowTrajectory(const std::shared_ptr< traffic_simulator_msgs::msg::PolylineTrajectory > &) -> void override
Definition: ego_entity.cpp:349
auto setVelocityLimit(double) -> void override
Definition: ego_entity.cpp:446
auto operator=(EgoEntity &&) -> EgoEntity &=delete
auto requestLaneChange(const lanelet::Id) -> void override
Definition: ego_entity.cpp:357
auto getCurrentAction() const -> std::string override
Definition: ego_entity.cpp:122
auto sendCooperateCommand(const std::string &module_name, const std::string &command) -> void
Definition: ego_entity.cpp:83
auto operator=(const EgoEntity &) -> EgoEntity &=delete
auto getRouteLanelets(double horizon=100) -> lanelet::Ids override
Definition: ego_entity.cpp:143
auto updateFieldOperatorApplication() -> void
Definition: ego_entity.cpp:169
EgoEntity(const EgoEntity &)=delete
auto requestAutoModeForCooperation(const std::string &module_name, bool enable) -> void
Definition: ego_entity.cpp:89
auto setStatus(Ts &&... xs) -> void
Definition: ego_entity.hpp:153
auto getTurnIndicatorsCommandName() const -> std::string
Definition: ego_entity.cpp:106
auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void override
Definition: ego_entity.cpp:423
auto getMinimumRiskManeuverStateName() const -> std::string
Definition: ego_entity.cpp:99
auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override
Definition: ego_entity.cpp:124
auto requestSpeedChange(const double, const speed_change::Transition, const speed_change::Constraint, const bool continuous) -> void override
Definition: ego_entity.cpp:373
auto getEmergencyStateName() const -> std::string
Definition: ego_entity.cpp:104
auto isControlledBySimulator() const -> bool override
Definition: ego_entity.cpp:347
auto getMinimumRiskManeuverBehaviorName() const -> std::string
Definition: ego_entity.cpp:94
const std::string name
Definition: entity_base.hpp:347
std::shared_ptr< CanonicalizedEntityStatus > status_
Definition: entity_base.hpp:354
virtual auto setStatus(const EntityStatus &status) -> void
Definition: entity_base.cpp:588
Definition: vehicle_entity.hpp:37
Definition: entity_status.hpp:32
Definition: lanelet_pose.hpp:28
#define THROW_SEMANTIC_ERROR(...)
Definition: exception.hpp:59
Definition: lanelet_wrapper.hpp:40
geometry_msgs::msg::Pose Pose
Definition: lanelet_map.hpp:30
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
geometry_msgs::msg::PoseStamped PoseStamped
Definition: lanelet_wrapper.hpp:67
auto route(Ts &&... xs)
Definition: route.hpp:30
Transition
Definition: speed_change.hpp:26
Definition: api.hpp:32
Definition: junit5.hpp:25
std::string string
Definition: junit5.hpp:26
Definition: field_operator_application.hpp:53
void RouteOption
Definition: field_operator_application.hpp:173
Definition: configuration.hpp:31