[Smac Planner] Enable goal orientation non-specificity #4019
Conversation
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
Want to populate the PR template and add a name to the PR? This is opened without any context or statement about the state of this work and if there are any questions (or this is a 'done' implementation) It would be nice to have some context 😉 |
stevedanomodolor
changed the title
There was a problem hiding this comment.
Generally this works! Now, you need to modify isGoal() [1] to use those goals to check if any are the item. That should be enough to start actually testing this working!
From there, the next major step is to expand the definitions for AnalyticExpansion to take in the goals and try to expand to each, each iteration. If any are valid, take the shortest.
Please make sure to lint using ament_cpplint and ament_uncrustify. It makes it much easier for me to review and keeps the styling in line with the rest of the project. A little TLC early can make your life alot easier later :-)
| @@ -337,6 +337,11 @@ float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) | |||
| return bin_idx * bin_size; | |||
| } | |||
|
|
|||
| unsigned int getNumOfBins() | |||
| { | |||
| return num_angle_quantization; | |||
There was a problem hiding this comment.
You could just use num_angle_quantization for the one instance this method is used? That seems in style with the rest of it. The table is treated like a global struct to grab info (for better or worse)
| } | ||
| } | ||
|
|
||
| // TODO(@stevedanomodolor) find a better way to do this |
There was a problem hiding this comment.
What's the concern? I think this works fine, anything more you want to expand on here?
|
|
||
| nav2_util::declare_parameter_if_not_declared( | ||
| node, name + ".goal_heading", rclcpp::ParameterValue("DEFAULT")); | ||
| node->get_parameter(name + ".goal_heading", _goal_heading_type); |
There was a problem hiding this comment.
You don't need to store the string type if you're storing the enum class
| // _goals_coordinates.clear(); | ||
| std::vector<Coordinates> goal_coordinates; | ||
| unsigned int number_of_bins = NodeT::motion_table.getNumOfBins(); | ||
| if(goal_heading == GoalHeading::DEFAULT){ |
There was a problem hiding this comment.
You could do this with a switch statement which would be more compact
| { | ||
| _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); | ||
| _goals.clear(); | ||
| // _goals_coordinates.clear(); |
| static_cast<float>(mx), | ||
| static_cast<float>(my), | ||
| static_cast<float>(dim_3)); | ||
| auto has_goals_changed = [](const std::vector<Coordinates> & current_goal_coordinates,const std::vector<Coordinates> & previous_goal_coordinates) -> bool |
There was a problem hiding this comment.
I don't understand this lambda. Why not simply do A != B? Vectors can be compared. If the Coordinates class doesn't implement the operator=, that seems like an easier solution :-) I'm also not 100% sure why you'er comparing goal_coordinates[i] != goal_coordinates[i+1]and never seem to usecurrent_orprevious_ones. Overall, I think this can be removed with a simpleif A != B` statement inline to the code without the lambda
|
Thanks for the review, but it is not ready yet. I have made some changes locally since I made this push. I will close it for now, untill it is fully ready. |
|
OK! |
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: