scenario_simulator_v2 C++ API
|
#include <entity_base.hpp>
Public Member Functions | |
EntityBase (const std::string &name, const CanonicalizedEntityStatus &entity_status, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils_ptr) | |
virtual | ~EntityBase ()=default |
virtual void | appendDebugMarker (visualization_msgs::msg::MarkerArray &) |
virtual auto | asFieldOperatorApplication () const -> concealer::FieldOperatorApplication & |
virtual void | cancelRequest () |
auto | getBoundingBox () const noexcept -> const traffic_simulator_msgs::msg::BoundingBox & |
Get BoundingBox of the entity. More... | |
auto | getCanonicalizedStatus () const noexcept -> const CanonicalizedEntityStatus & |
Get CanonicalizedStatus of the entity. More... | |
auto | getCanonicalizedStatusBeforeUpdate () const noexcept -> const CanonicalizedEntityStatus & |
Get CanonicalizedStatusBeforeUpdate of the entity. More... | |
auto | getCurrentAccel () const noexcept -> const geometry_msgs::msg::Accel & |
Get CurrentAccel of the entity. More... | |
auto | getCurrentTwist () const noexcept -> const geometry_msgs::msg::Twist & |
Get CurrentTwist of the entity. More... | |
auto | getDynamicConstraints () const noexcept -> traffic_simulator_msgs::msg::DynamicConstraints |
Get DynamicConstraints of the entity. More... | |
auto | getEntitySubtype () const noexcept -> const traffic_simulator_msgs::msg::EntitySubtype & |
Get EntitySubtype of the entity. More... | |
auto | getEntityType () const noexcept -> const traffic_simulator_msgs::msg::EntityType & |
Get EntityType of the entity. More... | |
auto | getLinearJerk () const noexcept -> double |
Get LinearJerk of the entity. More... | |
auto | getMapPose () const noexcept -> const geometry_msgs::msg::Pose & |
Get MapPose of the entity. More... | |
auto | getStandStillDuration () const noexcept -> double |
Get StandStillDuration of the entity. More... | |
auto | getTraveledDistance () const noexcept -> double |
Get TraveledDistance of the entity. More... | |
auto | laneMatchingSucceed () const -> bool |
auto | get2DPolygon () const -> std::vector< geometry_msgs::msg::Point > |
virtual auto | getCurrentAction () const -> std::string=0 |
virtual auto | getBehaviorParameter () const -> traffic_simulator_msgs::msg::BehaviorParameter=0 |
virtual auto | getDefaultDynamicConstraints () const -> const traffic_simulator_msgs::msg::DynamicConstraints &=0 |
virtual auto | getEntityTypename () const -> const std::string &=0 |
virtual auto | getGoalPoses () -> std::vector< CanonicalizedLaneletPose >=0 |
auto | getCanonicalizedLaneletPose () const -> std::optional< CanonicalizedLaneletPose > |
auto | getCanonicalizedLaneletPose (const double matching_distance) const -> std::optional< CanonicalizedLaneletPose > |
virtual auto | getMaxAcceleration () const -> double=0 |
virtual auto | getMaxDeceleration () const -> double=0 |
virtual auto | getDefaultMatchingDistanceForLaneletPoseCalculation () const -> double |
virtual auto | getObstacle () -> std::optional< traffic_simulator_msgs::msg::Obstacle >=0 |
virtual auto | getRouteLanelets (const double horizon=100.0) -> lanelet::Ids=0 |
virtual auto | getWaypoints () -> const traffic_simulator_msgs::msg::WaypointsArray=0 |
virtual auto | onUpdate (const double current_time, const double step_time) -> void |
virtual auto | onPostUpdate (const double current_time, const double step_time) -> void |
void | resetDynamicConstraints () |
virtual void | requestAcquirePosition (const CanonicalizedLaneletPose &)=0 |
virtual void | requestAcquirePosition (const geometry_msgs::msg::Pose &)=0 |
virtual void | requestAssignRoute (const std::vector< CanonicalizedLaneletPose > &)=0 |
virtual void | requestAssignRoute (const std::vector< geometry_msgs::msg::Pose > &)=0 |
virtual void | requestLaneChange (const lanelet::Id) |
virtual void | requestLaneChange (const traffic_simulator::lane_change::Parameter &) |
void | requestLaneChange (const lane_change::AbsoluteTarget &target, const lane_change::TrajectoryShape trajectory_shape, const lane_change::Constraint &constraint) |
void | requestLaneChange (const lane_change::RelativeTarget &target, const lane_change::TrajectoryShape trajectory_shape, const lane_change::Constraint &constraint) |
virtual void | requestSpeedChange (const double target_speed, const speed_change::Transition transition, const speed_change::Constraint constraint, const bool continuous) |
virtual void | requestSpeedChange (const speed_change::RelativeTargetSpeed &target_speed, const speed_change::Transition transition, const speed_change::Constraint constraint, const bool continuous) |
virtual void | requestSpeedChange (const double target_speed, const bool continuous) |
virtual void | requestSpeedChange (const speed_change::RelativeTargetSpeed &target_speed, const bool continuous) |
virtual void | requestClearRoute () |
virtual auto | isControlledBySimulator () const -> bool |
virtual auto | setControlledBySimulator (const bool) -> void |
virtual auto | requestFollowTrajectory (const std::shared_ptr< traffic_simulator_msgs::msg::PolylineTrajectory > &) -> void |
virtual void | requestWalkStraight () |
virtual void | setAccelerationLimit (const double acceleration)=0 |
virtual void | setAccelerationRateLimit (const double acceleration_rate)=0 |
virtual void | setDecelerationLimit (const double deceleration)=0 |
virtual void | setDecelerationRateLimit (const double deceleration_rate)=0 |
void | setDynamicConstraints (const traffic_simulator_msgs::msg::DynamicConstraints &constraints) |
virtual void | setBehaviorParameter (const traffic_simulator_msgs::msg::BehaviorParameter &)=0 |
void | setOtherStatus (const std::unordered_map< std::string, CanonicalizedEntityStatus > &status) |
virtual auto | setStatus (const EntityStatus &status, const lanelet::Ids &lanelet_ids) -> void |
virtual auto | setStatus (const EntityStatus &status) -> void |
auto | setCanonicalizedStatus (const CanonicalizedEntityStatus &status) -> void |
virtual auto | setLinearAcceleration (const double linear_acceleration) -> void |
virtual auto | setLinearVelocity (const double linear_velocity) -> void |
virtual void | setTrafficLights (const std::shared_ptr< traffic_simulator::TrafficLightsBase > &traffic_lights) |
virtual auto | activateOutOfRangeJob (const double min_velocity, const double max_velocity, const double min_acceleration, const double max_acceleration, const double min_jerk, const double max_jerk) -> void |
virtual auto | setVelocityLimit (const double) -> void=0 |
virtual auto | setMapPose (const geometry_msgs::msg::Pose &map_pose) -> void |
auto | setTwist (const geometry_msgs::msg::Twist &twist) -> void |
auto | setAcceleration (const geometry_msgs::msg::Accel &accel) -> void |
auto | setAction (const std::string &action) -> void |
auto | setLinearJerk (const double liner_jerk) -> void |
void | stopAtCurrentPosition () |
void | updateEntityStatusTimestamp (const double current_time) |
bool | reachPosition (const geometry_msgs::msg::Pose &target_pose, const double tolerance) const |
bool | reachPosition (const CanonicalizedLaneletPose &lanelet_pose, const double tolerance) const |
bool | reachPosition (const std::string &target_name, const double tolerance) const |
auto | requestSynchronize (const std::string &target_name, const CanonicalizedLaneletPose &target_sync_pose, const CanonicalizedLaneletPose &entity_target, const double target_speed, const double tolerance) -> bool |
Public Attributes | |
const std::string | name |
bool | verbose |
Protected Attributes | |
std::shared_ptr< CanonicalizedEntityStatus > | status_ |
CanonicalizedEntityStatus | status_before_update_ |
std::shared_ptr< hdmap_utils::HdMapUtils > | hdmap_utils_ptr_ |
std::shared_ptr< traffic_simulator::TrafficLightsBase > | traffic_lights_ |
double | stand_still_duration_ = 0.0 |
double | traveled_distance_ = 0.0 |
double | prev_job_duration_ = 0.0 |
double | step_time_ = 0.0 |
std::unordered_map< std::string, CanonicalizedEntityStatus > | other_status_ |
std::optional< double > | target_speed_ |
traffic_simulator::job::JobList | job_list_ |
std::unique_ptr< traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner > | speed_planner_ |
|
explicit |
|
virtualdefault |
|
virtual |
This value was determined heuristically rather than for technical reasons.
Checking if the values of velocity, acceleration and jerk are within the acceptable range
This job is always ACTIVE
|
virtual |
Reimplemented in traffic_simulator::entity::VehicleEntity, and traffic_simulator::entity::PedestrianEntity.
|
virtual |
Reimplemented in traffic_simulator::entity::EgoEntity.
|
virtual |
Reimplemented in traffic_simulator::entity::VehicleEntity, and traffic_simulator::entity::PedestrianEntity.
auto traffic_simulator::entity::EntityBase::get2DPolygon | ( | ) | const -> std::vector<geometry_msgs::msg::Point> |
|
pure virtual |
|
inlinenoexcept |
Get BoundingBox of the entity.
auto traffic_simulator::entity::EntityBase::getCanonicalizedLaneletPose | ( | ) | const -> std::optional<CanonicalizedLaneletPose> |
auto traffic_simulator::entity::EntityBase::getCanonicalizedLaneletPose | ( | const double | matching_distance | ) | const -> std::optional<CanonicalizedLaneletPose> |
|
inlinenoexcept |
Get CanonicalizedStatus of the entity.
|
inlinenoexcept |
Get CanonicalizedStatusBeforeUpdate of the entity.
|
inlinenoexcept |
Get CurrentAccel of the entity.
|
pure virtual |
|
inlinenoexcept |
Get CurrentTwist of the entity.
|
pure virtual |
|
virtual |
Reimplemented in traffic_simulator::entity::VehicleEntity.
|
inlinenoexcept |
Get DynamicConstraints of the entity.
|
inlinenoexcept |
Get EntitySubtype of the entity.
|
inlinenoexcept |
Get EntityType of the entity.
|
pure virtual |
|
pure virtual |
|
inlinenoexcept |
Get LinearJerk of the entity.
|
inlinenoexcept |
Get MapPose of the entity.
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
inlinenoexcept |
Get StandStillDuration of the entity.
|
inlinenoexcept |
Get TraveledDistance of the entity.
|
pure virtual |
|
virtual |
Reimplemented in traffic_simulator::entity::EgoEntity.
|
inline |
|
virtual |
|
virtual |
bool traffic_simulator::entity::EntityBase::reachPosition | ( | const CanonicalizedLaneletPose & | lanelet_pose, |
const double | tolerance | ||
) | const |
bool traffic_simulator::entity::EntityBase::reachPosition | ( | const geometry_msgs::msg::Pose & | target_pose, |
const double | tolerance | ||
) | const |
bool traffic_simulator::entity::EntityBase::reachPosition | ( | const std::string & | target_name, |
const double | tolerance | ||
) | const |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
inlinevirtual |
Reimplemented in traffic_simulator::entity::EgoEntity.
|
virtual |
void traffic_simulator::entity::EntityBase::requestLaneChange | ( | const lane_change::AbsoluteTarget & | target, |
const lane_change::TrajectoryShape | trajectory_shape, | ||
const lane_change::Constraint & | constraint | ||
) |
void traffic_simulator::entity::EntityBase::requestLaneChange | ( | const lane_change::RelativeTarget & | target, |
const lane_change::TrajectoryShape | trajectory_shape, | ||
const lane_change::Constraint & | constraint | ||
) |
|
inlinevirtual |
Reimplemented in traffic_simulator::entity::EgoEntity, and traffic_simulator::entity::VehicleEntity.
|
inlinevirtual |
Reimplemented in traffic_simulator::entity::VehicleEntity, and traffic_simulator::entity::EgoEntity.
|
virtual |
If the target entity reaches the target speed, return true.
Cancel speed change request.
If the target entity reaches the target speed, return true.
Cancel speed change request.
Reimplemented in traffic_simulator::entity::MiscObjectEntity, and traffic_simulator::entity::EgoEntity.
|
virtual |
Reimplemented in traffic_simulator::entity::MiscObjectEntity, and traffic_simulator::entity::EgoEntity.
|
virtual |
If the target entity reaches the target speed, return true.
If the target entity reaches the target speed, return true.
Cancel speed change request.
Reimplemented in traffic_simulator::entity::EgoEntity, and traffic_simulator::entity::MiscObjectEntity.
|
virtual |
Reimplemented in traffic_simulator::entity::EgoEntity.
auto traffic_simulator::entity::EntityBase::requestSynchronize | ( | const std::string & | target_name, |
const CanonicalizedLaneletPose & | target_sync_pose, | ||
const CanonicalizedLaneletPose & | entity_target, | ||
const double | target_speed, | ||
const double | tolerance | ||
) | -> bool |
Check if the entity has already arrived to the target lanelet.
Making entity speed up.
Making entity slow down.
Making entity keep the current speed.
|
virtual |
Reimplemented in traffic_simulator::entity::PedestrianEntity.
void traffic_simulator::entity::EntityBase::resetDynamicConstraints | ( | ) |
auto traffic_simulator::entity::EntityBase::setAcceleration | ( | const geometry_msgs::msg::Accel & | accel | ) | -> void |
|
pure virtual |
|
pure virtual |
auto traffic_simulator::entity::EntityBase::setAction | ( | const std::string & | action | ) | -> void |
|
pure virtual |
auto traffic_simulator::entity::EntityBase::setCanonicalizedStatus | ( | const CanonicalizedEntityStatus & | status | ) | -> void |
|
virtual |
Reimplemented in traffic_simulator::entity::EgoEntity.
|
pure virtual |
|
pure virtual |
void traffic_simulator::entity::EntityBase::setDynamicConstraints | ( | const traffic_simulator_msgs::msg::DynamicConstraints & | constraints | ) |
|
virtual |
auto traffic_simulator::entity::EntityBase::setLinearJerk | ( | const double | liner_jerk | ) | -> void |
|
virtual |
|
virtual |
Reimplemented in traffic_simulator::entity::EgoEntity.
void traffic_simulator::entity::EntityBase::setOtherStatus | ( | const std::unordered_map< std::string, CanonicalizedEntityStatus > & | status | ) |
|
virtual |
|
virtual |
|
virtual |
Reimplemented in traffic_simulator::entity::PedestrianEntity, and traffic_simulator::entity::VehicleEntity.
auto traffic_simulator::entity::EntityBase::setTwist | ( | const geometry_msgs::msg::Twist & | twist | ) | -> void |
|
pure virtual |
void traffic_simulator::entity::EntityBase::stopAtCurrentPosition | ( | ) |
void traffic_simulator::entity::EntityBase::updateEntityStatusTimestamp | ( | const double | current_time | ) |
|
protected |
|
protected |
const std::string traffic_simulator::entity::EntityBase::name |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
bool traffic_simulator::entity::EntityBase::verbose |