Skip to content

9.0.0

Latest
Latest
Compare
Choose a tag to compare
@github-actions github-actions released this 30 Jan 02:37
9.0.0
688dfaa

Description

Abstract

This pull request introduces the change of exposing direct access to Entity objects through the API. This change increases flexibility of using the API and removes the need to have many forwarding functions to Entities member functions.

Background

This pull request is one of many that aim to modularize the scenario_simulator_v2.

The main goal of this PR was to remove numerous member functions of EntityManager (and subsequently some of API too) that forwarded calls to Entities member functions:
https://github.com/tier4/scenario_simulator_v2/blob/0dbf4ec0e573372306716f8d77933891564b36d6/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L162-L216

This has largely been achieved by exposing direct access to Entity and its member functions through the API::getEntity function:
https://github.com/tier4/scenario_simulator_v2/blob/8940b3e2f127bbca92295a91d580887030e45be6/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp#L303

The following change was to adjust all cpp mock scenarios to use the new interface.
This is the main reason why this PR is so large - all mock scenarios had to be corrected.

Scenarios using the new interface have been changed similarly to the example below.
Before:
https://github.com/tier4/scenario_simulator_v2/blob/0dbf4ec0e573372306716f8d77933891564b36d6/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp#L41-L64
After:
https://github.com/tier4/scenario_simulator_v2/blob/8940b3e2f127bbca92295a91d580887030e45be6/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp#L41-L64

Similar changes had to be applied to the whole codebase relying on the API like simulator_core.hpp.

What is more, EgoEntity has been modified in such a way, that the function
https://github.com/tier4/scenario_simulator_v2/blob/0dbf4ec0e573372306716f8d77933891564b36d6/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp#L68
has been replaced with a set of other functions below:
https://github.com/tier4/scenario_simulator_v2/blob/8940b3e2f127bbca92295a91d580887030e45be6/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp#L155-L164

This change simplifies the interface of calling Ego specific actions like engage or sendCooperateCommand.

Details

Below are the detailed interface changes that have been made to EntityStatus, EntityBase, EntityManager and the API.

EntityStatus

  • Renamed laneMatchingSucceed to isInLanelet:
    auto laneMatchingSucceed() const noexcept -> bool;
    ->
    auto isInLanelet() const noexcept -> bool;

EntityBase

  • Removed forwarded using macro to EntityStatus: laneMatchingSucceed:
    laneMatchingSucceed

  • Removed asFieldOperatorApplication, reachPosition and requestClearRoute:
    auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &;
    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;
    void requestClearRoute();

  • Added: isStopped, isInPosition and isInLanelet.

auto isStopped() const -> bool;

