Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RJD-1057 (3/5): Remove non-API member functions: EntityManager’s member functions forwarded to EntityBase (1/2) #1473

Open
wants to merge 65 commits into
base: master
Choose a base branch
from

Conversation

dmoszynski
Copy link
Contributor

@dmoszynski dmoszynski commented Dec 4, 2024

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:

#define FORWARD_TO_ENTITY(IDENTIFIER, ...) \
/*! \
@brief Forward to arguments to the EntityBase::IDENTIFIER function. \
@return return value of the EntityBase::IDENTIFIER function. \
@note This function was defined by FORWARD_TO_ENTITY macro.         \
*/ \
template <typename... Ts> \
decltype(auto) IDENTIFIER(const std::string & name, Ts &&... xs) __VA_ARGS__ \
try { \
return entities_.at(name)->IDENTIFIER(std::forward<decltype(xs)>(xs)...); \
} catch (const std::out_of_range &) { \
THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); \
} \
static_assert(true, "")
// clang-format on
FORWARD_TO_ENTITY(activateOutOfRangeJob, );
FORWARD_TO_ENTITY(asFieldOperatorApplication, const);
FORWARD_TO_ENTITY(cancelRequest, );
FORWARD_TO_ENTITY(get2DPolygon, const);
FORWARD_TO_ENTITY(getBehaviorParameter, const);
FORWARD_TO_ENTITY(getBoundingBox, const);
FORWARD_TO_ENTITY(getCanonicalizedStatusBeforeUpdate, const);
FORWARD_TO_ENTITY(getCurrentAccel, const);
FORWARD_TO_ENTITY(getCurrentTwist, const);
FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const);
FORWARD_TO_ENTITY(getEntityType, const);
FORWARD_TO_ENTITY(getEntityTypename, const);
FORWARD_TO_ENTITY(getLinearJerk, const);
FORWARD_TO_ENTITY(getRouteLanelets, const);
FORWARD_TO_ENTITY(getStandStillDuration, const);
FORWARD_TO_ENTITY(getTraveledDistance, const);
FORWARD_TO_ENTITY(isControlledBySimulator, );
FORWARD_TO_ENTITY(laneMatchingSucceed, const);
FORWARD_TO_ENTITY(reachPosition, const);
FORWARD_TO_ENTITY(requestAcquirePosition, );
FORWARD_TO_ENTITY(requestAssignRoute, );
FORWARD_TO_ENTITY(requestClearRoute, );
FORWARD_TO_ENTITY(requestFollowTrajectory, );
FORWARD_TO_ENTITY(requestLaneChange, );
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, );
FORWARD_TO_ENTITY(requestSpeedChange, );

This has largely been achieved by exposing direct access to Entity and its member functions through the API::getEntity function:

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

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:

