Feature/put the planner together #232
Conversation
Co-authored-by: Erick Fuentes <erick@csail.mit.edu>
…. Working on goal scoring function
| @@ -416,13 +416,13 @@ planning::BeliefUpdater<RobotBelief> make_belief_updater(const planning::RoadMap | |||
| planning::BeliefUpdater<RobotBelief> make_belief_updater(const planning::RoadMap &road_map, | |||
| const double max_sensor_range_m, | |||
| const EkfSlam &ekf, | |||
| const std::vector<int> &present_beacons, | |||
| const std::optional<std::vector<int>> &present_beacons, | |||
There was a problem hiding this comment.
Hmm... I'm not sure I agree with this change. The intention behind this function is that it makes a belief updater given a particular configuration of the world defined by the present beacons list. If we are saying that the list of present beacons is optional, it's not clear what that should mean.
I see that it's currently only used in the unit tests. If you want to represent all beacons being present, you can pass in ekf_slam.estimate().beacon_ids. If you want to represent all beacons being absent, you can pass in {}.
| const GoalScoreFunctionType& goal_score_function, | ||
| const PlanningArgs& planning_args ); | ||
|
|
||
| std::ostream& operator<<(std::ostream& os, const Candidate& candidate); |
There was a problem hiding this comment.
I think this interface looks reasonable. I'm not suggesting that this needs to happen as part of the PR, but in general, I like to separate the generic planning bits from the problem specific bits. Looking at the interface, I think this could be achieved by templating on the belief type. Again, no need to make any changes, just some context for you.
|
|
||
|
|
||
| struct PlanningArgs { | ||
| unsigned int max_iterations; |
There was a problem hiding this comment.
Is there a reason that you're using unsigned int and float instead of int and double? I tend to default to int since it's what the google style guide suggests (https://google.github.io/styleguide/cppguide.html#Integer_Types). We're not bound by it, although the default linter rules are heavily influenced by it.
| .edge_cost = (neighbor_pos - map.point(parent.path_history.back())).norm(), | ||
| }); | ||
| } | ||
| int action_index = rand() % successors.size(); |
There was a problem hiding this comment.
I prefer to use the random bit generator defined in the standard library (std::mt19937) and the associated distributions instead of calling rand() directly.
I would then either pass in a seed or make the random bit generator an InOut<> argument. Debugging is way easier if you can reduce the number of uncontrolled sources of randomness.
Here is an example of taking in a seed and using it to sample an (x, y) position.
robot/planning/probabilistic_road_map.hh
Line 53 in 8bf7340
Here is an example of taking it in as a parameter:
https://en.cppreference.com/w/cpp/numeric/random/mersenne_twister_engine
https://en.cppreference.com/w/cpp/numeric/random
There was a problem hiding this comment.
also const if you don't expect to change the value.
| StepCandidateFunc make_random_step_candidate_function(const planning::RoadMap &map, const planning::BeliefUpdater<RobotBelief> &belief_updater) { | ||
| return [&map, &belief_updater](const Candidate& parent)->Candidate { | ||
| std::vector<planning::Successor<int>> successors; | ||
| for (const auto &[neighbor_idx, neighbor_pos] : map.neighbors(parent.path_history.back())) { |
There was a problem hiding this comment.
Nit: instead of creating the successors and then selecting one, consider picking one first and then only make the one you need.
| const auto &[road_map, ekf_slam, potential] = create_grid_environment(ekf_config, 0.5); | ||
|
|
||
|
|
||
| Candidate candidate = { |
|
|
||
| std::cout << "from initial candidate: " << candidate << std::endl; | ||
| for (const auto &candidate : candidates) { | ||
| std::cout << "\t candidate: " << candidate << std::endl; |
There was a problem hiding this comment.
Is there some property that you expect all candidates to have? Is there some property that you expect at least one candidate to have?
| Candidate candidate = { | ||
| .belief = ekf_slam.estimate().robot_belief(), | ||
| .path_history = {road_map.START_IDX}, | ||
| }; | ||
|
|
||
| RollOutArgs roll_out_args = { | ||
| .num_roll_outs = 1000, | ||
| }; | ||
|
|
||
| constexpr double max_sensor_range_m = 3.0; | ||
|
|
||
| planning::BeliefUpdater<RobotBelief> belief_updater = | ||
| make_belief_updater(road_map, | ||
| max_sensor_range_m, | ||
| ekf_slam, | ||
| std::nullopt, | ||
| TransformType::INFORMATION); | ||
|
|
||
| const StepCandidateFunc step_func = make_random_step_candidate_function(road_map, belief_updater); | ||
| const TerminateRolloutFunc terminate_rollout_func = make_max_steps_or_goal_terminate_func(10, road_map); | ||
| const RolloutFunctionType rollout_function = make_rollout_function(step_func, terminate_rollout_func, roll_out_args); | ||
| auto candidates = rollout_function(candidate); | ||
|
|
||
| const ScoringFunc scoring_func = make_goal_distance_scoring_function(road_map, 10, 20); | ||
|
|
||
| CullingArgs culling_args = { | ||
| .max_num_survivors = 100, | ||
| .entropy_proxy = 0.2, | ||
| .reseed_percentage = 0.1, | ||
| }; |
There was a problem hiding this comment.
This feels like a lot of machinery that isn't strictly necessary to test the culling function. It also means that if you change how candidates are generated, you may need to revisit this test. Consider generating candidates that is independent of the make_rollout_function implementation.
Feel free to disagree if you think I'm missing something.
|
|
||
| std::vector<bool> used(scored_candidates.size(), false); | ||
|
|
||
| CHECK(cullingArgs.entropy_proxy + cullingArgs.reseed_percentage <= 1.0); // can't have more than 100% of the population |
There was a problem hiding this comment.
This check is only a function of the culling arguments. Consider moving it outside of the lambda.
| int num_unused = scored_candidates.size(); | ||
| for (unsigned int i = 0; i < num_random_survivors; i++) { | ||
| int unused_index = rand() % num_unused; | ||
| int index = 0; | ||
| for (unsigned int j = 0; j < scored_candidates.size(); j++) { | ||
| if (!used[j]) { | ||
| if (index == unused_index) { | ||
| index = j; | ||
| break; | ||
| } | ||
| index++; | ||
| } | ||
| } | ||
|
|
||
| used[index] = true; | ||
| survivors.push_back(scored_candidates[index].second); | ||
| num_unused--; | ||
| if (num_unused == 0) { | ||
| break; | ||
| } | ||
| } |
In general, we should prefer to eliminate sources of randomness from test code. For example, we should prefer to test with known inputs, rather than testing with inputs that change on each invocation. If this isn't possible, then we should fix the seed and check for properties that must be true for all executions. We shouldn't for example, fix a seed and then require a specific result if that result is a function of the seed. We should require a specific result if it is meant to be invariant with respect to the seed. The reason for this is even if that even if the seed is fixed, the implementation can change in ways that are still correct, yet we produce different results. Ideally, we wouldn't want our tests misfiring in these situations.
I think defining a contract in the header is a reasonable first step. This can be done in comments in the header or in the type system if possible.
I don't think that duplicated code is inherently bad. One of the more frustrating experiences is making a change to fix a unit test and then realizing that it causes a different test to fail. If you truly believe that all tests require similar inputs, I would first reach for defining a function that constructs those inputs for you (e.g.
There are tons of options for serialization. If you can flatten your type, CSV's are always an option. If there is some structure you'd like to preserve, protobufs are an easy option.
I'm happy to take suggestions :) |
Big PR with the first pass of the sampling based planner. I expect this will be a longer review but wanted to get it into a usable state before field trials.
A couple of initial questions and comments :