auto isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const -> bool;
auto isInPosition(const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool;

auto isInLanelet() const -> bool;
auto isInLanelet(const lanelet::Id lanelet_id, std::optional<double> tolerance = std::nullopt) const -> bool;

Note: Maybe it's a good idea to provide that tolerance is always of type std::optional<double>?

EgoEntity

  • Removed asFieldOperatorApplication:
    auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & override;

FieldOperatorApplicationFor

  • Added visibility to members of FieldOperatorApplication (see this comment).
  • EgoEntity now exposes these inherited members as a replacement of previous asFieldOperatorApplication:
auto isStopRequested() const -> bool;
auto isEngageable() const -> bool;
auto isEngaged() const -> bool;
auto engage() -> void;
auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void;
auto getAutowareStateName() const -> const std::string &;
auto getEmergencyStateName() const -> const std::string &;
auto getMinimumRiskManeuverBehaviorName() const -> const std::string &;
auto getMinimumRiskManeuverStateName() const -> const std::string &;
auto getTurnIndicatorsCommandName() const -> std::string;
auto requestAutoModeForCooperation(const std::string & module_name, const bool enable) -> void;
auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray;

EntityManager

  • Removed forwarded using macro to EntityBase: activateOutOfRangeJob, asFieldOperatorApplication, cancelRequest, get2DPolygon, getBehaviorParameter, getBoundingBox, getCanonicalizedStatusBeforeUpdate, getCurrentAccel, getCurrentTwist, getDefaultMatchingDistanceForLaneletPoseCalculation, getEntityType, getEntityTypename, getLinearJerk, getRouteLanelets, getStandStillDuration, getTraveledDistance, isControlledBySimulator, laneMatchingSucceed, reachPosition, requestAcquirePosition, requestAssignRoute, requestClearRoute, requestFollowTrajectory, requestLaneChange, requestSpeedChange, requestSynchronize, requestWalkStraight, setAcceleration, setAccelerationLimit, setAccelerationRateLimit, setBehaviorParameter, setControlledBySimulator, setDecelerationLimit, setDecelerationRateLimit, setLinearJerk, setLinearVelocity, setMapPose, setTwist, setVelocityLimit:
    FORWARD_TO_ENTITY(activateOutOfRangeJob);,
    FORWARD_TO_ENTITY(asFieldOperatorApplication);,
    FORWARD_TO_ENTITY(cancelRequest);,
    FORWARD_TO_ENTITY(get2DPolygon);,
    FORWARD_TO_ENTITY(getBehaviorParameter);,
    FORWARD_TO_ENTITY(getBoundingBox);,
    FORWARD_TO_ENTITY(getCanonicalizedStatusBeforeUpdate);,
    FORWARD_TO_ENTITY(getCurrentAccel);,
    FORWARD_TO_ENTITY(getCurrentTwist);,
    FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation);,
    FORWARD_TO_ENTITY(getEntityType);,
    FORWARD_TO_ENTITY(getEntityTypename);,
    FORWARD_TO_ENTITY(getLinearJerk);,
    FORWARD_TO_ENTITY(getRouteLanelets);,
    FORWARD_TO_ENTITY(getStandStillDuration);,
    FORWARD_TO_ENTITY(getTraveledDistance);,
    FORWARD_TO_ENTITY(isControlledBySimulator);,
    FORWARD_TO_ENTITY(laneMatchingSucceed);,
    FORWARD_TO_ENTITY(reachPosition);,
    FORWARD_TO_ENTITY(requestAcquirePosition);,
    FORWARD_TO_ENTITY(requestAssignRoute);,
    FORWARD_TO_ENTITY(requestClearRoute);,
    FORWARD_TO_ENTITY(requestFollowTrajectory);,
    FORWARD_TO_ENTITY(requestLaneChange);,
    FORWARD_TO_ENTITY(requestSpeedChange);,
    FORWARD_TO_ENTITY(requestSynchronize);,
    FORWARD_TO_ENTITY(requestWalkStraight);,
    FORWARD_TO_ENTITY(setAcceleration);,
    FORWARD_TO_ENTITY(setAccelerationLimit);,
    FORWARD_TO_ENTITY(setAccelerationRateLimit);,
    FORWARD_TO_ENTITY(setBehaviorParameter);,
    FORWARD_TO_ENTITY(setControlledBySimulator);,
    FORWARD_TO_ENTITY(setDecelerationLimit);,
    FORWARD_TO_ENTITY(setDecelerationRateLimit);,
    FORWARD_TO_ENTITY(setLinearJerk);,
    FORWARD_TO_ENTITY(setLinearVelocity);,
    FORWARD_TO_ENTITY(setMapPose);,
    FORWARD_TO_ENTITY(setTwist);,
    FORWARD_TO_ENTITY(setVelocityLimit);

  • Removed is, isStopping, isInLanelet, checkCollision, getEntityStatus, getCurrentAction:
    bool is(const std::string & name) const;
    bool isStopping(const std::string & name) const;
    bool isInLanelet(const std::string & name, const lanelet::Id lanelet_id, const double tolerance);
    bool checkCollision(const std::string & first_entity_name, const std::string & second_entity_name);
    auto getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus &;
    auto getCurrentAction(const std::string & name) const -> std::string;

  • Renamed entityExists to isEntitySpawned
    auto entityExists(const std::string & name) -> bool;
    ->
    auto isEntitySpawned(const std::string & name) -> bool;

  • Renamed getEntity to getEntityOrNullptr (which does not throw an exception but returns nullptr)
    auto getEntity(const std::string & name) const -> std::shared_ptr<entity::EntityBase>;
    ->
    auto getEntityOrNullptr(const std::string & name) const -> std::shared_ptr<entity::EntityBase>;

  • Renamed isEgoSpawned to isAnyEgoSpawned:
    auto isEgoSpawned() const -> bool;
    ->
    auto isAnyEgoSpawned() const -> bool;

  • Added getEntity (which throws an exception).

