Feature/minimal arbitrator #390

kjrush · 2019-10-11T14:45:57Z

PR Details

Description

Implementation of the minimal Arbitrator and mock Plan Delegator nodes.

Adds the arbitrator package which contains both the arbitrator library (for unit testing) and arbitrator node proper. Arbitrator consists of a skeleton main function delegating execution flow to arbitrator.cpp which is primarily structural code that uses a generic PlanningStrategy to generate plans. TreePlanner is the only existing implementation of PlanningStrategy at present, and is itself generalized over CostFunction, NeighborGenerator, and SearchStrategy for testability and flexibility. The only implementations of these interfaces at present serve to implement a kind of Beam Search that uses the plugins to generate new states, and a list of fixed priorities to derive cost for each state. The search proceeds until a plan reaches the target length or if no new states can be expanded and the length is not reached, the longest plan is returned.

The Plan Delegator implementation is extremely minimalistic as this is not the focus of this task. It simply implements enough functionality to receive a maneuver plan and call the plan_trajectory service on the associated plugin, finally forwarding the new trajectory on to the Trajectory Executor node.

Motivation and Context

These nodes form 2 of the remaining elements of the core CARMA3 planning flow and thus are needed for all future plugin-based planning operation.

How Has This Been Tested?

25 unit tests have been written and passed for all algorithmically relevant components of the Arbitrator (i.e., non-structural, I/O, etc. code). The Plan Delegator node has no unit tests since most of it will be scrapped within the next sprint or two.

Types of changes

Defect fix (non-breaking change that fixes an issue)
New feature (non-breaking change that adds functionality)
Breaking change (fix or feature that cause existing functionality to change)

Checklist:

I have added any new packages to the sonar-scanner.properties file
My change requires a change to the documentation.
I have updated the documentation accordingly.
I have read the CONTRIBUTING document.
CARMA Contributing Guide
I have added tests to cover my changes.
All new and existing tests passed. (KR: Existing tests which failed prior to this change may still fail)

…bitrator

kjrush · 2019-10-11T15:17:18Z

Note: The dependency on usdot-fhwa-stol/carma-msgs#19 causes unit tests to break when run against develop branch CARMAMsgs. Unit tests for Arbitrator pass locally when run against the correct CARMAMsgs:

  /home/rushk/carma-ws/devel/lib/arbitrator/arbitrator-test --gtest_output=xml:/home/rushk/carma-ws/build/test_results/arbitrator/gtest-arbitrator-test.xml
