search

UbiquityRobotics / move_basic

A minimal navigation node
BSD 3-Clause "New" or "Revised" License
69 stars 21 forks source link

Using planning frame from goal. #52

Closed jim-v closed 4 years ago

jim-v commented 4 years ago

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.

rohbotics commented 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.

rohbotics commented 4 years ago

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 image

rohbotics commented 4 years ago

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.

rohbotics commented 4 years ago

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.

rohbotics commented 4 years ago

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.