auto getEntity(const std::string & name) const -> std::shared_ptr<entity::EntityBase>;
  • Added getEgoEntity:
auto getEgoEntity(const std::string & name) const -> std::shared_ptr<entity::EgoEntity>;
auto getEgoEntity() const -> std::shared_ptr<entity::EgoEntity>;

API

  • Removed forwarded using macro to EntityManager: activateOutOfRangeJob, asFieldOperatorApplication, cancelRequest, checkCollision, entityExists, getBehaviorParameter, getBoundingBox, getCanonicalizedStatusBeforeUpdate, getCurrentAccel, getCurrentAction, getCurrentTwist, getDefaultMatchingDistanceForLaneletPoseCalculation, getEgoName, getEntityNames, getEntityStatus, getLinearJerk, getStandStillDuration, getTraveledDistance, isEgoSpawned, isInLanelet, laneMatchingSucceed, reachPosition, requestAcquirePosition, requestAssignRoute, requestClearRoute, requestFollowTrajectory, requestSpeedChange, requestSynchronize, requestWalkStraight, setAcceleration, setAccelerationLimit, setAccelerationRateLimit, setBehaviorParameter, setDecelerationLimit, setDecelerationRateLimit, setLinearVelocity, setMapPose, setTwist, setVelocityLimit:
    FORWARD_TO_ENTITY_MANAGER(activateOutOfRangeJob);,
    FORWARD_TO_ENTITY_MANAGER(asFieldOperatorApplication);,
    FORWARD_TO_ENTITY_MANAGER(cancelRequest);,
    FORWARD_TO_ENTITY_MANAGER(checkCollision);,
    FORWARD_TO_ENTITY_MANAGER(entityExists);,
    FORWARD_TO_ENTITY_MANAGER(getBehaviorParameter);,
    FORWARD_TO_ENTITY_MANAGER(getBoundingBox);,
    FORWARD_TO_ENTITY_MANAGER(getCanonicalizedStatusBeforeUpdate);,
    FORWARD_TO_ENTITY_MANAGER(getCurrentAccel);,
    FORWARD_TO_ENTITY_MANAGER(getCurrentAction);,
    FORWARD_TO_ENTITY_MANAGER(getCurrentTwist);,
    FORWARD_TO_ENTITY_MANAGER(getDefaultMatchingDistanceForLaneletPoseCalculation);,
    FORWARD_TO_ENTITY_MANAGER(getEgoName);,
    FORWARD_TO_ENTITY_MANAGER(getEntityNames);,
    FORWARD_TO_ENTITY_MANAGER(getEntityStatus);,
    FORWARD_TO_ENTITY_MANAGER(getLinearJerk);,
    FORWARD_TO_ENTITY_MANAGER(getStandStillDuration);,
    FORWARD_TO_ENTITY_MANAGER(getTraveledDistance);,
    FORWARD_TO_ENTITY_MANAGER(isEgoSpawned);,
    FORWARD_TO_ENTITY_MANAGER(isInLanelet);,
    FORWARD_TO_ENTITY_MANAGER(laneMatchingSucceed);,
    FORWARD_TO_ENTITY_MANAGER(reachPosition);,
    FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition);,
    FORWARD_TO_ENTITY_MANAGER(requestAssignRoute);,
    FORWARD_TO_ENTITY_MANAGER(requestClearRoute);,
    FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory);,
    FORWARD_TO_ENTITY_MANAGER(requestSpeedChange);,
    FORWARD_TO_ENTITY_MANAGER(requestSynchronize);,
    FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);,
    FORWARD_TO_ENTITY_MANAGER(setAcceleration);,
    FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit);,
    FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit);,
    FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter);,
    FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit);,
    FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit);,
    FORWARD_TO_ENTITY_MANAGER(setLinearVelocity);,
    FORWARD_TO_ENTITY_MANAGER(setMapPose);,
    FORWARD_TO_ENTITY_MANAGER(setTwist);,
    FORWARD_TO_ENTITY_MANAGER(setVelocityLimit);

  • Removed setEntityStatus, requestLaneChange and getTimeHeadway:
    auto setEntityStatus(...) -> void;
    void requestLaneChange(...);
    std::optional<double> getTimeHeadway(const std::string & from, const std::string & to);

  • Added checkCollision:

