Skip to content

Commit

Permalink
ref(cpp_mock_scenario): adapt to getEntity changes - return reference
Browse files Browse the repository at this point in the history
  • Loading branch information
dmoszynski committed Jan 3, 2025
1 parent 57fcf94 commit 2f74a79
Show file tree
Hide file tree
Showing 48 changed files with 367 additions and 371 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -42,13 +42,13 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode
/// @note When using the do_nothing plugin, the return value of the `getCurrentAction` function is always do_nothing.

if (
api_.getEntity("ego")->getCurrentAction() != "do_nothing" ||
api_.getEntity("pedestrian")->getCurrentAction() != "do_nothing") {
api_.getEntity("ego").getCurrentAction() != "do_nothing" ||
api_.getEntity("pedestrian").getCurrentAction() != "do_nothing") {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (
api_.getEntity("vehicle_spawn_with_behavior_tree")->getCurrentAction() == "do_nothing" ||
api_.getEntity("pedestrian_spawn_with_behavior_tree")->getCurrentAction() == "do_nothing") {
api_.getEntity("vehicle_spawn_with_behavior_tree").getCurrentAction() == "do_nothing" ||
api_.getEntity("pedestrian_spawn_with_behavior_tree").getCurrentAction() == "do_nothing") {
stop(cpp_mock_scenarios::Result::FAILURE);
}
api_.resetBehaviorPlugin(
Expand All @@ -58,8 +58,8 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode
"pedestrian_spawn_with_behavior_tree",
traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing());
if (
api_.getEntity("vehicle_spawn_with_behavior_tree")->getCurrentAction() != "do_nothing" ||
api_.getEntity("pedestrian_spawn_with_behavior_tree")->getCurrentAction() != "do_nothing") {
api_.getEntity("vehicle_spawn_with_behavior_tree").getCurrentAction() != "do_nothing" ||
api_.getEntity("pedestrian_spawn_with_behavior_tree").getCurrentAction() != "do_nothing") {
stop(cpp_mock_scenarios::Result::FAILURE);
}

Expand Down
14 changes: 7 additions & 7 deletions mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,21 +56,21 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(0.0);
ego_entity->requestSpeedChange(15, true);
auto & ego_entity = api_.getEntity("ego");
ego_entity.setLinearVelocity(0.0);
ego_entity.requestSpeedChange(15, true);
traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter;
behavior_parameter.see_around = false;
ego_entity->setBehaviorParameter(behavior_parameter);
ego_entity.setBehaviorParameter(behavior_parameter);

api_.spawn(
"npc",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto npc_entity = api_.getEntity("npc");
npc_entity->setLinearVelocity(0.0);
npc_entity->requestSpeedChange(5, true);
auto & npc_entity = api_.getEntity("npc");
npc_entity.setLinearVelocity(0.0);
npc_entity.requestSpeedChange(5, true);
}
};
} // namespace cpp_mock_scenarios
Expand Down
12 changes: 6 additions & 6 deletions mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,18 +54,18 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 0.2, 1.3, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(0);
ego_entity->requestSpeedChange(0, true);
auto & ego_entity = api_.getEntity("ego");
ego_entity.setLinearVelocity(0);
ego_entity.requestSpeedChange(0, true);

api_.spawn(
"bob",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 0.0, -0.874, api_.getHdmapUtils()),
getPedestrianParameters());
auto bob_entity = api_.getEntity("bob");
bob_entity->setLinearVelocity(0);
bob_entity->requestSpeedChange(0, true);
auto & bob_entity = api_.getEntity("bob");
bob_entity.setLinearVelocity(0);
bob_entity.requestSpeedChange(0, true);
}
};
} // namespace cpp_mock_scenarios
Expand Down
16 changes: 8 additions & 8 deletions mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,8 +106,8 @@ void CppScenarioNode::spawnEgoEntity(
api_.updateFrame();
std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
api_.spawn("ego", spawn_lanelet_pose, parameters, traffic_simulator::VehicleBehavior::autoware());
auto ego_entity = api_.getEgoEntity("ego");
ego_entity->setParameter<bool>("allow_goal_modification", true);
auto & ego_entity = api_.getEgoEntity("ego");
ego_entity.setParameter<bool>("allow_goal_modification", true);
api_.attachLidarSensor("ego", 0.0);

api_.attachDetectionSensor("ego", 200.0, true, 0.0, 0, 0.0, 0.0);
Expand All @@ -126,12 +126,12 @@ void CppScenarioNode::spawnEgoEntity(
// clang-format on
return configuration;
}());
ego_entity->requestAssignRoute(goal_lanelet_poses);
ego_entity.requestAssignRoute(goal_lanelet_poses);

using namespace std::chrono_literals;
while (!ego_entity->isEngaged()) {
if (ego_entity->isEngageable()) {
ego_entity->engage();
while (!ego_entity.isEngaged()) {
if (ego_entity.isEngageable()) {
ego_entity.engage();
}
api_.updateFrame();
std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
Expand All @@ -140,13 +140,13 @@ void CppScenarioNode::spawnEgoEntity(

auto CppScenarioNode::isVehicle(const std::string & name) const -> bool
{
return api_.getEntity(name)->getEntityType().type ==
return api_.getEntity(name).getEntityType().type ==
traffic_simulator_msgs::msg::EntityType::VEHICLE;
}

auto CppScenarioNode::isPedestrian(const std::string & name) const -> bool
{
return api_.getEntity(name)->getEntityType().type ==
return api_.getEntity(name).getEntityType().type ==
traffic_simulator_msgs::msg::EntityType::PEDESTRIAN;
}

Expand Down
18 changes: 9 additions & 9 deletions mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,11 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode
* @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P
*/
if (t == 1.0) {
if (t != api_.getEntity("bob")->getCurrentTwist().linear.x) {
if (t != api_.getEntity("bob").getCurrentTwist().linear.x) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x;
const auto ego_linear_velocity = api_.getEntity("ego").getCurrentTwist().linear.x;
if (t >= 6.6) {
if (7.5 >= t) {
if (std::fabs(0.1) <= ego_linear_velocity) {
Expand All @@ -79,10 +79,10 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
120545, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(10);
ego_entity->requestSpeedChange(8, true);
ego_entity->requestAssignRoute(std::vector<traffic_simulator::CanonicalizedLaneletPose>{
auto & ego_entity = api_.getEntity("ego");
ego_entity.setLinearVelocity(10);
ego_entity.requestSpeedChange(8, true);
ego_entity.requestAssignRoute(std::vector<traffic_simulator::CanonicalizedLaneletPose>{
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34675, 0.0, 0.0, api_.getHdmapUtils()),
traffic_simulator::helper::constructCanonicalizedLaneletPose(
Expand All @@ -93,9 +93,9 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34378, 0.0, 0.0, api_.getHdmapUtils()),
getPedestrianParameters());
auto bob_entity = api_.getEntity("bob");
bob_entity->setLinearVelocity(0);
bob_entity->requestSpeedChange(
auto & bob_entity = api_.getEntity("bob");
bob_entity.setLinearVelocity(0);
bob_entity.requestSpeedChange(
1.0, traffic_simulator::speed_change::Transition::LINEAR,
traffic_simulator::speed_change::Constraint(
traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,9 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
private:
void onUpdate() override
{
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;
const double ego_linear_acceleration = api_.getEntity("ego").getCurrentAccel().linear.x;
const double ego_linear_velocity = api_.getEntity("ego").getCurrentTwist().linear.x;
const double npc_linear_velocity = api_.getEntity("npc").getCurrentTwist().linear.x;

// LCOV_EXCL_START
if (npc_linear_velocity > (ego_linear_velocity + 1) && ego_linear_acceleration < 0) {
Expand All @@ -63,16 +62,16 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
api_.getEntity("ego")->setLinearVelocity(3);
api_.getEntity("ego").setLinearVelocity(3);

api_.spawn(
"npc",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto npc_entity = api_.getEntity("npc");
npc_entity->setLinearVelocity(10);
npc_entity->requestSpeedChange(10, true);
auto & npc_entity = api_.getEntity("npc");
npc_entity.setLinearVelocity(10);
npc_entity.requestSpeedChange(10, true);
}
};
} // namespace cpp_mock_scenarios
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,9 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
private:
void onUpdate() override
{
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;
const double ego_linear_acceleration = api_.getEntity("ego").getCurrentAccel().linear.x;
const double ego_linear_velocity = api_.getEntity("ego").getCurrentTwist().linear.x;
const double npc_linear_velocity = api_.getEntity("npc").getCurrentTwist().linear.x;

// LCOV_EXCL_START
if (ego_linear_velocity > (npc_linear_velocity + 1) && ego_linear_acceleration > 0) {
Expand All @@ -63,16 +62,16 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
api_.getEntity("ego")->setLinearVelocity(15);
api_.getEntity("ego").setLinearVelocity(15);

api_.spawn(
"npc",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 15.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto npc_entity = api_.getEntity("npc");
npc_entity->setLinearVelocity(10);
npc_entity->requestSpeedChange(10, true);
auto & npc_entity = api_.getEntity("npc");
npc_entity.setLinearVelocity(10);
npc_entity.requestSpeedChange(10, true);
}
};
} // namespace cpp_mock_scenarios
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar
bool requested = false;
void onUpdate() override
{
if (api_.getEntity("ego")->isInLanelet(34408, 0.1)) {
if (api_.getEntity("ego").isInLanelet(34408, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand All @@ -51,16 +51,16 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
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));
ego_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()));
ego_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 @@ -40,7 +40,7 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN
bool requested = false;
void onUpdate() override
{
if (api_.getEntity("ego")->isInLanelet(34630, 0.1)) {
if (api_.getEntity("ego").isInLanelet(34630, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand All @@ -51,17 +51,17 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(10);
ego_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())));
ego_entity->requestAssignRoute(goal_poses);
ego_entity.requestAssignRoute(goal_poses);
}
};
} // namespace cpp_mock_scenarios
Expand Down
16 changes: 8 additions & 8 deletions mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode
bool canceled = false;
void onUpdate() override
{
const auto ego_entity = api_.getEntity("ego");
if (ego_entity->isInPosition(
auto & ego_entity = api_.getEntity("ego");
if (ego_entity.isInPosition(
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 30, 0, api_.getHdmapUtils()),
3.0)) {
ego_entity->cancelRequest();
ego_entity.cancelRequest();
canceled = true;
}
if (ego_entity->isInLanelet(34507, 0.1)) {
if (ego_entity.isInLanelet(34507, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}
Expand All @@ -58,12 +58,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(7);
ego_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());
ego_entity->requestAcquirePosition(goal_pose);
ego_entity.requestAcquirePosition(goal_pose);
}
};
} // namespace cpp_mock_scenarios
Expand Down
12 changes: 6 additions & 6 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,11 +40,11 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
bool requested = false;
void onUpdate() override
{
if (const auto ego_entity = api_.getEntity("ego"); !ego_entity->isInLanelet()) {
if (const auto & ego_entity = api_.getEntity("ego"); !ego_entity.isInLanelet()) {
stop(cpp_mock_scenarios::Result::FAILURE);
} else if (ego_entity->isInLanelet(34507, 0.1)) {
} else if (ego_entity.isInLanelet(34507, 0.1)) {
stop(cpp_mock_scenarios::Result::SUCCESS);
} else if (std::abs(ego_entity->getCanonicalizedStatus().getLaneletPose().offset) <= 2.8) {
} else if (std::abs(ego_entity.getCanonicalizedStatus().getLaneletPose().offset) <= 2.8) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
}
Expand All @@ -55,9 +55,9 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34513, 0.0, 3.0, api_.getHdmapUtils()),
getVehicleParameters());
auto ego_entity = api_.getEntity("ego");
ego_entity->setLinearVelocity(10);
ego_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
Loading

0 comments on commit 2f74a79

Please sign in to comment.