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

Merged
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
78 commits
Select commit Hold shift + click to select a range
7ff6248
Add getEntityOrThrow function to mimic forwarding functionality where…
TauTheLepton Jun 19, 2024
aacd0b3
Remove forwarding of getEntityType in EntityManager
TauTheLepton Jun 19, 2024
b972a16
Forward getEntityOrThrow from API to EntityManager
TauTheLepton Jun 19, 2024
38d9552
Remove forwarding of getEntityStatus in EntityManager and API
TauTheLepton Jun 19, 2024
26ce1d1
Add const
TauTheLepton Jun 19, 2024
cb0fbb2
Remove forwarding of getEntityStatusBeforeUpdate in EntityManager and…
TauTheLepton Jun 19, 2024
9c60bb3
Remove forwarding of getBehaviorParameter in EntityManager and API
TauTheLepton Jun 19, 2024
1a0c2be
Remove forwarding of getCurrentTwist in EntityManager and API
TauTheLepton Jun 19, 2024
421a18e
Clean unused template
TauTheLepton Jun 19, 2024
d3e83a3
Remove forwarding of getCurrentAccel in EntityManager and API
TauTheLepton Jun 19, 2024
89ef194
Remove forwarding of getLinearJerk in EntityManager and API
TauTheLepton Jun 19, 2024
24e7321
Remove forwarding of getBoundingBox in EntityManager and API
TauTheLepton Jun 19, 2024
be319aa
Remove forwarding of get2DPolygon in EntityManager
TauTheLepton Jun 19, 2024
9d942f9
Remove forwarding of getStandStillDuration in EntityManager and API
TauTheLepton Jun 19, 2024
9953d8c
feat(cpp_mock, traffic_simulator): change getEntity, use getEntityOrN…
dmoszynski Jun 26, 2024
3921d57
Merge remote-tracking branch 'origin/RJD-1056-remove-current-time-ste…
dmoszynski Jun 26, 2024
2c78d59
Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-starte…
dmoszynski Jun 26, 2024
e7091e5
feat(entity_base, traffic_simulator, cpp_mock): move isInLanelet to E…
dmoszynski Jun 26, 2024
ecb46de
feat(entity_base, traffic_simulator): move reachPosition as isInPosit…
dmoszynski Jun 26, 2024
0ed40a3
feat(entity_base, traffic_simulator): rename laneMatchingSucceed to i…
dmoszynski Jun 26, 2024
249ec7d
feat(entity_base, traffic_simulator): remove forwarding setters to En…
dmoszynski Jun 27, 2024
9233cf8
feat(entity_base, traffic_simulator): move isStopping to EntityBase, …
dmoszynski Jun 27, 2024
e3bc5f9
feat(entity_base, traffic_simulator, simulator_core): remove forwardi…
dmoszynski Jun 27, 2024
6dbf3ac
feat(entity_base, traffic_simulator, simulator_core): remove forwardi…
dmoszynski Jun 27, 2024
f952a11
feat(entity_manager, behavior_tree, cpp_mock): remove getCurrentActio…
dmoszynski Jun 27, 2024
f142c10
feat(entity_manager, simulator_core): remove activateOutOfRangeJob fo…
dmoszynski Jun 27, 2024
f057e68
feat(api, entity_manager, cpp_mock): rename entityExist to isEntitySp…
dmoszynski Jun 27, 2024
d287360
feat(ego_entity, sumulator_core): remove asFieldOperatorApplication, …
dmoszynski Jun 28, 2024
daa67d1
Merge branch 'RJD-1057-remove-traffic-lights-from-entity-manager' int…
dmoszynski Jun 28, 2024
0fcc813
feat(api, simulator_core): emove getTimeHeadway from API, use directl…
dmoszynski Jun 28, 2024
52b44ca
feat(simulator_core, api, entity_base, cpp_mock): move setEntityStatu…
dmoszynski Jun 28, 2024
8b478db
Fix mock scenario with invalid entity
TauTheLepton Jun 28, 2024
56ca207
Restore previous scenario condition
TauTheLepton Jun 28, 2024
8cf8c3a
Fix concealer local address returning
TauTheLepton Jul 1, 2024
07aae15
feat(entity_base, entity_status): improve ::setStatus
dmoszynski Jul 1, 2024
9ca9eed
Fix random_test_runner
TauTheLepton Jul 30, 2024
ce9b59a
Merge remote-tracking branch 'tier4/RJD-1057-remove-traffic-lights-fr…
TauTheLepton Jul 31, 2024
1869aa1
Merge remote-tracking branch 'tier4/RJD-1057-traffic-lights-tests' in…
TauTheLepton Jul 31, 2024
8940b3e
Adjust mock scenarios to new API
TauTheLepton Jul 31, 2024
f438056
Merge branch 'RJD-1057-traffic-lights-tests' into RJD-1057-remove-fun…
TauTheLepton Oct 3, 2024
4681b25
Merge remote-tracking branch 'origin/RJD-1057-traffic-lights-tests' i…
dmoszynski Oct 14, 2024
fec8821
fix(cpp_mock_scenarios): merge changes in random001
dmoszynski Oct 14, 2024
f559200
ref(cpp_mock_scenarios): ref random001
dmoszynski Oct 14, 2024
18daa8b
ref(behavior_tree,traffic_simulator): improve code by basing on sonar…
dmoszynski Oct 15, 2024
4eef0ec
ref(cpp_mock_scenarios): improve overall
dmoszynski Oct 15, 2024
1d771b2
ref(cpp_mock_scenarios): improve move_backward
dmoszynski Oct 15, 2024
ace3bc8
ref(traffic_simulator, simulator_core): improve overall
dmoszynski Oct 15, 2024
a2d095f
ref(traffic_simulator): improve requestLaneChange, getEgoName
dmoszynski Oct 16, 2024
8d210d7
Merge remote-tracking branch 'origin/RJD-1057-traffic-lights-tests' i…
dmoszynski Nov 8, 2024
74b99df
ref(traffic_simulator): add const to LongitudinalSpeedPlanner::isTarg…
dmoszynski Nov 8, 2024
9ff87c5
Merge branch 'RJD-1057-traffic-lights-tests' into RJD-1057-remove-fun…
dmoszynski Nov 13, 2024
bf61c9a
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 4, 2024
59e12f2
ref(traffic_simulator): improve requestClearRoute call for ego_entity
dmoszynski Dec 4, 2024
f794dff
Merge branch 'RJD-1057-remove-functions-forwarded-to-entity-base-midd…
dmoszynski Dec 4, 2024
926c712
ref(traffic_simulator): tiny formatting change
dmoszynski Dec 4, 2024
ca2223c
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 5, 2024
a0a0cb5
Merge remote-tracking branch 'origin/master' into RJD-1057-remove-fun…
dmoszynski Dec 9, 2024
f1166ed
Merge remote-tracking branch 'origin/master' into RJD-1057-remove-fun…
dmoszynski Dec 11, 2024
4e0c479
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 11, 2024
153e7b1
ref(traffic_simulator): rename isEntitySpawned to isEntityExist
dmoszynski Dec 12, 2024
7af86bb
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 12, 2024
27efbd5
ref(behavior_tree_plugin): rename default value of current_action_: "…
dmoszynski Dec 16, 2024
40ff422
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 16, 2024
73c5223
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 16, 2024
f2b2dfc
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Dec 17, 2024
1c8fe92
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Jan 3, 2025
3cf2412
Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-ent…
dmoszynski Jan 7, 2025
9b252cb
ref(traffic_simulator, cpp_mock_scenarios): rename isInPosition to is…
dmoszynski Jan 8, 2025
5fa7281
ref(traffic_simulator, simulator_interface): revert format unexpected…
dmoszynski Jan 8, 2025
80dd5a2
Merge remote-tracking branch 'origin/master' into RJD-1057-remove-fun…
robomic Jan 13, 2025
8d90c0e
move functions from EgoEntity to FieldOperatorApplication
robomic Jan 14, 2025
82d6085
remove function shadowing
robomic Jan 15, 2025
70bbb58
Revert "remove function shadowing"
robomic Jan 20, 2025
ec038eb
Revert "move functions from EgoEntity to FieldOperatorApplication"
robomic Jan 20, 2025
6541162
Merge tag '7.4.7' into RJD-1057-remove-functions-forwarded-to-entity-…
robomic Jan 20, 2025
ab4296d
remove recursive calls after merge
robomic Jan 20, 2025
26051f3
merge 8.0.2
robomic Jan 28, 2025
ed0dcc2
remove double canonicalization
robomic Jan 28, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
ref(cpp_mock_scenarios): improve overall
  • Loading branch information
dmoszynski committed Oct 15, 2024
commit 4eef0ecd09d2a29fde82dc1f78f8dad78455560c
5 changes: 3 additions & 2 deletions mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,13 +54,14 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
if (t >= 6.6) {
if (7.5 >= t) {
if (std::fabs(0.1) <= api_.getEntity("ego")->getCurrentTwist().linear.x) {
if (std::fabs(0.1) <= ego_linear_velocity) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
} else {
if (0.1 >= api_.getEntity("ego")->getCurrentTwist().linear.x) {
if (0.1 >= ego_linear_velocity) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,13 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
private:
void onUpdate() override
{
double ego_accel = api_.getEntity("ego")->getCurrentAccel().linear.x;
double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x;
// double npc_accel = static_cast<EntityStatus>(api_.getEntity("npc")->getStatus()).action_status.accel.linear.x;
double npc_twist = api_.getEntity("npc")->getCurrentTwist().linear.x;
;
double ego_linear_acceleration = api_.getEntity("ego")->getCurrentAccel().linear.x;
double ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
// double npc_linear_acceleration = api_.getEntity("npc")->getCurrentAccel().linear.x;
double npc_linear_velocity = api_.getEntity("npc")->getCurrentTwist().linear.x;

// LCOV_EXCL_START
if (npc_twist > (ego_twist + 1) && ego_accel < 0) {
if (npc_linear_velocity > (ego_linear_velocity + 1) && ego_linear_acceleration < 0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (api_.checkCollision("ego", "npc")) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,13 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
private:
void onUpdate() override
{
double ego_accel = api_.getEntity("ego")->getCurrentAccel().linear.x;
double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x;
// double npc_accel = static_cast<EntityStatus>(api_.getEntity("npc")->getStatus()).action_status.accel.linear.x;
double npc_twist = api_.getEntity("npc")->getCurrentTwist().linear.x;
;
double ego_linear_acceleration = api_.getEntity("ego")->getCurrentAccel().linear.x;
double ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
// double npc_linear_acceleration = api_.getEntity("npc")->getCurrentAccel().linear.x;
double npc_linear_velocity = api_.getEntity("npc")->getCurrentTwist().linear.x;

// LCOV_EXCL_START
if (ego_twist > (npc_twist + 1) && ego_accel > 0) {
if (ego_linear_velocity > (npc_linear_velocity + 1) && ego_linear_acceleration > 0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (api_.checkCollision("ego", "npc")) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,16 +51,16 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto entity = api_.getEntity("ego");
entity->setStatus(
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));
entity->requestSpeedChange(10, true);
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()));
entity->requestAcquirePosition(goal_pose);
ego_entity->requestAcquirePosition(goal_pose);
}
};
} // namespace cpp_mock_scenarios
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,17 +51,17 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto entity = api_.getEntity("ego");
entity->setLinearVelocity(10);
entity->requestSpeedChange(10, true);
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(10);
ego_entity->requestSpeedChange(10, true);
std::vector<geometry_msgs::msg::Pose> goal_poses;
goal_poses.emplace_back(traffic_simulator::pose::toMapPose(
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34408, 1.0, 0.0, api_.getHdmapUtils())));
goal_poses.emplace_back(traffic_simulator::pose::toMapPose(
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34408, 10, 0.0, api_.getHdmapUtils())));
entity->requestAssignRoute(goal_poses);
ego_entity->requestAssignRoute(goal_poses);
}
};
} // namespace cpp_mock_scenarios
Expand Down
8 changes: 4 additions & 4 deletions mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,12 +58,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto entity = api_.getEntity("ego");
entity->setLinearVelocity(7);
entity->requestSpeedChange(7, true);
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(7);
ego_entity->requestSpeedChange(7, true);
const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose(
traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils());
entity->requestAcquirePosition(goal_pose);
ego_entity->requestAcquirePosition(goal_pose);
}
};
} // namespace cpp_mock_scenarios
Expand Down
15 changes: 6 additions & 9 deletions mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,11 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
bool requested = false;
void onUpdate() override
{
const auto entity = api_.getEntity("ego");
if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) {
if (const auto ego_entity = api_.getEntity("ego"); !ego_entity->isInLanelet()) {
stop(cpp_mock_scenarios::Result::FAILURE);
} else if (traffic_simulator::pose::isInLanelet(
lanelet_pose.value(), 34507, 0.1, api_.getHdmapUtils())) {
} else if (ego_entity->isInLanelet(34507, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
} else if (
std::abs(static_cast<traffic_simulator::LaneletPose>(lanelet_pose.value()).offset) <= 2.8) {
} else if (std::abs(ego_entity->getCanonicalizedStatus().getLaneletPose().offset) <= 2.8) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
Expand All @@ -58,9 +55,9 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 3.0, api_.getHdmapUtils()),
getVehicleParameters());
auto entity = api_.getEntity("ego");
entity->setLinearVelocity(10);
entity->requestSpeedChange(10, true);
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(10);
ego_entity->requestSpeedChange(10, true);
}
};
} // namespace cpp_mock_scenarios
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,32 +63,32 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario

void onUpdate() override
{
const auto entity = api_.getEntity("ego");
const auto ego_entity = api_.getEntity("ego");
// LCOV_EXCL_START
if (api_.getCurrentTime() >= 10.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
// LCOV_EXCL_STOP
if (equals(api_.getCurrentTime(), 0.0, 0.01) && !entity->isInPosition(spawn_pose, 0.1)) {
if (equals(api_.getCurrentTime(), 0.0, 0.01) && !ego_entity->isInPosition(spawn_pose, 0.1)) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (
equals(api_.getCurrentTime(), 1.0, 0.01) &&
!entity->isInPosition(trajectory_start_pose, 0.1)) {
!ego_entity->isInPosition(trajectory_start_pose, 0.1)) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (
equals(api_.getCurrentTime(), 1.5, 0.01) &&
!entity->isInPosition(trajectory_waypoint_pose, 0.1)) {
!ego_entity->isInPosition(trajectory_waypoint_pose, 0.1)) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (
equals(api_.getCurrentTime(), 2.0, 0.01) &&
!entity->isInPosition(trajectory_goal_pose, 0.1)) {
!ego_entity->isInPosition(trajectory_goal_pose, 0.1)) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (equals(api_.getCurrentTime(), 2.5, 0.01)) {
if (entity->isInPosition(trajectory_goal_pose, 0.1)) {
if (ego_entity->isInPosition(trajectory_goal_pose, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
} else {
stop(cpp_mock_scenarios::Result::FAILURE);
Expand Down
6 changes: 3 additions & 3 deletions mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,11 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode
bool lanechange_finished = false;
void onUpdate() override
{
const auto entity = api_.getEntity("ego");
if (entity->isInLanelet(34513, 0.1)) {
const auto ego_entity = api_.getEntity("ego");
if (ego_entity->isInLanelet(34513, 0.1)) {
lanechange_finished = true;
}
if (lanechange_finished && entity->isInLanelet(34510, 0.1)) {
if (lanechange_finished && ego_entity->isInLanelet(34510, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
// LCOV_EXCL_START
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -58,9 +58,9 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar
const std::string & from_entity_name, const std::string & to_entity_name,
const double matching_distance) -> std::optional<double>
{
auto from_entity_lanelet_pose =
const auto from_entity_lanelet_pose =
api_.getEntity(from_entity_name)->getCanonicalizedLaneletPose(matching_distance);
auto to_entity_lanelet_pose =
const auto to_entity_lanelet_pose =
api_.getEntity(to_entity_name)->getCanonicalizedLaneletPose(matching_distance);
if (from_entity_lanelet_pose && to_entity_lanelet_pose) {
return traffic_simulator::distance::lateralDistance(
Expand Down
9 changes: 3 additions & 6 deletions mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,11 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode
void onUpdate() override
{
// LCOV_EXCL_START
const auto entity = api_.getEntity("ego");
if (!entity) {
stop(cpp_mock_scenarios::Result::FAILURE);
} else if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) {
if (const auto ego_entity = api_.getEntity("ego"); !ego_entity->isInLanelet()) {
stop(cpp_mock_scenarios::Result::FAILURE);
} else if (const auto difference = std::abs(
static_cast<traffic_simulator::LaneletPose>(lanelet_pose.value()).s -
entity->getTraveledDistance());
ego_entity->getCanonicalizedStatus().getLaneletPose().s -
ego_entity->getTraveledDistance());
difference > std::numeric_limits<double>::epsilon()) {
stop(cpp_mock_scenarios::Result::FAILURE);
} // LCOV_EXCL_STOP
Expand Down
2 changes: 1 addition & 1 deletion mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode
const auto map_pose = traffic_simulator::pose::toMapPose(
traffic_simulator::helper::constructCanonicalizedLaneletPose(
120545, 0.0, 0.0, api_.getHdmapUtils()));
if (api_.getEntity("ego")->reachPosition(map_pose, 0.1)) {
if (api_.getEntity("ego")->isInPosition(map_pose, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
} else {
stop(cpp_mock_scenarios::Result::FAILURE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,14 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode
private:
void onUpdate() override
{
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
if (api_.getEntity("front")->getCurrentTwist().linear.x < 10.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (api_.getCurrentTime() <= 0.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) {
if (api_.getCurrentTime() <= 0.9 && ego_linear_velocity > 10.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (
api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0 &&
api_.getEntity("ego")->getCurrentTwist().linear.x >= 9.9) {
if (api_.getCurrentTime() >= 1.0 && ego_linear_velocity <= 10.0 && ego_linear_velocity >= 9.9) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,19 +43,16 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp
/**
* @brief checking linear speed
*/
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
if (api_.getCurrentTime() <= 0.95) {
if (!equals(
api_.getCurrentTime() * 10.0, api_.getEntity("ego")->getCurrentTwist().linear.x,
0.01)) {
if (!equals(api_.getCurrentTime() * 10.0, ego_linear_velocity, 0.01)) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
if (api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0) {
if (api_.getCurrentTime() >= 1.0 && ego_linear_velocity <= 10.0) {
speed_reached = true;
}
if (
speed_reached && api_.getCurrentTime() >= 1.5 &&
api_.getEntity("ego")->getCurrentTwist().linear.x >= 13.88) {
if (speed_reached && api_.getCurrentTime() >= 1.5 && ego_linear_velocity >= 13.88) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,16 +39,15 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari
private:
void onUpdate() override
{
const auto entity_linear_velocity = api_.getEntity("front")->getCurrentTwist().linear.x;
if (api_.getCurrentTime() <= 1.9) {
if (!equals(
api_.getCurrentTime() + 3.0, api_.getEntity("front")->getCurrentTwist().linear.x,
0.01)) {
if (!equals(api_.getCurrentTime() + 3.0, entity_linear_velocity, 0.01)) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
if (
api_.getCurrentTime() >= 2.0 && api_.getEntity("front")->getCurrentTwist().linear.x <= 5.05 &&
api_.getEntity("front")->getCurrentTwist().linear.x >= 4.95) {
api_.getCurrentTime() >= 2.0 && entity_linear_velocity <= 5.05 &&
entity_linear_velocity >= 4.95) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,11 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar
private:
void onUpdate() override
{
if (api_.getCurrentTime() <= 0.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) {
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
if (api_.getCurrentTime() <= 0.9 && ego_linear_velocity > 10.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (
api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 5.1 &&
api_.getEntity("ego")->getCurrentTwist().linear.x >= 4.9) {
if (api_.getCurrentTime() >= 1.0 && ego_linear_velocity <= 5.1 && ego_linear_velocity >= 4.9) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand All @@ -56,10 +55,10 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto entity = api_.getEntity("ego");
entity->setLinearVelocity(0);
entity->setVelocityLimit(5.0);
entity->requestSpeedChange(
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(0);
ego_entity->setVelocityLimit(5.0);
ego_entity->requestSpeedChange(
10.0, traffic_simulator::speed_change::Transition::LINEAR,
traffic_simulator::speed_change::Constraint(
traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,12 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios::
private:
void onUpdate() override
{
if (api_.getCurrentTime() <= 3.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) {
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
if (api_.getCurrentTime() <= 3.9 && ego_linear_velocity > 10.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (api_.getCurrentTime() >= 3.999) {
if (
api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0 &&
api_.getEntity("ego")->getCurrentTwist().linear.x >= 9.9) {
if (ego_linear_velocity <= 10.0 && ego_linear_velocity >= 9.9) {
stop(cpp_mock_scenarios::Result::SUCCESS);
} else {
stop(cpp_mock_scenarios::Result::FAILURE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,12 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario
private:
void onUpdate() override
{
if (api_.getCurrentTime() <= 3.9 && api_.getEntity("ego")->getCurrentTwist().linear.x >= 3.0) {
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
if (api_.getCurrentTime() <= 3.9 && ego_linear_velocity >= 3.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (api_.getCurrentTime() >= 3.9999) {
if (
api_.getEntity("ego")->getCurrentTwist().linear.x <= 3.1 &&
api_.getEntity("ego")->getCurrentTwist().linear.x >= 2.9) {
if (ego_linear_velocity <= 3.1 && ego_linear_velocity >= 2.9) {
stop(cpp_mock_scenarios::Result::SUCCESS);
} else {
stop(cpp_mock_scenarios::Result::FAILURE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode
// SUCCESS
if (
npc->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) &&
ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) &&
ego->isInPosition(ego_target, 1.0) && npc->isInPosition(npc_target, 1.0) &&
npc->getCurrentTwist().linear.x < 0.5) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode
// SUCCESS
if (
npc->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) &&
ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) &&
ego->isInPosition(ego_target, 1.0) && npc->isInPosition(npc_target, 1.0) &&
npc->getCurrentTwist().linear.x < 2.5) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
Expand Down
Loading
Loading