Planner integration#20
Conversation
orduno
commented
Aug 9, 2018
- Created the DijkstraPlanner based on the core algorithms from Navfn ROS1 nav package.
- Updated the SimpleNavigator to use the new DijkstraPlanner
- Created an initial WorldModel node and Costmap mockup to test the new planner.
- Fixed bug with TaskClient.
- Added a few new messages and services related to Costmap. Made a few updates to the Path messages.
| endif() | ||
|
|
||
| find_package(ament_cmake REQUIRED) | ||
| # find_package(rclcpp REQUIRED) |
There was a problem hiding this comment.
Please remove commented-out sections to keep things tidy.
|
|
||
| include_directories(include) | ||
|
|
||
| add_library(costmap STATIC |
There was a problem hiding this comment.
We may want a general utility library that can hold Costmap, etc., instead of proliferating directories for each library.
There was a problem hiding this comment.
Agree, made a note to discuss this on our next meeting.
There was a problem hiding this comment.
As discussed, i moved it to a util library
| #ifndef WORLD_MODEL__COST_VALUES_H_ | ||
| #define WORLD_MODEL__COST_VALUES_H_ | ||
| /** Provides a mapping for often used cost values */ | ||
| namespace costmap |
There was a problem hiding this comment.
Please run ament_cpplint and ament_uncrustify on all code and address issues. It may not be practical yet for old code (Navfn), but should do as much as possible.
| <version>0.1.0</version> | ||
| <description>TODO</description> | ||
| <author email="[email protected]">Oregon Robotics Team</author> | ||
| <author email="[email protected]">Carlos Orduno</author> |
There was a problem hiding this comment.
Indentation is off. Also should discuss (again) how we're handing author and maintainer fields.
There was a problem hiding this comment.
Added to list of discussion topics.
| RCLCPP_INFO(node_->get_logger(), "Costmap::getCostmap"); | ||
|
|
||
| // TODO(orduno): faking out a costmap for now | ||
| (void)specifications; |
There was a problem hiding this comment.
Instead of casting to void, you can also comment out the name ("specifications") in the function interface (line 27). I prefer commenting out the name as it results in fewer lines of code.
|
|
||
| // Some arbitrary numbers | ||
| costmap.info.resolution = 1; // m/cell | ||
| costmap.info.width = 10; // cells |
There was a problem hiding this comment.
Can we change the name of the field from "width" to make it clearer (and not require the comment)? The general principle being to improve the variable names to avoid needing comments to explain them.
There was a problem hiding this comment.
Agree on the principle. In this case, a better name is number_cells_x_direction, same for height. However I took the variable name as a carry over from Occupancy Grid, and as we see on other ROS message definitions, the preference is to use something short. Added to the list of things to discuss.
| Costmap::getTestData(const int width, const int height, vector<uint8_t>& data) | ||
| { | ||
| // TODO(orduno): fixed size for now | ||
| (void)width; |
|
|
||
| // TODO(orduno): instead of hardcoding, define a function | ||
|
|
||
| const uint8_t n = 255; // no information |
There was a problem hiding this comment.
Each of these special values should have a symbol. It could be from an enumeration (with explicit values) or a macro. That way, the code wouldn't need comments and would be clearer.
There was a problem hiding this comment.
After further discussion, it was changed to static const within Costmap class
| #include "costmap/Costmap.hpp" | ||
| #include <tf2/LinearMath/Quaternion.h> | ||
|
|
||
| using std::vector; |
There was a problem hiding this comment.
We should discuss our team conventions on whether we're opening namespaces (std) or not. We should be consistent as a team (and as individuals).
There was a problem hiding this comment.
I'd be in favor of opening with some rules: not on header files, only introducing the name being used. Added to the discussion list.
| std_msgs/Header header | ||
|
|
||
| # MetaData for the map | ||
| CostmapMetaData info |
There was a problem hiding this comment.
Perhaps the field should be called "metadata" instead of "info."
There was a problem hiding this comment.
This is related to a previous comment about ROS message definitions and carryover from ROS1. Added to the discussion list.
| CostmapMetaData info | ||
|
|
||
| # The cost data, in row-major order, starting with (0,0). | ||
| uint8[] data No newline at end of file |
There was a problem hiding this comment.
Seems like there needs to be newline here.
|
|
||
| # The origin of the costmap [m, m, rad]. | ||
| # This is the real-world pose of the cell (0,0) in the map. | ||
| geometry_msgs/Pose origin No newline at end of file |
| geometry_msgs/PoseWithCovariance goal | ||
|
|
||
| # The start pose for the plan | ||
| geometry_msgs/PoseStamped start |
There was a problem hiding this comment.
Why do we need a stamped version of the pose? We have the timestamp in the header already. Same for the goal.
There was a problem hiding this comment.
Currently not used. In future we might want to add time constraints to start and reach goal.
There was a problem hiding this comment.
Fixed, Mike is right, posestamp is not needed, and we don't want to overload the term. If we need to add time constraints we'll have to add a new field.
| # Specifications for the requested costmap | ||
| nav2_msgs/Costmap map | ||
| --- | ||
| nav2_msgs/Costmap map No newline at end of file |
There was a problem hiding this comment.
Fixed. Also realized that the request should be nav2_msgs/CostmapMetaData -- no need for the data field.
| #include <memory> | ||
| #include <exception> | ||
| #include <chrono> | ||
| #include <iostream> |
There was a problem hiding this comment.
I don't see that iostream is required. Still using it? Should remove any #includes that aren't required.
| }; | ||
|
|
||
| #endif // PLANNING__ASTARPLANNER_HPP_ | ||
| #endif // PLANNING__ASTARPLANNER_HPP_ No newline at end of file |
| bool computePotential(const geometry_msgs::msg::Point& world_point); | ||
|
|
||
| // Compute a plan to a goal from a potential - must call computePotential first | ||
| bool getPlanFromPotential(const geometry_msgs::msg::PoseStamped& goal, std::vector<geometry_msgs::msg::PoseStamped>& plan); |
There was a problem hiding this comment.
Please run (all files) through ament_cpplint and ament_uncrustify. Several lines are too long.
| * @class NavFn | ||
| * @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down. | ||
| */ | ||
| class NavFn |
There was a problem hiding this comment.
We should refactor this class. It's not very clear as-is. We can probably extract out the various ideas into different (reusable) classes. Doesn't have to be done immediately though.
There was a problem hiding this comment.
Agreed. Once we have a sufficient level of functionality and have done tests across the components, we can proceed to refactor / replace some of the core algos.
| @@ -0,0 +1,32 @@ | |||
| // Copyright (c) 2018 Intel Corporation | |||
There was a problem hiding this comment.
This file is obsolete and can be removed. It shouldn't be there in the ros2_devel branch.
| <version>0.1.0</version> | ||
| <description>TODO</description> | ||
| <author email="[email protected]">Oregon Robotics Team</author> | ||
| <author email="[email protected]">Carlos Orduno</author> |
|
|
||
| return TaskStatus::SUCCEEDED; | ||
| } | ||
| } No newline at end of file |
There was a problem hiding this comment.
Added, not sure how i removed those...
| } | ||
| catch (...) { | ||
| RCLCPP_ERROR(this->get_logger(), "DijkstraPlanner::makePlan: failed to obtain costmap from server"); | ||
| throw; |
There was a problem hiding this comment.
I recommend throwing a specific kind of exception. See the standard exception types.
There was a problem hiding this comment.
The intention here is to re-throw the current exception.
| planner_ = std::make_shared<NavFn>(costmap_.info.width, costmap_.info.height); | ||
|
|
||
| // Plan publisher for visualization purposes | ||
| plan_publisher_ = this->create_publisher<nav2_msgs::msg::Path>("plan", 1); |
There was a problem hiding this comment.
Did you intend to include the second parameter (qos_history_depth)? Or, is this a left-over from porting?
There was a problem hiding this comment.
left-over from porting
| @@ -0,0 +1,17 @@ | |||
| #pragma once | |||
There was a problem hiding this comment.
Should port the code to not require this header (and remove it).
There was a problem hiding this comment.
Yes, also needs to be moved to a util folder
| { | ||
| costmap_ = std::make_unique<Costmap>(this); | ||
|
|
||
| // Create a (lambda) callback function for when a costmap service is received. |
There was a problem hiding this comment.
Why a lambda function here versus a class method? Is there an advantage? In my opinion, it would be clearer to have a separate method and use it here.
There was a problem hiding this comment.
I'm having problems defining a ROS2 service that takes in a class method for the callback.
| rclcpp::shutdown(); | ||
|
|
||
| return 0; | ||
| } No newline at end of file |
| costmap | ||
| ARCHIVE DESTINATION lib | ||
| LIBRARY DESTINATION lib) | ||
|
|
There was a problem hiding this comment.
You should add these lines here so other packages can find this one.
ament_export_include_directories(include)
ament_export_libraries(costmap)
| #define COSTMAP__COSTVALUES_H_ | ||
|
|
||
| /** Provides a mapping for often used cost values */ | ||
| enum class CostValue: uint8_t |
There was a problem hiding this comment.
I'm not a big fan of enums to represent constants due to various portability issues in the past. I'd rather just see these as static consts defined in the Costmap class.
| } | ||
|
|
||
| void | ||
| Costmap::getCostmap(const nav2_msgs::msg::CostmapMetaData & /*specifications*/, |
There was a problem hiding this comment.
The coding standard says you should break after the parenthesis and put the first parameter on the next line.
Also, return type is typically on the same line in the coding standard, though I don't know if it is explicitly required.
eg.
void Costmap::getCostmap(
const nav2_msgs::msg::CostmapMetaData & spec, nav2_msgs::msg::Costmap & costmap)
|
|
||
| void | ||
| Costmap::getCostmap(const nav2_msgs::msg::CostmapMetaData & /*specifications*/, | ||
| nav2_msgs::msg::Costmap & costmap) |
There was a problem hiding this comment.
Rather than use an output parameter here, I'd just return the costmap. eg.
nav2_msgs::msg::Costmap Costmap::getCostmap(const nav2_msgs::msg::CostmapMetaData & /*specifications*/)
{
nav2_msgs::msg::Costmap costmap;
...
return costmap;
}
The return value elision optimization will remove any overhead.
| } | ||
|
|
||
| void | ||
| Costmap::getTestData(const int /*width*/, const int /*height*/, vector<uint8_t> & data) |
There was a problem hiding this comment.
Just leave out the parameters you aren't using. You can always add them when you need them.
| n,o,o,o,o,o,o,o,o,n, //8 | ||
| n,n,n,n,n,n,n,n,n,n}; //9 | ||
|
|
||
| data = costmapMaze2; |
There was a problem hiding this comment.
If you made these a vector of vectors, you could select the maze you want by index. That way you could run all mazes in one test set instead of having to recompile every time you want to run a different maze.
There was a problem hiding this comment.
Will modify once I integrate the PlannerTestNode, right now is just a place holder for the actual costmap.
|
|
||
| // The client can wait for a result with a timeout | ||
| TaskStatus waitForResult(typename ResultMsg::SharedPtr result, unsigned int milliseconds) | ||
| TaskStatus waitForResult(typename ResultMsg::SharedPtr& result, unsigned int milliseconds) |
There was a problem hiding this comment.
Just to elaborate a bit, passing the shared_ptr by value is the safe option, but it comes at a cost since the implementation has to acquire a lock to increment the reference count.
Passing by reference is a performance optimization when you know that the callee won't hang on to the pointer. Passing by const reference makes passing by reference a bit safer.
None of this applies in this particular case though, since it is an output parameter.
| TaskStatus waitForResult(typename ResultMsg::SharedPtr result, unsigned int milliseconds) | ||
| TaskStatus waitForResult(typename ResultMsg::SharedPtr& result, unsigned int milliseconds) | ||
| { | ||
| std::mutex m; |
There was a problem hiding this comment.
This looks suspicious. Nobody else could ever acquire this mutex since it is a local variable.
There was a problem hiding this comment.
@mjeronimo Can you look at @crdelsey 's question
| void executeAsync(const typename CommandMsg::SharedPtr msg) | ||
| { | ||
| taskSucceeded_ = false; | ||
| receivedNewMsg_ = false; |
There was a problem hiding this comment.
This should be called something like receivedResult_ instead of receivedNewMsg_
There was a problem hiding this comment.
changed it to receivedResult_
| if (taskSucceeded_) { | ||
| // TODO: possible race condition between receiving status message and actual data | ||
| // for now implemented a method using a flag, but might want to review further | ||
| if (taskSucceeded_ && receivedNewMsg_) { |
There was a problem hiding this comment.
@mjeronimo How is taskSucceeded supposed to work? It seems like if we get a status message, then it has succeeded. It can never fail, just timeout. Or is this just unimplimented?
| @@ -115,6 +120,8 @@ class TaskClient | |||
|
|
|||
| // A convenience variable for whether the task succeeded or not | |||
| bool taskSucceeded_; | |||
There was a problem hiding this comment.
@mjeronimo Can you confirm @crdelsey 's comment?
…and costmap service. Created initial world model class
…changes to support these.
70203ff to
b2b8e9d
Compare
|
…ath, clear local costmap before narrow driving (#20)
…_nav2_main AUTO-795 Backport nav2 main features
* added bt navigator parameters * - set use_sim_time default to false - expanded on the example - removed parameter sub-title * added bt nav page * updating bt navigator page title Co-authored-by: Steve Macenski <[email protected]> * add spacing between and the objects in plugin_lib_names list * minor formatting Co-authored-by: Steve Macenski <[email protected]>
* Setup guide for sensors and costmap Squashed commit of the following: commit 61a3010d4eb0a2fc3cb50dd2b7244849fca4b4d2 Author: Josen Daniel Obordo De Leon <[email protected]> Date: Wed Jul 28 11:53:32 2021 +0800 Flow improvements to costmap section commit 2b179a7e3783f2982aba13ee082218bf3333285c Author: Areeya Ultra Rubenecia <[email protected]> Date: Thu Jul 22 16:55:12 2021 +0800 changes to build, run and verification of costmap section and minor writing edits commit 12a52c7da92185fa453596ba2f8672f14525af9d Author: Areeya Ultra Rubenecia <[email protected]> Date: Mon Jul 19 19:27:00 2021 +0800 Made changes to Gazebo demo and added Build, Run and Verification subsection commit ba9691d71e31ef077e9f44fdbe8426665073ba72 Merge: 8f47deb 5a76ce3 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Mon Jul 12 16:24:35 2021 +0800 Merge pull request ros-navigation#21 from cc-marquez/setup_sensors_editing2 Writing and editing improvements for sensor tutorials commit 5a76ce348084bb936c7c53f5abd6a172d96d537f Author: Josen Daniel Obordo De Leon <[email protected]> Date: Mon Jul 12 13:48:56 2021 +0800 Writing and editing improvements commit 8f47debb771757e7c6308f76bd40132a5f5c6b1f Author: Areeya Ultra Rubenecia <[email protected]> Date: Wed Jul 7 16:10:02 2021 +0800 Edited contents and figures according to comments commit 69769a3d4eff36f62597912a5ca62c37b35c69be Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:57:58 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 15362c9a13bf789465e4fafa5564bf4d50df04d9 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:54:09 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 5fcb9daa329cc8b3a31f4c8eb697d154a7b0b7fa Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:53:35 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit beed1b55227ebb794a54e7077466c1092668063d Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:52:49 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 4a53d583b3bb0891fe305449b243d869472641c2 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:52:22 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit e61429705984876003e5ebe3e3115d7fdf476d51 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:28:04 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 6b81d695acbddce28aac47afd92c96b8a3b6241f Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:27:45 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit d113ece97f834897f4631e2841026ba4d0c4521e Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:27:21 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 73b94841ba95c6ab2d04b7b73ca9e80f1685cb3a Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:25:56 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 2eb2a5f73e5a842ff540bc29a3c76f896219fc7d Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:23:40 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 394d5eab4c11487a06118ae202850768d734a4f5 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:23:22 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit d8fe8ab94765e2746a7aa0acfe8a9b7ce28d20ac Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:22:57 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 668fa1f77706d81b2decd38895472b624603b865 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:22:16 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit a2dbce7df6e4f981209b6cf0840b603cb00bcd2d Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Fri Jul 2 13:22:02 2021 +0800 Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Steven Macenski <[email protected]> commit 8863af547d4bfd2d454aba311b84a9ba8252796b Merge: 6288058 ef35c4b Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Thu Jul 1 10:52:56 2021 +0800 Merge pull request ros-navigation#20 from cc-marquez/f-estoperez-patch-9 Proofreading commit ef35c4b0facf51f43f0fc21ae6c7d34f7111f20d Author: Fiona Estoperez/R&D Strategy Group /SRPH/Professional/Samsung Electronics <[email protected]> Date: Tue Jun 29 21:51:26 2021 +0800 Proofreading Initial proofreading commit 62880580f5e766503f3bd6e4c32634a701b2712c Author: Areeya Ultra Rubenecia <[email protected]> Date: Thu Jun 24 17:43:00 2021 +0800 Completed initial draft commit 22bddd978244c5e2b6045ff5ef3c9c4d9518130a Merge: 3d15a3f 4cde509 Author: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Date: Wed Jun 23 18:43:23 2021 +0800 Merge pull request ros-navigation#19 from cc-marquez/setup_sensors-review1 Editing and revisions of sensor tutorials commit 4cde509c6d25c4b6ae36ecd8e735374afcb5d90d Author: Josen Daniel Obordo De Leon <[email protected]> Date: Fri Jun 18 16:46:03 2021 +0800 Editing and revisions of sensor tutorials commit 3d15a3f420c4f92294d2228d060c371eb158c730 Author: Areeya Ultra Rubenecia <[email protected]> Date: Wed Jun 9 16:48:08 2021 +0800 Add initial draft of Setting Up Sensors Tutorial * Change sample rviz pointcloud2 image * Changes to costmap configuration snippet * Apply suggestions from code review Co-authored-by: Steve Macenski <[email protected]> * Changes according to code review * Improvements to costmap section * Update setup_guides/sensors/setup_sensors.rst Co-authored-by: Areeya Ultra Rubenecia/SW Optimization Group /SRPH/Engineer/Samsung Electronics <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
…on#20) Signed-off-by: Karsten Knese <[email protected]>