auto API::checkCollision(const std::string & first_entity_name, const std::string & second_entity_name); -> bool
  • Added forwarded using macro to EntityManager: isEntitySpawned, isAnyEgoSpawned, getEntityOrNullptr, getEgoEntity:
FORWARD_TO_ENTITY_MANAGER(isEntitySpawned);
FORWARD_TO_ENTITY_MANAGER(isAnyEgoSpawned);
FORWARD_TO_ENTITY_MANAGER(getEntityOrNullptr);
FORWARD_TO_ENTITY_MANAGER(getEgoEntity);

Examples of interface changes

Below are many examples of how to use the interface after the changes.

entityExists -> isEntitySpawned

api_.entityExists("bob")

->

api_.isEntitySpawned("bob")

is<>

entity_manager_ptr_->is<entity::EgoEntity>(entity_name)

->

auto entity = api_.getEntity(entity_name);   
entity->is<entity::EgoEntity>())

getEntity -> getEntityOrNullptr

if (const auto from_entity = core->getEntity(from_entity_name)) {...}

->

if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) {...}

reachPosition -> isInPosition

api_.reachPosition("npc",traffic_simulator::helper::constructLaneletPose(34579, 0.0), 5)

->

const auto entity = getEntity("npc");
entity->isInPosition(traffic_simulator::helper::constructLaneletPose(34579, 0.0), 5)

getCurrentAction

api_.getCurrentAction("pedestrian")

->

api_.getEntity("pedestrian")->getCurrentAction()

setEntityStatus -> setStatus

api_.setEntityStatus(
"ego",
  traffic_simulator::helper::constructCanonicalizedLaneletPose(
  34513, 0.0, 0.0, api_.getHdmapUtils()),
  traffic_simulator::helper::constructActionStatus(10));
  api_.requestSpeedChange("ego", 10, true);
  const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose(
  traffic_simulator::helper::constructCanonicalizedLaneletPose(
  34408, 1.0, 0.0, api_.getHdmapUtils()));
  api_.requestAcquirePosition("ego", goal_pose);

->

auto ego_entity = api_.getEntity("ego");
ego_entity->setStatus(
  traffic_simulator::helper::constructCanonicalizedLaneletPose(
  34513, 0.0, 0.0, api_.getHdmapUtils()),
  traffic_simulator::helper::constructActionStatus(10));
  ego_entity->requestSpeedChange(10, true);
  const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose(
  traffic_simulator::helper::constructCanonicalizedLaneletPose(
  34408, 1.0, 0.0, api_.getHdmapUtils()));
  ego_entity->requestAcquirePosition(goal_pose);

asFieldOperatorApplication -> replacements

while (!api_.asFieldOperatorApplication("ego").engaged()) {
    if (api_.asFieldOperatorApplication("ego").engageable()) {
      api_.asFieldOperatorApplication("ego").engage();
      }

->

auto ego_entity = api_.getEgoEntity("ego");    
while (!ego_entity->isEngaged()) {
    if (ego_entity->isEngageable()) {
      ego_entity->engage();
      }

setMapPose(name, entity_status.pose);
setTwist(name, entity_status.action_status.twist);
setAcceleration(name, entity_status.action_status.accel);
entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute();
entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose});
entity_manager_ptr_->asFieldOperatorApplication(name).engage();

->

auto ego_entity = entity_manager_ptr_->getEgoEntity(name);
ego_entity->setMapPose(entity_status.pose);
ego_entity->setTwist(entity_status.action_status.twist);
ego_entity->setAcceleration(entity_status.action_status.accel);
ego_entity->requestReplanRoute({goal_pose});

Others

api_.setLinearVelocity("ego", 0.0);
api_.requestSpeedChange("ego", 15, true);
api_.setBehaviorParameter("ego", behavior_parameter);

->

auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(0.0);
ego_entity->requestSpeedChange(15, true)
ego_entity->setBehaviorParameter(behavior_parameter);

double ego_linear_acceleration = api_.getCurrentAccel("ego").linear.x;
double ego_linear_velocity = api_.getCurrentTwist("ego").linear.x;

->

double ego_linear_acceleration = api_.getEntity("ego")->getCurrentAccel().linear.x;
double ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;

References

INTERNAL LINK1
INTERNAL LINK2

Destructive Changes

--

Known Limitations

--

Related Issues

Assets 2
Loading