support external marker detection #12
Conversation
mikeferguson
commented
Feb 7, 2024
mikeferguson
commented
- subscribe to marker topic
- add parameters for marker->dock transformation
- publish debug topics
|
@SteveMacenski - update on current state:
|
|
This commit dc8ef8a has the launch files I'm using |
| node, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); | ||
| nav2_util::declare_parameter_if_not_declared( | ||
| node, name + ".external_detection_timeout", rclcpp::ParameterValue(0.5)); | ||
| nav2_util::declare_parameter_if_not_declared( |
There was a problem hiding this comment.
Might be easier to handle this as a parameter array of doubles with XYZRPY
| const double yaw = tf2::getYaw(dock_pose_.pose.orientation); | ||
| dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ - sin(yaw) * external_detection_translation_y_; | ||
| dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ + cos(yaw) * external_detection_translation_y_; | ||
| dock_pose_.pose.position.z = 0.0; |
There was a problem hiding this comment.
I'm not asking you necessarily to implement, but would right here be the spot you'd comment if we're going to take multiple successive dock detections to smooth them by averaging? Construct the dock pose (then in a new object), add to to our buffer, then average to return dock_pose_?
Do you think in your experience this is necessary / worthwhile? Its not alot of code, but also KISS
There was a problem hiding this comment.
Yes - this is where we would want to filter (more specifically, we'd want to construct a new pose so we don't overwrite "dock_pose_" and then filter the new pose into dock_pose, maybe with some sort of IIR).
I'm actually glad you brought this up though - because it got me thinking about how we would have to carefully "filter the yaw" - and now I realize that I never actually forced the dock_pose_ orientation into 2D here (which I think we should do - so that the offsets are always planar).
I'll definitely update the yaw tomorrow - and probably add a really basic IIR filter.
There was a problem hiding this comment.
OK. Make sure the filter is a separate object so that it can be reused by other dock implementations! I was thinking just a rolling averager of the last N samples, but you have the implementer's choice; I think there are no wrong choices for how basic this is
If we force it to be 2D, we could also remove the RP in RPY for offsets
There was a problem hiding this comment.
- When I say "really basic IIR" - I literally mean 1 line equation - really doesn't merit a whole module
- We still need the RPY offsets - they are what actually correct our orientation (and after that, we want to remove any "noise" by locking to 2D). I'll probably end up updating the default RPY so that it works correctly for the typical case where AR marker has Z pointed at you and you want to get to X forward (basically what I have in my testing launch file now)
* subscribe to marker topic * add parameters for marker->dock transformation * publish debug topics
mikeferguson
force-pushed
the
on_robot
branch
from
8cb6a38 to
651ca00
Compare
|
I've added the "force into 2d" and "filter the pose" to #10 - I'm going to merge this since it's in a good place and I can continue to build off it with smaller PRs. |
|
Also - track marker node is merged to rolling - ros-perception/image_pipeline#930 - still need to back port to iron and humble (and waiting for some unknown amount of time until the doc job runs and the docs for the node will appear on docs.ros.org) |
