Skip to content

Commit

Permalink
Merge branch 'RJD-1057-remove-functions-forwarded-to-entity-base-midd…
Browse files Browse the repository at this point in the history
…le' into RJD-1057-remove-functions-forwarded-to-entity-base-refactor
  • Loading branch information
dmoszynski authored Oct 14, 2024
2 parents 508e3f0 + f559200 commit 0149eec
Showing 1 changed file with 23 additions and 27 deletions.
50 changes: 23 additions & 27 deletions mock/cpp_mock_scenarios/src/random_scenario/random001.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -186,37 +186,33 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
spawn_and_cross_pedestrian(i);
}

{
const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
34621, 10, 0.0, api_.getHdmapUtils());
const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
34606, 0.0, 0.0, api_.getHdmapUtils());
const auto entity_name = "spawn_nearby_ego";

if (ego_entity) {
if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) {
api_.spawn(
entity_name,
traffic_simulator::pose::transformRelativePoseToGlobal(
ego_entity->getMapPose(),
geometry_msgs::build<geometry_msgs::msg::Pose>()
.position(geometry_msgs::build<geometry_msgs::msg::Point>().x(10).y(-5).z(0))
.orientation(geometry_msgs::msg::Quaternion())),
getVehicleParameters(),
traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
}
const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
34621, 10, 0.0, api_.getHdmapUtils());
constexpr auto entity_name = "spawn_nearby_ego";
if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) {
api_.spawn(
entity_name,
traffic_simulator::pose::transformRelativePoseToGlobal(
ego_entity->getMapPose(),
geometry_msgs::build<geometry_msgs::msg::Pose>()
.position(geometry_msgs::build<geometry_msgs::msg::Point>().x(10).y(-5).z(0))
.orientation(geometry_msgs::msg::Quaternion())),
getVehicleParameters(),
traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
}

if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) {
api_.despawn(entity_name);
}
}
if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) {
api_.despawn(entity_name);
}

if (ego_entity->isInPosition(ego_goal_position, 1.0)) {
api_.despawn("ego");
stop(cpp_mock_scenarios::Result::SUCCESS);
}
const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
34606, 0.0, 0.0, api_.getHdmapUtils());
if (ego_entity->isInPosition(ego_goal_position, 1.0)) {
api_.despawn("ego");
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}

void onInitialize() override
{
// api_.setVerbose(true);
Expand Down

0 comments on commit 0149eec

Please sign in to comment.