Closed jim-v closed 4 years ago
IIRC the reason we had this separation of planning and movement frames is because map would jump around wildly, and we wanted move_basic to be stupid simple and not have to deal with that.
I do like how this eliminates the need for a separate node, but I think a odom->fiducial publisher will help a lot for debuggability.
I will do a more proper review later today, but these are my initial thoughts.
I just did some basic testing on this, and it has very weird behavior when I send it a goal relative to odom.
In simulation, with the robot at the origin I sent the goal
header:
seq: 0
stamp:
secs: 25
nsecs: 475000000
frame_id: "odom"
pose:
position:
x: 1.01004266739
y: -0.00311359763145
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
And the robot ended up here
Looks like this was my screw up in testing, I had a very stupid map frame that may have been the root of my issues. I tested again without fiducial_slam running (instead just a node that publishes the camera->fiducial into TF) and everything seems to work.
Since part of the idea here is to support map-less operation, it might make sense to support the TF publishing directly in aruco_detect, assuming we are sticking with the camera->fiducial method of arranging the TF tree.
Also I only tested in sim, but I also need to test if this plan works at all in reality. The fiducial poses might be far to jumpy for it work without some kind of filtering or damping system.
Hey @jim-v,
Sorry about the delay in testing here. I just tested with a floor based fiducial on my robot and this change seems to work reasonably well.
Merging.
If the planning frame is set to "goal", then the frame from the goal is used for planning. This allows goals to be specified in arbitraty frames, such as 'fid104', meaning relative to the current observation of that fiducial.
During movement, we attempt to update the goal in the driving frame based on the tf from the planning frame to the driving frame, if it exists. The rationale behind this is that the observations will become more accurate as we get closer, in the case above.