[==========] Running 25 tests from 5 test cases.
[----------] Global test environment set-up.
[----------] 8 tests from ArbitratorStateMachineTest
[ RUN      ] ArbitratorStateMachineTest.testInitalize
[       OK ] ArbitratorStateMachineTest.testInitalize (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testPlanningCycle
[       OK ] ArbitratorStateMachineTest.testPlanningCycle (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testPauseResumeFromPlanning
[       OK ] ArbitratorStateMachineTest.testPauseResumeFromPlanning (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testPauseResumeFromWaiting
[       OK ] ArbitratorStateMachineTest.testPauseResumeFromWaiting (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testShutdown1
[       OK ] ArbitratorStateMachineTest.testShutdown1 (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testShutdown2
[       OK ] ArbitratorStateMachineTest.testShutdown2 (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testShutdown3
[       OK ] ArbitratorStateMachineTest.testShutdown3 (0 ms)
[ RUN      ] ArbitratorStateMachineTest.testShutdown4
[       OK ] ArbitratorStateMachineTest.testShutdown4 (0 ms)
[----------] 8 tests from ArbitratorStateMachineTest (0 ms total)

[----------] 3 tests from PluginNeighborGeneratorTest
[ RUN      ] PluginNeighborGeneratorTest.testInitalize
[       OK ] PluginNeighborGeneratorTest.testInitalize (0 ms)
[ RUN      ] PluginNeighborGeneratorTest.testGetNeighbors1
[       OK ] PluginNeighborGeneratorTest.testGetNeighbors1 (1 ms)
[ RUN      ] PluginNeighborGeneratorTest.testGetNeighbors2
[       OK ] PluginNeighborGeneratorTest.testGetNeighbors2 (0 ms)
[----------] 3 tests from PluginNeighborGeneratorTest (1 ms total)

[----------] 4 tests from FixedPriorityCostFunctionTest
[ RUN      ] FixedPriorityCostFunctionTest.testSingleManeuver
[       OK ] FixedPriorityCostFunctionTest.testSingleManeuver (0 ms)
[ RUN      ] FixedPriorityCostFunctionTest.testMixedPlanners
[       OK ] FixedPriorityCostFunctionTest.testMixedPlanners (0 ms)
[ RUN      ] FixedPriorityCostFunctionTest.testSingleManeuverUnitDistance
[       OK ] FixedPriorityCostFunctionTest.testSingleManeuverUnitDistance (1 ms)
[ RUN      ] FixedPriorityCostFunctionTest.testUnitDistanceCost
[       OK ] FixedPriorityCostFunctionTest.testUnitDistanceCost (0 ms)
[----------] 4 tests from FixedPriorityCostFunctionTest (1 ms total)

[----------] 7 tests from BeamSearchStrategyTest
[ RUN      ] BeamSearchStrategyTest.testEmptyList
[       OK ] BeamSearchStrategyTest.testEmptyList (0 ms)
[ RUN      ] BeamSearchStrategyTest.testSort1
[       OK ] BeamSearchStrategyTest.testSort1 (0 ms)
[ RUN      ] BeamSearchStrategyTest.testSort2
[       OK ] BeamSearchStrategyTest.testSort2 (0 ms)
[ RUN      ] BeamSearchStrategyTest.testSort3
[       OK ] BeamSearchStrategyTest.testSort3 (0 ms)
[ RUN      ] BeamSearchStrategyTest.testSortAndPrune1
[       OK ] BeamSearchStrategyTest.testSortAndPrune1 (0 ms)
[ RUN      ] BeamSearchStrategyTest.testSortAndPrune2
[       OK ] BeamSearchStrategyTest.testSortAndPrune2 (0 ms)
[ RUN      ] BeamSearchStrategyTest.testSortAndPrune3
[       OK ] BeamSearchStrategyTest.testSortAndPrune3 (0 ms)
[----------] 7 tests from BeamSearchStrategyTest (0 ms total)

[----------] 3 tests from TreePlannerTest
[ RUN      ] TreePlannerTest.testGeneratePlan1
[       OK ] TreePlannerTest.testGeneratePlan1 (1 ms)
[ RUN      ] TreePlannerTest.testGeneratePlan2
[       OK ] TreePlannerTest.testGeneratePlan2 (1 ms)
[ RUN      ] TreePlannerTest.testGeneratePlan3
[       OK ] TreePlannerTest.testGeneratePlan3 (1 ms)
[----------] 3 tests from TreePlannerTest (3 ms total)

[----------] Global test environment tear-down
[==========] 25 tests from 5 test cases ran. (5 ms total)
[  PASSED  ] 25 tests.```

arbitrator/CMakeLists.txt

arbitrator/config/arbitrator_params.yaml

arbitrator/include/arbitrator.hpp

arbitrator/package.xml

arbitrator/include/capabilities_interface.tpp

qswawrq · 2019-10-15T14:25:40Z

arbitrator/include/cost_function.hpp

+            /**
+             * \brief Virtual destructor provided for memory safety
+             */
+            virtual ~CostFunction(){};


You can mark it as pure virtual as well to make sure it will never be called by any child class. This apply to all destructors you have in other abstract class.

Hmm, defining this as pure virtual seems to remove the ability for my child classes to use the implicit destructor (which seems cleaner to me). I think it'd rather leave this as is, if it's not a big deal.

qswawrq · 2019-10-15T14:31:40Z

arbitrator/include/capabilities_interface.tpp

+
+namespace arbitrator 
+{
+    template<typename MSrv>


Not sure I get the reason why you template this class.

It's just this function that's templated, but it's so it could be used for multiple different kinds of services calls (each needing a different kind of MSrv template parameter for their message type).

Sorry I meant this template function. Does arbitrator needs to call anything else than STRATEGIC_PLAN service?

Not in the present, thought in the future I believe it probably will. For example it'll need to use a different service to interact with the cost plugins to get their costs for a given plan.

arbitrator/src/capabilities_interface.cpp

qswawrq · 2019-10-15T14:38:55Z

arbitrator/src/fixed_priority_cost_function.cpp

+        // Normalize the list and invert into costs
+        for (auto it = plugin_priorities.begin(); it != plugin_priorities.end(); it++)
+        {
+            plugin_costs_[it->first] = 1.0 - (it->second / max_priority);


Will be a potential "Divide by Zero" error here if all priorities are not set and default to zero? This also apply to the following compute_cost_per_unit_distance function.

Yeah, it would be. The alternative to throwing a divide by zero exception here would be to catch the exception (or check if it would be thrown) and then throw another exception instead, so I'm not clear if that's any better than just throwing a divide by zero.

I think the easy way to handle it is to check if max_priority == 0 and assign its cost to infinity/maximum.

qswawrq

Two small comments.

arbitrator/include/arbitrator.hpp

arbitrator/src/arbitrator.cpp

maefromm · 2019-10-18T19:02:27Z

@qswawrq feel free to merge to develop since freeze has been lifted now. whenever it's ready. thanks.

kjrush added 28 commits September 18, 2019 10:12

Initial commit of arbitrator package

3915e69

Initial implementation of the ArbitratorStateMachine

2becc78

Initial build out of capabilities interface

7a18e1b

Initial header files for planning mode

0d25f77

Finish setup of header files for Arbitrator planning

d64544c

Implement tree planner search

8f709b6

Refactor search strategy to use map

4ad261c

Refactor back to std::vector

ba4dd7b

Instantiate arbitrator with dependency injection

13d5b43

Add virtual destructors to interfaces

6005be6

Finish implementation of fixed_priority_cost_function

63c42b0

Merge remote-tracking branch 'origin/develop' into feature/minimal-ar…

9b9f7ad

…bitrator

Implement initial plugin neighbor generator

d25537a

First take at capabilities interface

7a621f2

Resolve compilation issues

6a9d630

Resolve linker errors

f88af02

Fix template substitution errors

5cf6e39

Add waiting and shutdown states

8bb2825

Merge remote-tracking branch 'origin/develop' into feature/minimal-ar…

bee572e

…bitrator

Add doc comments

0f0962a

Implement tests for ArbitratorStateMachine

24846f6

Implement tests for FixedPriorityCostFunction

3d8f26d

Implement tests for beam search strategy

2357b1e

Implement tests for plugin neighbor generator

c356c78

Implement tests for tree planner

a8559bf

Merge remote-tracking branch 'origin/develop' into feature/minimal-ar…

5223419

…bitrator

Implement minimal plan delegator

9198b91

Update sonar scanner properties to include arbitrator

a586b34

kjrush requested a review from qswawrq October 11, 2019 14:45

kjrush self-assigned this Oct 11, 2019

kjrush assigned qswawrq Oct 11, 2019

Remove sonar tests module for plan_delegator

779724a

kjrush mentioned this pull request Oct 11, 2019

Fix maneuver planning messages for minimal arbitrator usdot-fhwa-stol/carma-msgs#19

Merged

kjrush added 6 commits October 11, 2019 17:03

Fix linker error and add basic launch file

68cf489

Fix arbitrator launch and link

a7eef41

Add basic plan delegator launch file

7a920d6

Commit actual launch file

350361d

Integrate arbitrator and plan delegator with main launch file

cc53f53

Add launch directory to install

70e941d

qswawrq suggested changes Oct 15, 2019

View reviewed changes

kjrush added 2 commits October 15, 2019 14:05

Address PR comments

a3a2995

Fix unit tests after PR feedback

db931c8

kjrush requested a review from qswawrq October 15, 2019 19:28

qswawrq suggested changes Oct 15, 2019

View reviewed changes

arbitrator/include/arbitrator.hpp Outdated Show resolved Hide resolved

arbitrator/src/arbitrator.cpp Outdated Show resolved Hide resolved

Address final PR comments

7e80a76

qswawrq approved these changes Oct 16, 2019

View reviewed changes

kjrush merged commit f3f24b3 into develop Oct 21, 2019

kjrush deleted the feature/minimal-arbitrator branch October 21, 2019 12:53

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/minimal arbitrator #390

Feature/minimal arbitrator #390

kjrush commented Oct 11, 2019 •

edited

kjrush commented Oct 11, 2019

qswawrq Oct 15, 2019

kjrush Oct 15, 2019

qswawrq Oct 15, 2019

kjrush Oct 15, 2019

qswawrq Oct 15, 2019

kjrush Oct 15, 2019

qswawrq Oct 15, 2019

kjrush Oct 15, 2019 •

edited

qswawrq Oct 15, 2019

qswawrq left a comment

maefromm commented Oct 18, 2019

Feature/minimal arbitrator #390

Feature/minimal arbitrator #390

Conversation

kjrush commented Oct 11, 2019 • edited

PR Details

Description

Motivation and Context

How Has This Been Tested?

Types of changes

Checklist:

kjrush commented Oct 11, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kjrush Oct 15, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

qswawrq left a comment

Choose a reason for hiding this comment

maefromm commented Oct 18, 2019

kjrush commented Oct 11, 2019 •

edited

kjrush Oct 15, 2019 •

edited