void onUpdate() override
{
if (api_.isInLanelet("ego", 34408, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
void onInitialize() override
{
api_.spawn(
"ego",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
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);
}

After:
void onUpdate() override
{
if (api_.getEntity("ego")->isInLanelet(34408, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
void onInitialize() override
{
api_.spawn(
"ego",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto entity = api_.getEntity("ego");
entity->setStatus(
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 0.0, api_.getHdmapUtils()),
traffic_simulator::helper::constructActionStatus(10));
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()));
entity->requestAcquirePosition(goal_pose);
}

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

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

has been replaced with a set of other functions below:
auto engage() -> void;
auto isEngaged() const -> bool;
auto isEngageable() const -> bool;
auto replanRoute(const std::vector<geometry_msgs::msg::PoseStamped> & route) -> void;
auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void;
auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void;
auto getMinimumRiskManeuverBehaviorName() const -> std::string;
auto getMinimumRiskManeuverStateName() const -> std::string;
auto getEmergencyStateName() const -> std::string;
auto getTurnIndicatorsCommandName() const -> const std::string;

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;

  • Added many methods as asFieldOperatorApplication replacements:

auto setParameter(const std::string & name, const ParameterType & default_value = {}) const -> ParameterType    

auto engage() -> void;
auto isEngaged() const -> bool;
auto isEngageable() const -> bool;

auto getMinimumRiskManeuverBehaviorName() const -> std::string;
auto getMinimumRiskManeuverStateName() const -> std::string;
auto getEmergencyStateName() const -> std::string;
auto getTurnIndicatorsCommandName() const -> std::string;

auto requestReplanRoute(const std::vector<geometry_msgs::msg::PoseStamped> & route) -> void;
auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void;

auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void;

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

--

TauTheLepton and others added 30 commits June 19, 2024 13:16
… exceptions were thrown

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
… API

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
…p-time' into RJD-1057-remove-functions-forwarded-to-entity-base
…d' into RJD-1057-remove-functions-forwarded-to-entity-base
…tityBase, also some getters, left setVelocityLimit and setBehaviorParameter
…develop templated is() in EntityBase and use it, rename isEgoSpawned to isAnyEgoSpawned, refactor
…ng setBehaviorParameter and setVelocityLimit
…ng request*, move requestLaneChange to EntityBase
…n forwarding, set "waiting" as init action state in behavior_tree
…develop getEgoEntity and dedicated methods in EgoEntity
…o RJD-1057-remove-functions-forwarded-to-entity-base
…y in calc it directly in evaluateTimeHeadway
Copy link

github-actions bot commented Dec 4, 2024

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

  • Is this pull request written in a way that is easy to read from a third-party perspective?
  • Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
  • If this pull request contains a destructive change, does this pull request contain the migration guide?
  • Labels of this pull request are valid?
  • All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
  • The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

@dmoszynski dmoszynski self-assigned this Dec 4, 2024
@dmoszynski dmoszynski marked this pull request as ready for review December 4, 2024 16:19
@dmoszynski
Copy link
Contributor Author

No regressions confirmed here

@hakuturu583 hakuturu583 self-requested a review December 12, 2024 01:22
Comment on lines +98 to +108
auto EntityBase::isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const
-> bool
{
return math::geometry::getDistance(getMapPose(), pose) < tolerance;
}

auto EntityBase::isInPosition(
const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool
{
return isInPosition(static_cast<geometry_msgs::msg::Pose>(lanelet_pose), tolerance);
}
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Consider modifying the function name.
This function checks whether a circle exists inside a certain range from the first argument.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested candidates are isInRange, isInCircle, isNearbyPosition, reachPosition, isReachPosition, etc.

Copy link
Contributor Author

@dmoszynski dmoszynski Dec 12, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

whether a circle exists inside a certain range from the first argument.

I believe EntityBase::isInPosition(...) method checks if the Entity is in a specific position - with some tolerance. More specifically, it checks if the euclidean distance (3d) between the Entity's map position and the passed map position is smaller than the passed tolerance.
So it's more like checking if a point (passed map position) is inside a sphere of a specified radius (tolerance), which center is located at the reference point (Entity's map position).

While choosing the name of this method, I based it on the name of method IsInLanelet(...) see below - so as to keep it consistent.

/* */ 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 { return status_->isInLanelet(); };
/* */ auto isInLanelet(
const lanelet::Id lanelet_id, std::optional<double> tolerance = std::nullopt) const -> bool;

About suggested candidates

I believe that the names isInRange and isInCircle do not suit here because they do not refer to what is really being checked. Because, it is simply a comparison of two positions.

In addition, I believe that the word 'reach' in the names reachPosition and isReachPosition does not add anything good, but only adds additional context like an Entity is executing some kind of move - and it doesn't have to be that because it can be a static object.

The only reasonable candidate here is isNearbyPosition however, I cannot find a rationale why the name 'nearby' should be added here. Could you please explain why you think inNearbyPosition is better than isInPosition? Because in my feelings, since we have isInLanelet, isInPosition seems like a good choice.

About tolerance

I would also add that I believe that in isInPosition method tolerance should be optional (as I wrote in the description of PR). It should be handled similarly to how it is in isInLanelet(...).

To be more precise, if we assume that tolerance is std::optional, then if nothing is passed (tolerance is std::nullopt): tolerance should be taken as equal to std::numeric_limits<double>::epsilon().
Thus, the isInPosition(some_position) call will return true if the Entity's map position is the same as passed map position with std::numeric_limits<double>::epsilon() precision.

Please let me know what you think about it 🙇

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From the text is in position I get the impression that it is a function that determines if a specific xyz coordinate is an exact match to an input coordinate.

If it is nearby position, I intuitively understand that there is some tolerance since it means “nearby”.

Copy link
Contributor Author

@dmoszynski dmoszynski Dec 16, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you mean here a situation such that:

  • the method name (isInNearbyPosition) is supposed to itself provide information to the user that the user can use this method to compare positions with any tolerance

Since, from my point of view, the requirement to pass the tolerance argument provides the same information, I thought that adding this information to the name itself could be redundant.

If you mean this situation, I understand, however, I would like to maintain consistency between the method names IsInLanelet and IsInPosition (since they do the same thing only in a different representation). Because of this, adding 'nearby' to IsInPosition also causes the need to add 'nearby' to IsInLanelet.

In this circumstance, I would like to make two proposals:

1. Nearby

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

auto isInNearbyPosition(const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool;

auto isInLanelet() const -> bool;

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

2. Optional tolerance

auto isInPosition(const geometry_msgs::msg::Pose & pose, const std::optional<double> tolerance = std::nullopt) const -> bool;

auto isInPosition(const CanonicalizedLaneletPose & lanelet_pose, const std::optional<double> tolerance = std::nullopt) const -> bool;

auto isInLanelet() const -> bool;

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

The 2. proposal involves the development of what I wrote here:

About tolerance
I would also add that I believe that in isInPosition method tolerance should be optional (as I wrote in the description of PR). It should be handled similarly to how it is in isInLanelet(...).

To be more precise, if we assume that tolerance is std::optional, then if nothing is passed (tolerance is std::nullopt): tolerance should be taken as equal to std::numeric_limits::epsilon().
Thus, the isInPosition(some_position) call will return true if the Entity's map position is the same as passed map position with std::numeric_limits::epsilon() precision.

Please let me know which proposal is better in your opinion - then I will implement it. 🙇

bool despawnEntity(const std::string & name);

bool entityExists(const std::string & name);
auto isEntitySpawned(const std::string & name) const -> bool;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This renaming makes it difficult to express "has been spawned but now not exists", so please change it back.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've changed the name to isEntityExist, please confirm if this is okay for you 🙇

Comment on lines +100 to +101
bool API::checkCollision(
const std::string & first_entity_name, const std::string & second_entity_name)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If possible, it is better to change this function to EntityBase::collidesWith(pointer-to-other-EntityBase) from the viewpoint of uniformity. Please consider whether it is reasonably possible to change it.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you so much for your valuable insight. 🙇

I've been thinking about this, and looking at the EntityBase, EntityManager and API versions after the final PR 5/5 is merged, it doesn't seem like a good idea to me.

The main problem in developing the EntityBase and API interfaces is the answer to the question “which features should be in the API and which directly in EntityBase”.
We could indeed move all the checkCollision-like features from API to EntityBase::someFeature(pointer-to-other-EntityBase), but I don't think it's a good idea to implement it this way.

Certainly, I don't think that the 'features split ' that occurs in PR 5/5 is ideal, however, looking at EntityBase header (entity_base.hpp) i don't think that adding method collidesWith(pointer-to-other-EntityBase) here would be reasonable. The main reason is the need to pass reference to other Entity.
In my mind, EntityBase should have methods associated with a single entity and its attributes. Creating methods like EntityBase::someFeature(pointer-to-other-EntityBase) will involve passing a large amount of information to EntityBase. If we want to add the collidesWith method, it is good to pass only those elements that are indeed related to collides check.

Therefore, other suggestion as I can propose here is to, for example, create a method

  • EntityBase::collidesWith(const geometry_msgs::msg::Pose & object_pose, const std::vector<geometry_msgs::msg::Point> object_polygon)

or

  • EntityBase::collidesWith(const geometry_msgs::msg::Pose & object_pose, const boost_polygon & object_polygon) (see bounding_box.hpp)

However I'm not sure if that makes sense for checkCollision feature.
It seems to me that the option to call API::checkCollision(const std::string & first_entity_name, const std::string & second_entity_name) is from the point of view of the whole architecture obtained in PR 5/5 proper, and at this point I do not see the need to create an additional method EntityBase::collidesWith(...) - please see api.hpp and entity_base.hpp.

I would add that I tried to use common sense in the 'features split' between the API and EntityBase, and it is often subjective. However, if two Entities are explicitly needed for an operation (calculating the distance between them, checking collisions, calculation of time headway), then I would try to keep such features in the API.

Please let me know what you think about it 🙇

Comment on lines +175 to +176
auto getEntityOrNullptr(const std::string & name) const
-> std::shared_ptr<traffic_simulator::entity::EntityBase>;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The name "OrNullptr" is redundant because all functions that return pointers should assume the possibility of a null pointer when used. Change interfaces that return references to getEntity and interfaces that return pointers to getEntityPointer.

Copy link
Contributor Author

@dmoszynski dmoszynski Dec 12, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I understand your point of view.
However, I would like to add that there is still the getEntity method:

  • getEntity throws an exception now - when the Entity does not exist - is commonly used in traffic_simulator.
  • getEntityOrNullptr does not throw exceptions but returns nullptr - is mainly used by simulator_core, due to the need to handle cases when the Entity does not exist.

If I will change the name of getEntityOrNullptr to getEntityPointer, then there will be two methods: getEntityPointer and getEntity, both returning a pointer - which I think is more confusing than getEntityOrNullptr+ getEntity.

Please see the source

auto EntityManager::getEntityOrNullptr(const std::string & name) const
-> std::shared_ptr<traffic_simulator::entity::EntityBase>
{
if (auto it = entities_.find(name); it != entities_.end()) {
return it->second;
} else {
/*
This method returns nullptr, due to the fact that the interpretation of the scenario operates in
such a way that checking a condition, e.g. DistanceCondition, is called also for Entities that
have not yet been spawned. For example, if for DistanceCondition any getEntity() returns
nullptr, the condition returns a distance equal to NaN. For this reason, using getEntity() with
throwing an exception is not recommended.
*/
return nullptr;
}
};
auto EntityManager::getEntity(const std::string & name) const
-> std::shared_ptr<traffic_simulator::entity::EntityBase>
{
if (const auto entity = getEntityOrNullptr(name)) {
return entity;
} else {
THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " does not exist.");
}
}

I would also like to add that in PR 5/5 there are changes developed, in which the getEntityOrNullptr method will be completely removed. In parts of the code where it is now necessary, isEntitySpawned (existEntity) will be used - below I've attached an example.
This change is included in PR 5/5 because in PR 5/5 I focused more on the simulator_core<->api connection itself.

Future example - here getEntityOrNullptr no longer exist

class ConditionEvaluation
{
protected:
static auto evaluateSimulationTime() -> double
{
if (SimulatorCore::active()) {
return core->getCurrentTime();
} else {
return std::numeric_limits<double>::quiet_NaN();
}
}
static auto evaluateStandStill(const std::string & entity_name) -> double
{
if (core->isEntitySpawned(entity_name)) {
return core->getEntity(entity_name)->getStandStillDuration();
} else {
return std::numeric_limits<double>::quiet_NaN();
}
}
static auto evaluateSpeed(const std::string & entity_name) -> double
{
if (core->isEntitySpawned(entity_name)) {
return core->getEntity(entity_name)->getCurrentTwist().linear.x;
} else {
return std::numeric_limits<double>::quiet_NaN();
}
}
static auto evaluateAcceleration(const std::string & entity_name) -> double
{
if (core->isEntitySpawned(entity_name)) {
return core->getEntity(entity_name)->getCurrentAccel().linear.x;
} else {
return std::numeric_limits<double>::quiet_NaN();
}
}
static auto evaluateCollisionCondition(
const std::string & first_entity_name, const std::string & second_entity_name) -> bool
{
if (core->isEntitySpawned(first_entity_name) && core->isEntitySpawned(second_entity_name)) {
return core->checkCollision(first_entity_name, second_entity_name);
} else {
return false;
}
}
static auto evaluateTimeHeadway(
const std::string & from_entity_name, const std::string & to_entity_name) -> double
{
if (core->isEntitySpawned(from_entity_name) && core->isEntitySpawned(to_entity_name)) {
if (const auto time_headway = core->timeHeadway(from_entity_name, to_entity_name)) {
return time_headway.value();
}
}
return std::numeric_limits<double>::quiet_NaN();
}
// User defined conditions
static auto evaluateCurrentState(const std::string & entity_name) -> const std::string
{
if (core->isEntitySpawned(entity_name)) {
return core->getEntity(entity_name)->getCurrentAction();
} else {
return "not spawned";
}
}
static auto evaluateRelativeHeading(
const std::string & entity_name, const traffic_simulator::LaneletPose & lanelet_pose)
-> double
{
if (core->isEntitySpawned(entity_name)) {
if (const auto relative_yaw = core->laneletRelativeYaw(entity_name, lanelet_pose)) {
return std::abs(relative_yaw.value());
}
}
return std::numeric_limits<double>::quiet_NaN();
}
static auto evaluateRelativeHeading(const std::string & entity_name) -> double
{
if (core->isEntitySpawned(entity_name)) {
if (const auto relative_yaw = core->getEntity(entity_name)->getLaneletRelativeYaw()) {
return std::abs(relative_yaw.value());
}
}
return std::numeric_limits<double>::quiet_NaN();
}
};

Please let me know what you think about it 🙇

@@ -65,7 +65,7 @@ class CanonicalizedEntityStatus
auto getLinearJerk() const noexcept -> double;
auto setLinearJerk(double) -> void;

auto laneMatchingSucceed() const noexcept -> bool;
auto isInLanelet() const noexcept -> bool;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why was laneMatchingSucceed renamed to isInLanelet?
Since successful lane matching and being inside a lanelet polygon have different meanings, I wonder if the renaming was appropriate.

Copy link
Contributor Author

@dmoszynski dmoszynski Dec 12, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've added the isInLanelet() method to EntityBase with the lanelet_id argument because it is often used.
As result, in my feeling, it was a good thing to also change the name from laneMatchingSucceed() to isInLanelet()

Please let me explain 🙇:

I based EntityBase::isInLanelet(...) on the pose::isInLanelet(...) function (from utils).
Method pose::isInLanelet(...) lets us check if canonicalized_lanelet_pose is on the lanelet with the passed lanelet_id:

auto isInLanelet(
const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id,
const double tolerance, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> bool;

On the other hand, EntityBase::isInLanelet(...) lets us check if the Entity (its current position) is on the lanelet with the passed lanelet_id (here is the possibility of not passing the tolerance value, since we can use the default value of matching):

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

auto EntityBase::isInLanelet(const lanelet::Id lanelet_id, std::optional<double> tolerance) const
-> bool
{
if (const auto lanelet_pose = getCanonicalizedLaneletPose()) {
const auto tolerance_value =
tolerance ? tolerance.value() : getDefaultMatchingDistanceForLaneletPoseCalculation();
return pose::isInLanelet(lanelet_pose.value(), lanelet_id, tolerance_value, hdmap_utils_ptr_);
}
return false;
}

An additional overload of EntityBase::isInLanelet method can be checking if the Entity is on any lanelet (with any lanelet_id) - then we do not pass this argument as well. In this way we get a method that gives the same result as laneMatchingSucceed(). Because if the Entity is on any lanelet, then its EntityStatus attribute canonicalized_lanelet_pose_ always will have a value.
So EntityBase::isInLanelet() without the arguments passed is equivalent to EntityBase::laneMatchingSucceed()

/* */ auto isInLanelet() const -> bool { return status_->isInLanelet(); };

auto CanonicalizedEntityStatus::isInLanelet() const noexcept -> bool
{
return canonicalized_lanelet_pose_.has_value();
}

Please let me know what you think about it 🙇

@dmoszynski dmoszynski added bump major If this pull request merged, bump major version of the scenario_simulator_v2 refactor labels Dec 12, 2024
@dmoszynski
Copy link
Contributor Author

dmoszynski commented Dec 16, 2024

@hakuturu583
As for this sonar issue, analyzing the source code everything seems to be as it should be. Sonar probably detects this issue due to the fact that some of the EntityBase::requestLaneChange overloads are virtual and some are not. However, those that are virtual are properly overridden in EgoEntity.

I would like to also add that this is not the first PR in which I see this issue - I have no idea how to get rid of it.

virtual auto requestLaneChange(const lanelet::Id) -> void
{
/**
* @note There are Entities such as MiscObjectEntity for which lane change is not possible,
* and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type.
*/
}
virtual auto requestLaneChange(const lane_change::Parameter &) -> void
{
/**
* @note There are Entities such as MiscObjectEntity for which lane change is not possible,
* and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type.
*/
}
/* */ auto requestLaneChange(const lane_change::Direction & direction) -> void;
/* */ auto requestLaneChange(
const lane_change::AbsoluteTarget & target, const lane_change::TrajectoryShape trajectory_shape,
const lane_change::Constraint & constraint) -> void;
/* */ auto requestLaneChange(
const lane_change::RelativeTarget & target, const lane_change::TrajectoryShape trajectory_shape,
const lane_change::Constraint & constraint) -> void;

auto requestLaneChange(const lanelet::Id) -> void override;
auto requestLaneChange(const lane_change::Parameter &) -> void override;

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bump major If this pull request merged, bump major version of the scenario_simulator_v2 refactor
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants