Feature/dyn obstacle utils #34
Conversation
chiragmakwana0296
force-pushed
the
feature/dyn_obstacle_utils
branch
from
d802c23 to
6a788f2
Compare
Signed-off-by: Chiragkumar Makwana <[email protected]>
Signed-off-by: Chiragkumar Makwana <[email protected]>
Signed-off-by: Chiragkumar Makwana <[email protected]>
Signed-off-by: Chiragkumar Makwana <[email protected]>
Signed-off-by: Chiragkumar Makwana <[email protected]>
… type Signed-off-by: Chiragkumar Makwana <[email protected]>
Signed-off-by: Chiragkumar Makwana <[email protected]>
chiragmakwana0296
force-pushed
the
feature/dyn_obstacle_utils
branch
from
21e45e4 to
fc61c9c
Compare
There was a problem hiding this comment.
In general, it might be good to stop and start with a design document and developing a set of requirements before going into an implementation. I think many things would become clear and help make progress faster.
For example, a requirement is that we're taking in a set of tracked obstacles at some unknown rate from the tracker. Separately, we want to be able to project those tracks as we see fit. As a result, we very well may need to do some TF-style interpolation to get accurate estimates of pose -- as the request time of the algorithm that wants to get some poses, the dt in the future of the request time, and the last message triggering are all different times. We also probably need to handle the case where we need to store a small buffer of tracks in case times that we're given are actually older than the latest message.
Similarly with the motion models classes: thinking about how we need to use it in an application will help make sure we design good, robust interfaces for customization.
Also, if we want to project just poses or Objects which contain velocity and covariance information. If we're using some like of AI-based solution that is predicting models (not just constant velocity), then it may not just be that velocity, covariance, etc are the same after predicting into the future. We should also definitely consider propagating the uncertainty of prediction into the future -- which probably means the timestamp of the tracked-message itself is important to know how far dt is in the future to scale the confidence of the estimate of the pose. This will help in something like the dynamic obstacle layer where future predictions should be given less cost-weighting due to increasing uncertainty. The prediction containing an accurate confidence would be very reasonable and also simplify later algorithms that need to use it.
So, before going much further in code, I think a google doc on requirements, design intent, etc is good to discuss and agree upon first. So far, this looks overly simplistic - but we have to start somewhere! We should have a clear understanding of how these utils / classes are to be:
- used in a functional application and its ergonomics,
- what inputs and outputs they expect and how they might be used or populated,
- how they should handle the synchronization of data,
- Separation of responsibility of the various objects,
- What kinds of applications it'll be used in to design good interfaces that enable users,
- How we plan to design the solution to enable this all
| namespace nav2_dynamic_motion_model | ||
| { | ||
|
|
||
| class MotionModelInterface |
There was a problem hiding this comment.
Like we talked about, I think we should make sure to include initialize() methods so that there's an obvious place to do initialization activities like creating subscriptions, getting parameters, etc.
Think a bit about what we might want the arguments for it to be. It might be good to look at other Nav2 plugin interfaces for example. (rclcpp::node, std::string name, etc)
| #include <nav2_dynamic_msgs/msg/obstacle.hpp> | ||
| #include <nav2_dynamic_msgs/msg/obstacle_array.hpp> | ||
|
|
||
| namespace nav2_dynamic_motion_model |
There was a problem hiding this comment.
We probably want a set of unique exceptions for various kinds of errors. TF, cannot project that footprint to that time, extrapolating too far into the future/past, etc. That would be good for applications using these to get errors and handle issues
| <package format="3"> | ||
| <name>nav2_dynamic_motion_model</name> | ||
| <version>0.0.0</version> | ||
| <description>TODO: Package description</description> |
There was a problem hiding this comment.
Consider what a package should be named if we want it to include the motion models and the util that uses them with track messages for predictions in one package.
| virtual ~MotionModelInterface() = default; | ||
|
|
||
| virtual geometry_msgs::msg::Pose predictObstaclePose( | ||
| const nav2_dynamic_msgs::msg::Obstacle &obstacle, |
There was a problem hiding this comment.
Is the assumption that whatever is calling the predict methods on these objects is already going to find the dt w.r.t. the last obstacle message instance timestamp and the request time?
I don't think it'll be so easy, given that the messages come into the utility function at some arbitrary rate from the obstacle detector/tracking pipeline. We may need to take messages from some non-fixed time ago and the current time to find thees values.
It would also be good to consider the covariance for the output so we have some confidence in the output pose (i.e. geometry_msgs::msg::PoseStampedWithCovariance
|
|
||
| virtual ~MotionModelInterface() = default; | ||
|
|
||
| virtual geometry_msgs::msg::Pose predictObstaclePose( |
There was a problem hiding this comment.
Would we want a pose, or an obstacle message containing updated pose, velocity, propagated covariance information, etc?
Yeah makes sense, do we have sample design doc of nav2, that can be used as reference to re-consider all the aspects already covered in nav2. |
TODO:
result