search

DFKI-NI / mir_robot

ROS support for the MiR Robots. This is a community project to use the MiR Robots with ROS. It is not affiliated with Mobile Industrial Robots.
BSD 3-Clause "New" or "Revised" License
239 stars 157 forks source link

Did anyone attempt to extend the navigation stack for navigation with a trailer / cart attached (via MiR Hook)? #111

Closed moooeeeep closed 2 years ago

moooeeeep commented 2 years ago

I'm working with a simulated MiR 100 with a hook and a cart attached. I'd like to implement some basic navigation, if possible forwards and backwards with the tractor-trailer system. So I'd like to know if there are any starting points to implement this with an external move_base navigation stack.

What I found in that direction was mainly simple demos (Python or Julia), not at all integrated with ROS. Then I figured the MiR+trailer could fit a bicycle model sufficiently well, s.t. a teb_local_planner with a car-like configuration could do the job. But when I tried to implement it, I'm now stuck with some quirks that I'm not sure if they come from the planners (SBPL and teb_local_planner) or my approach to translate the cmd_vel command for the bicycle model to the cmd_vel command for the diff-drive tractor.

Long story short: I would like to prevent debugging on a X-Y problem for a long time. I'd appreciate any input (or even pointers to existing packages that I might have missed).

mintar commented 2 years ago

I think you're on the right track. A long time ago, I've seen a nice cart pushing demo with the PR2 that was implemented using SBPL. Your problem seems related. They implemented a specialized search space for SBPL (x, y, theta, theta of the cart), which allowed taking the changing robot footprint and the extra controlled joint (the angle between robot and cart) into account. In your problem, the joint between robot and cart can also be actively controlled via a motor, correct?

The ROS wiki site is here, but that seems to be about all that's left of it; the code was on the ROS SVN repo (code.ros.org), which is long dead. Also, the latest supported ROS version was fuerte.

mintar commented 2 years ago

@dmronga: You're also using a MiR hook + cart, correct? Do you use an external move_base stack or only the internal one on the MiR?

moooeeeep commented 2 years ago

Thanks for your input! I don't seem to find a fork/mirror of the old repositories too. Maybe I'll post a question on the ROS discourse/askbot pages, if anyone can dig up a copy somewhere for inspiration.

The joint between robot and cart is actually passive. Luckily it is located at the same axis as the MiR100 diff-drive base.

Actually I'm in touch with @dmronga. We tested the official internal navigation capabilities. They require huge safety margins, such that the approach doesn't work in our environment. It seems they just add enough margin to the footprint that any off-tracking peculiarities are hidden within and they can get away with using a normal planner/controller still. I've seen the official video of reverse parking with a trailer, but our robot would fail planning if it was needed (we enabled reverse parking in the mission). Maybe it's due to the sensor setup though, as the trailer blocks vision of the LIDARs, so maybe it just doesn't want to reverse blindly. So right now we try to get a little bit further with an external move_base stack...

dmronga commented 2 years ago

Hi @mintar , thank for the input. We are working on the same problem/project

moooeeeep commented 2 years ago

I found a bug in my code and now it works sufficiently well in the simulation.

FWIW: I now use the SBPL lattice planner with a footprint fixed to the fixed axis of the trailer and a TEB local planner with a "two_circles" footprint config for a car-like vehicle and then have some custom controller node that converts the motion commands for the imaginary car to the MiR-tractor (calculate required steering angle, rotate base to match that, then allow longitudinal speed as desired). The move base stack is running very slow at times, but I have the node stop when it didn't receive a command for some time, so it doesn't run into things. Probably this could be very well optimized still...