Skip to content
Snippets Groups Projects
Commit 794c247f authored by Stephen Ryan's avatar Stephen Ryan
Browse files

fix: Override events on the same tick

parent 95cd6a1d
No related branches found
No related tags found
No related merge requests found
Pipeline #64940 failed
......@@ -13,6 +13,8 @@
#include <memory>
#include <agnostic_behavior_tree/composite/parallel_node.h>
#include "Conversion/OscToNode/ParseEvents.h"
#include "Node/ManeuverNode.h"
......@@ -21,8 +23,8 @@ namespace OpenScenarioEngine::v1_3
yase::BehaviorNode::Ptr parse(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IManeuver> maneuver)
{
auto maneuver_node = std::make_shared<Node::ManeuverNode>("Maneuver");
maneuver_node->setChild(parse(maneuver->GetEvents()));
auto events = parse(maneuver->GetEvents());
maneuver_node->AddEvents(std::dynamic_pointer_cast<yase::ParallelNode>(events));
// parse(maneuver->GetParameterDeclarations())
return maneuver_node;
}
......
......@@ -17,15 +17,15 @@ EventNode::EventNode(const std::string& name,
yase::BehaviorNode::Ptr actions,
yase::BehaviorNode::Ptr start_trigger,
EventPriority priority)
: TriggerableCompositeNode{name}, event_priority_{priority}, start_trigger_{start_trigger}
: yase::DecoratorNode{name}, event_priority_{priority}, start_trigger_{std::move(start_trigger)}
{
set(std::move(actions), StopTriggerPtr{nullptr}, StartTriggerPtr{std::move(start_trigger)});
setChild(std::move(actions));
}
void EventNode::lookupAndRegisterData(yase::Blackboard& blackboard)
{
event_prioritizer_ = blackboard.get<std::shared_ptr<OpenScenarioEngine::v1_3::IEventPrioritizer> >("EventPrioritizer");
event_prioritizer_->RegisterEvent(name(), event_priority_);
event_prioritizer_->RegisterEvent(name(), event_priority_, start_trigger_);
}
yase::NodeStatus EventNode::tick()
......@@ -35,14 +35,7 @@ yase::NodeStatus EventNode::tick()
return yase::NodeStatus::kSuccess;
}
auto current_status = TriggerableCompositeNode::tick();
if (start_trigger_->status() == yase::NodeStatus::kSuccess)
{
event_prioritizer_->EventStarted(name());
}
return current_status;
return child().executeTick();
}
} // namespace OpenScenarioEngine::v1_3::Node
......@@ -12,7 +12,8 @@
#pragma once
#include "Node/TriggerableCompositeNode.h"
#include <agnostic_behavior_tree/decorator_node.h>
#include "Utils/EventPriority.h"
#include "Utils/IEventPrioritizer.h"
......@@ -20,7 +21,7 @@ namespace OpenScenarioEngine::v1_3::Node
{
/// @brief Runs a list of actions when the start trigger is successful. It can be given a priority to interact with other nodes in the same manevuer.
class EventNode : public TriggerableCompositeNode
class EventNode : public yase::DecoratorNode
{
public:
/// @brief Construct a new event node
......@@ -34,10 +35,14 @@ public:
yase::BehaviorNode::Ptr start_trigger = nullptr,
EventPriority priority = EventPriority::kUnknown);
virtual ~EventNode() = default;
void lookupAndRegisterData(yase::Blackboard& blackboard) override;
yase::NodeStatus tick() override;
yase::BehaviorNode::Ptr GetStartTrigger() { return start_trigger_; }
private:
EventPriority event_priority_;
yase::BehaviorNode::Ptr start_trigger_;
......
......@@ -10,19 +10,43 @@
#include "Node/ManeuverNode.h"
#include "Node/EventNode.h"
#include "Utils/EventPrioritizer.h"
namespace OpenScenarioEngine::v1_3::Node
{
ManeuverNode::ManeuverNode(const std::string& name)
: yase::DecoratorNode{name}, eventPrioritizer_{std::make_shared<EventPrioritizer>()}
: yase::CompositeNode{name}, eventPrioritizer_{std::make_shared<EventPrioritizer>()}
{
start_triggers_ = std::make_shared<yase::ParallelNode>("Event Start Triggers");
addChild(start_triggers_);
}
void ManeuverNode::onInit()
{
for (size_t i = 0; i < events_->childrenCount(); i++)
{
auto& event = dynamic_cast<EventNode&>(events_->child(i));
start_triggers_->addChild(event.GetStartTrigger());
}
std::for_each(m_children_nodes.begin(),
m_children_nodes.end(),
[](auto&& child)
{ child->onInit(); });
}
void ManeuverNode::AddEvents(yase::ParallelNode::Ptr events)
{
events_ = std::move(events);
addChild(events_);
}
yase::NodeStatus ManeuverNode::tick()
{
start_triggers_->executeTick();
eventPrioritizer_->UpdateOverriddenEvents();
return child().executeTick();
return events_->executeTick();
}
void ManeuverNode::lookupAndRegisterData(yase::Blackboard &blackboard)
......
......@@ -10,7 +10,8 @@
#pragma once
#include <agnostic_behavior_tree/decorator_node.h>
#include <agnostic_behavior_tree/composite/parallel_node.h>
#include <agnostic_behavior_tree/composite_node.h>
namespace OpenScenarioEngine::v1_3
{
......@@ -20,7 +21,7 @@ namespace Node
{
/// @brief Runs a list of events. Nodes in the same maneuver can be skipped or overridden based on an event's priority.
class ManeuverNode : public yase::DecoratorNode
class ManeuverNode : public yase::CompositeNode
{
public:
/// @brief Construct a new maneuver node
......@@ -28,11 +29,18 @@ public:
/// @param name Name of the maneuver node
explicit ManeuverNode(const std::string& name);
virtual ~ManeuverNode() = default;
void onInit() override;
void AddEvents(yase::ParallelNode::Ptr events);
private:
void lookupAndRegisterData(yase::Blackboard& blackboard) override;
yase::NodeStatus tick() final;
std::shared_ptr<IEventPrioritizer> eventPrioritizer_;
yase::ParallelNode::Ptr start_triggers_;
yase::ParallelNode::Ptr events_;
};
} // namespace Node
......
......@@ -29,6 +29,7 @@ public:
explicit TransientNode(const std::string& name)
: DecoratorNode{name} {};
TransientNode(TransientNode& other) = default;
~TransientNode() override = default;
yase::NodeStatus tick() override
......@@ -267,4 +268,4 @@ private:
yase::NodeStatus stop_trigger_status_{yase::NodeStatus::kIdle};
};
} // namespace OpenScenarioEngine::v1_3::Node
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3::Node
......@@ -13,20 +13,11 @@
namespace OpenScenarioEngine::v1_3
{
void EventPrioritizer::RegisterEvent(const Event& name, EventPriority priority)
void EventPrioritizer::RegisterEvent(const Event& name, EventPriority priority, yase::BehaviorNode::Ptr start_trigger)
{
event_priorities_.insert({name, priority});
}
void EventPrioritizer::EventStarted(const Event& name)
{
auto [_, just_started] = started_events_.insert(name);
if (just_started &&
(event_priorities_.at(name) == EventPriority::kOverride ||
event_priorities_.at(name) == EventPriority::kOverwrite))
{
overriding_event_ = name;
}
start_triggers_.insert({name, *start_trigger});
events_.insert(name);
}
bool EventPrioritizer::ShouldSkipChild(const Event& name) const
......@@ -36,29 +27,48 @@ bool EventPrioritizer::ShouldSkipChild(const Event& name) const
bool EventPrioritizer::ShouldStopChild(const Event& name) const
{
if (auto event = events_to_override_.find(name); event != events_to_override_.end())
return (events_to_override_.count(name) > 0);
}
void EventPrioritizer::UpdateOverriddenEvents()
{
update_started_events();
mark_events_overridden();
overriding_event_.clear();
}
void EventPrioritizer::event_started(const Event& name)
{
auto [_, just_started] = started_events_.insert(name);
if (just_started &&
(event_priorities_.at(name) == EventPriority::kOverride ||
event_priorities_.at(name) == EventPriority::kOverwrite))
{
return true;
overriding_event_ = name;
}
return false;
}
void EventPrioritizer::UpdateOverriddenEvents()
void EventPrioritizer::update_started_events()
{
if (!overriding_event_.empty())
for (const auto& event : events_)
{
mark_events_overridden(overriding_event_);
if (start_triggers_.at(event).status() == yase::NodeStatus::kSuccess)
{
event_started(event);
}
}
overriding_event_.clear();
}
void EventPrioritizer::mark_events_overridden(const Event& overriding_event)
void EventPrioritizer::mark_events_overridden()
{
for (const auto& event : started_events_)
if (!overriding_event_.empty())
{
if (event != overriding_event)
for (const auto& event : started_events_)
{
events_to_override_.insert(event);
if (event != overriding_event_)
{
events_to_override_.insert(event);
}
}
}
}
......
......@@ -24,17 +24,20 @@ namespace OpenScenarioEngine::v1_3
class EventPrioritizer : public IEventPrioritizer
{
public:
void RegisterEvent(const Event& name, EventPriority priority) override;
void EventStarted(const Event& name) override;
void RegisterEvent(const Event& name, EventPriority priority, yase::BehaviorNode::Ptr start_trigger) override;
[[nodiscard]] bool ShouldSkipChild(const Event& name) const override;
[[nodiscard]] bool ShouldStopChild(const Event& name) const override;
void UpdateOverriddenEvents() override;
private:
void mark_events_overridden(const Event& overriding_event);
void event_started(const Event& name);
void update_started_events();
void mark_events_overridden();
std::unordered_map<Event, EventPriority> event_priorities_;
std::unordered_map<Event, yase::BehaviorNode&> start_triggers_;
std::set<Event> started_events_;
std::set<Event> events_;
std::set<Event> events_to_override_;
Event overriding_event_;
};
......
......@@ -13,6 +13,8 @@
#include <string>
#include <vector>
#include <agnostic_behavior_tree/behavior_node.h>
#include "Utils/EventPriority.h"
namespace OpenScenarioEngine::v1_3
......@@ -32,12 +34,13 @@ public:
///
/// @param name The name of the event
/// @param priority The priority of the event
virtual void RegisterEvent(const Event& name, EventPriority priority) = 0;
/// @param start_trigger The start trigger of the event
virtual void RegisterEvent(const Event& name, EventPriority priority, yase::BehaviorNode::Ptr start_trigger) = 0;
/// @brief Set the state of an event to started
///
/// @param name The name of the event
virtual void EventStarted(const Event& name) = 0;
// virtual void EventStarted(const Event& name) = 0;
/// @brief Check whether an event should be skipped based on the priority
///
......
......@@ -8,9 +8,9 @@
* SPDX-License-Identifier: EPL-2.0
********************************************************************************/
#include <agnostic_behavior_tree/decorator/data_declaration_node.h>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <agnostic_behavior_tree/decorator/data_declaration_node.h>
#include <memory>
......@@ -60,7 +60,7 @@ protected:
: root{tree.get_root()}, event_prioritizer{tree.get_event_prioritizer()}
{
ON_CALL(event_prioritizer, RegisterEvent).WillByDefault(Return());
ON_CALL(*mock_action_node, tick()).WillByDefault(Return(yase::NodeStatus::kSuccess));
ON_CALL(*mock_action_node, tick()).WillByDefault(Return(yase::NodeStatus::kRunning));
ON_CALL(*mock_start_trigger, tick()).WillByDefault(Return(yase::NodeStatus::kSuccess));
}
......@@ -84,7 +84,6 @@ TEST_F(EventNodeTest, GivenUnstartedNodeWithParallelPriority_WhenParentTicks_The
EXPECT_CALL(event_prioritizer, ShouldStopChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, ShouldSkipChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, EventStarted).Times(1).WillOnce(Return());
ASSERT_THAT(root.executeTick(), yase::NodeStatus::kRunning);
}
......@@ -96,7 +95,6 @@ TEST_F(EventNodeTest, GivenUnstartedNodeWithOverridePriority_WhenParentTicks_The
EXPECT_CALL(event_prioritizer, ShouldStopChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, ShouldSkipChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, EventStarted).Times(1).WillOnce(Return());
ASSERT_THAT(root.executeTick(), yase::NodeStatus::kRunning);
}
......@@ -108,7 +106,6 @@ TEST_F(EventNodeTest, GivenUnstartedNodeWithOverwritePriority_WhenParentTicks_Th
EXPECT_CALL(event_prioritizer, ShouldStopChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, ShouldSkipChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, EventStarted).Times(1).WillOnce(Return());
ASSERT_THAT(root.executeTick(), yase::NodeStatus::kRunning);
}
......@@ -120,7 +117,6 @@ TEST_F(EventNodeTest, GivenUnstartedNodeWithSkipPriority_WhenParentTicks_ThenEve
EXPECT_CALL(event_prioritizer, ShouldStopChild).Times(1).WillOnce(Return(false));
EXPECT_CALL(event_prioritizer, ShouldSkipChild).Times(1).WillOnce(Return(true));
EXPECT_CALL(event_prioritizer, EventStarted).Times(0);
ASSERT_THAT(root.executeTick(), yase::NodeStatus::kSuccess);
}
......@@ -132,6 +128,5 @@ TEST_F(EventNodeTest, GivenStartedNodeWithNonSkipPriority_WhenShouldStopChildAnd
EXPECT_CALL(event_prioritizer, ShouldStopChild).Times(1).WillOnce(Return(true));
EXPECT_CALL(event_prioritizer, ShouldSkipChild).Times(0);
EXPECT_CALL(event_prioritizer, EventStarted).Times(0);
ASSERT_THAT(root.executeTick(), yase::NodeStatus::kSuccess);
}
......@@ -37,11 +37,11 @@ enum class OscStatus : std::uint8_t
class ManeuverNodeTest : public testing::Test
{
protected:
using EventWithStartTrigger = std::pair<yase::BehaviorNode&, FakeActionNode&>;
using EventWithStartTrigger = std::pair<EventNode&, FakeActionNode&>;
ManeuverNodeTest()
{
maneuver_node->setChild(events_node);
maneuver_node->AddEvents(events_node);
}
EventWithStartTrigger AddEventWithPriority(EventPriority priority)
......@@ -61,10 +61,10 @@ protected:
auto event = std::make_shared<EventNode>("Event " + std::to_string(events_node->childrenCount() + 1), mock_action_node, mock_start_trigger, priority);
events_node->addChild(event);
return EventWithStartTrigger{events_node->child(events_node->childrenCount() - 1), *mock_start_trigger};
return EventWithStartTrigger{*event, *mock_start_trigger};
}
static OscStatus GetEventStatus(const yase::BehaviorNode& event, const FakeActionNode& start_trigger)
static OscStatus GetEventStatus(const EventNode& event, const FakeActionNode& start_trigger)
{
auto osc_status = OscStatus::kUnknown;
const auto& event_status = event.status();
......@@ -103,6 +103,8 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithParallelPriority_WhenManeuverTick
auto [event1, start_trigger1] = AddEventWithPriority(EventPriority::kParallel);
auto [event2, start_trigger2] = AddEventWithPriority(EventPriority::kParallel);
auto [event3, start_trigger3] = AddEventWithPriority(EventPriority::kParallel);
maneuver_node->onInit();
maneuver_node->distributeData();
maneuver_node->executeTick();
......@@ -122,6 +124,8 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithParallelorSkipPriority_WhenManeuv
auto [event1, start_trigger1] = AddEventWithPriority(EventPriority::kParallel);
auto [event2, start_trigger2] = AddEventWithPriority(EventPriority::kSkip);
auto [event3, start_trigger3] = AddEventWithPriority(EventPriority::kParallel);
maneuver_node->onInit();
maneuver_node->distributeData();
maneuver_node->executeTick();
......@@ -137,12 +141,13 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithOneOverridePriority_WhenManeuverT
auto [event3, start_trigger3] = AddEventWithPriority(EventPriority::kParallel);
auto [event4, start_trigger4] = AddEventWithPriority(EventPriority::kOverride);
maneuver_node->onInit();
maneuver_node->distributeData();
maneuver_node->executeTick();
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event2, start_trigger2), OscStatus::kSkipped);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event4, start_trigger4), OscStatus::kRunning);
maneuver_node->executeTick();
......@@ -163,12 +168,13 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithOneOverridePriority_WhenManeuverT
auto [event3, start_trigger3] = AddEventWithPriority(EventPriority::kParallel);
auto [event4, start_trigger4] = AddEventWithPriority(EventPriority::kOverride);
maneuver_node->onInit();
maneuver_node->distributeData();
maneuver_node->executeTick();
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kStandby);
EXPECT_THAT(GetEventStatus(event2, start_trigger2), OscStatus::kSkipped);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event4, start_trigger4), OscStatus::kRunning);
maneuver_node->executeTick();
......@@ -203,12 +209,13 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithMultipleOverridePriority_WhenMane
.WillOnce(Return(yase::NodeStatus::kRunning))
.WillRepeatedly(Return(yase::NodeStatus::kSuccess));
maneuver_node->onInit();
maneuver_node->distributeData();
maneuver_node->executeTick();
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event2, start_trigger2), OscStatus::kSkipped);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event4, start_trigger4), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event5, start_trigger5), OscStatus::kStandby);
EXPECT_THAT(GetEventStatus(event6, start_trigger6), OscStatus::kStandby);
......@@ -227,8 +234,8 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithMultipleOverridePriority_WhenMane
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event2, start_trigger2), OscStatus::kSkipped);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event4, start_trigger4), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event5, start_trigger5), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event4, start_trigger4), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event5, start_trigger5), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event6, start_trigger6), OscStatus::kRunning);
maneuver_node->executeTick();
......@@ -249,12 +256,13 @@ TEST_F(ManeuverNodeTest, GivenMultipleNodesWithOneOverwritePriority_WhenManeuver
auto [event3, start_trigger3] = AddEventWithPriority(EventPriority::kParallel);
auto [event4, start_trigger4] = AddEventWithPriority(EventPriority::kOverwrite);
maneuver_node->onInit();
maneuver_node->distributeData();
maneuver_node->executeTick();
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event1, start_trigger1), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event2, start_trigger2), OscStatus::kSkipped);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kRunning);
EXPECT_THAT(GetEventStatus(event3, start_trigger3), OscStatus::kSuccess);
EXPECT_THAT(GetEventStatus(event4, start_trigger4), OscStatus::kRunning);
maneuver_node->executeTick();
......
......@@ -19,13 +19,12 @@ namespace testing::OpenScenarioEngine::v1_3
class MockEventPrioritizer : public ::OpenScenarioEngine::v1_3::IEventPrioritizer
{
public:
MOCK_METHOD(void, RegisterEvent, (const ::OpenScenarioEngine::v1_3::Event&, ::OpenScenarioEngine::v1_3::EventPriority), (override));
MOCK_METHOD(void, EventStarted, (const ::OpenScenarioEngine::v1_3::Event&), (override));
MOCK_METHOD(void, RegisterEvent, (const ::OpenScenarioEngine::v1_3::Event&, ::OpenScenarioEngine::v1_3::EventPriority, ::yase::BehaviorNode::Ptr), (override));
MOCK_METHOD(bool, ShouldSkipChild, (const ::OpenScenarioEngine::v1_3::Event&), (const, override));
MOCK_METHOD(bool, ShouldStopChild, (const ::OpenScenarioEngine::v1_3::Event&), (const, override));
MOCK_METHOD(void, UpdateOverriddenEvents, (), (override));
static inline std::shared_ptr<MockEventPrioritizer> make_shared()
static std::shared_ptr<MockEventPrioritizer> make_shared()
{
return std::make_shared<MockEventPrioritizer>();
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment