We need mobile base controllers

[adolfo-rt]

All,

To make the ros_controll(ers) project appealing to a larger user base, we need to offer functionality needed by that user base, and controllers are a big part of that. Currently, the list of available general-purpose controllers is still quite limited, and we have a big zero on mobile base / vehicle controllers.

I've convinced a colleague to make a diff-drive controller that should land sometime soon, but I wonder about the interest and generality of other implementations.

Interest

Does it make sense to have mobile base controllers inside the (possibly high-frequency) controller_manager loop, or is it enough to control individual wheels with low-level single-joint controllers?. Many navigation scenarios have relatively slow dynamics, but I can imagine many scenarios (self-driving cars!) having requirements for faster dynamics.

Further, having a single ros_control plugin that takes in higher level navigation commands results in simpler deployments than single_joint controllers + an additional node for translating navigation commands to joint commands.

Generality

Implementations like the diff-drive generalize well to multiple robot platforms. Does the same apply to an Ackermann-type controller, are they easily configurable (@jim-rothrock, @jack-oquin)?. How about omnidirectional bases?, these seem more complex to generalize, is it the case?. Are there other controller types that would be of general interest?.

[jack-oquin]

My personal experience with Ackermann navigation is limited to a single no-longer-operational vehicle.

From reviews and discussions conducted by the ROS Ackermann Group, I believe the ackermann_msgs definitions are sufficiently general for any four-wheeled vehicle steering only with the front wheels. Some three-wheel vehicles would probably work, too.

I am encouraged to see the recent announcement of wunderkammer-laboratory/ackermann_vehicle, a new Gazebo-based simulation for Ackermann drive vehicles.

Integrating Ackermann steering into the standard ros-planning/navigation packages appears challenging. I doubt that planning in the configuration space of an Ackermann vehicle fits neatly into their current implementation.

[jack-oquin]

Is there a difference between omnidirectional and holonomic?

I think ROS navigation already handles that for PR2 support.

[jim-rothrock]

The main difference I have found between omnidirectional and holonomic is that holonomic has a more precise definition (http://en.wikipedia.org/wiki/Holonomic_%28robotics%29). One might consider a differential drive base or a base with two sets of orthogonal wheels to be omnidirectional, because the base can travel in any direction, if you don't consider orientation to be part of the definition of omnidirectional.

This paper (http://ftp.mi.fu-berlin.de/Rojas/omniwheel/00509272.pdf) states that omnidirectional bases with four steerable wheels are not holonomic. The author might be using the strict definition of "holonomic," in which case the base must have exactly three actuators for three degrees of freedom. A base with eight actuators would be redundant instead of holonomic (http://www.robotplatform.com/knowledge/Classification_of_Robots/Holonomic_and_Non-Holonomic_drive.html).

ROS navigation handles holonomic and non-holonomic bases. A holonomic base must be able to control its velocities along x and y separately from its angular velocity around z. A non-holonomic base must be able to control is angular velocity around z, but (this is my understanding, I haven't tried it) needs to control its velocity only along x. These coordinates are in the base_link frame.

[jim-rothrock]

Further, having a single ros_control plugin that takes in higher level navigation commands results in simpler deployments than single_joint controllers + an additional node for translating navigation commands to joint commands.

"single joint controllers + translation node" is how I'm controlling the base of my Cerberus robot. The node (base_ctrlr.py) subscribes to cmd_vel (a Twist message) and publishes to three steering and three axle command (Float64) topics. The system would be cleaner if the node were turned into a controller, but I have the following reservations:

1) base_ctrlr does a moderate bit of processing in its main loop. e.g. computing the steering angles to keep three wheel axles pointing at the center of the turning circle, computing wheel velocities depending on how far the wheel is from the center and how far its steering differs from the desired angle, etc. This kind of processing is neither necessary nor desirable to be performed at a 1 KHz rate. I run the main base_ctrlr loop at 30 Hz, and even that may be overkill. It suppose this problem can be solved by passing an update_frequency parameter to a ros_control version of base_ctrlr. The controller would update the joint PID controllers every time update() is called, but would check a last_updatetime variable and update the steering angles and wheel velocities at a lower rate.

2) By publishing the joint angles to Float64 command topics, base_ctrlr.py works with gazebo_ros_control, dynamixel_motor, and my own roboteq_ctrlr node. Making base_ctrlr into a ros_control controller would require dynamixel_motor to be ros_control compatible, which I don't think it is. I know roboteq_ctrlr isn't.

Implementations like the diff-drive generalize well to multiple robot platforms. Does the same apply to an Ackermann-type controller, are they easily configurable (@jim-rothrock, @jack-oquin)?. How about omnidirectional bases?, these seem more complex to generalize, is it the case?.

An Ackermann controller would generalize well to multiple platforms. It would basically be like my ackermann_controller.py node, but less Gazebo-centric. By default, such a controller would receive commands from ackermann_cmd. I don't know how popular such a controller would be, however, since ROS navigation publishes Twist messages only, and isn't set up for bases that can't turn in place.

Most omnidirectional bases could be covered by three controllers: steered-wheels (two or more individually-steered wheels), three-omniwheels, and four-Mecanum-wheels. By default, these controllers would receive Twist messages from cmd_vel.

[adolfo-rt]

Thanks for all the feedback. Some further comments below:

@jim-rothrock:

This kind of processing is neither necessary nor desirable to be performed at a 1 KHz rate. I run the main base_ctrlr loop at 30 Hz, and even that may be overkill.

The controller_manager does not have to run at 1kHz, it runs at whatever frequency you tell it to. This also applies to the Gazebo simulation. Mobile-base only robots (eg. a turtlebot) should be able to work with a Linux-scheduled low frequency controller_manager.

Making base_ctrlr into a ros_control controller would require dynamixel_motor to be ros_control compatible, which I don't think it is. I know roboteq_ctrlr isn't.

I'm not familiar with dynamixel_motor, but a proper separation of concerns would make it ros_control-friendly: The functionality for extracting raw data should be decoupled from the one that binds it to a ROS interface. A ros_control enabled dynamixel robot would only use the former as a link-time dependency. Of course, these overheads need to be well justified to invest resources in them. Pros like robot-independent controllers (robot == hardware platform, simulation framework) should outweight the development effort required to make it happen. This starts to make sense when you have to maintain multiple platforms with different hardware bindings.

@jack-oquin:

Integrating Ackermann steering into the standard ros-planning/navigation packages appears challenging. I doubt that planning in the configuration space of an Ackermann vehicle fits neatly into their current implementation.

This is not so much about integration with the ROS navigation stack, but about providing controllers that allow you to use higher-level navigation functionality out of the box (like the ROS navigation stack, or an Ackermann-specific one, or...). It's about exposing Twist or AckermannDrive messages instead of low-level joint commands.

A non-navigation example we're currently working on: If you have a ros_control enabled arm, you can now instantiate a JointTrajectoryController out of the box, which exposes the action interface expected by MoveIt, greatly reducing the barrier for having collision-free motion planning and autonomous grasping in your robot. The idea is to try to deliver this experience all across the board.

[jim-rothrock]

I'm begun writing a base controller, and I'm confused. Here is how my controller is supposed to work:

Input:

• Subscribes to cmd_vel topic (Twist)

Output:

• Controls the positions of three steering joints. They might be position-controlled (e.g. a Roboteq motor controller, which does its own PID position control) or effort-controlled (e.g. gazebo_ros_control).
• Controls the velocities of three wheels. They might be velocity-controlled (e.g. a Roboteq motor controller, which does its own PID velocity control) or effort-controlled (e.g. gazebo_ros_control).

The Gazebo scenario is simple, because all the joints are effort-controlled. When using real hardware, however, the steering joints will be position-controlled and the axle joints will be velocity-controlled. I've been looking at the joint_trajectory_controller source code as an example, but I don't see how the class inheritance system allows a controller to control position-controlled and velocity-controlled joints simultaneously. In the general case, there might also be effort-controlled joints, e.g. the front steering joint might use a motor driver instead a motor controller, in which case its PID loop would have to run in software.

[jim-rothrock]

Is there a public example of interfacing a controller manager with hardware? I've considered following the model used in dynamixel_motor (http://wiki.ros.org/dynamixel_motor), in which there is a dynamixel_mgr node that acts as the controller manager (Note: dynamixel_motor does not use ros_control). In a ros_control version, the RobotHW-derived class object that is passed to the controller manager would be a chunk of code that associates joint names with combinations of serial ports and motor numbers. The node I am considering writing would do essentially the same thing, except with Roboteq motor controllers. A downside of this approach is that it does not support controllers that need to use both Dynamixel and Roboteq controlled joints, because those joints are controlled by separate controller managers.

Is the intent of ros_control that there are several controller managers, each controlling a certain type of hardware? Or is there supposed to be just one controller manager, controlling all the hardware?

[jim-rothrock]

I'm getting my steered_wheel_base_controller ready to be released, but there is a problem I'm trying to fix, and I'd like some opinions about it. When the controller is initialized (i.e. in initRequest()), I need to use tf to find the positions of the wheels in the base frame. However, when the controller is running within Gazebo and /use_sim_time is true (both always true for me right now), this hangs. The problem is that tf requires a time server, but Gazebo is the time server, but it is not publishing time values because it is still initializing the gazbo_ros_control plug-in. My current work-around is to check in update() whether tf-related initialization of the controller has taken place. If not, update() returns immediately. Otherwise, tf-related initialization is attempted, and a flag is set if it succeeded. The problem with this approach is that it possibly (Probably? Definitely?) will break the real-time constraint during the update() call during which tf is used.

I've been thinking of setting up a timer callback in initRequest(), called once per second. The callback will attempt tf-related initialization, calling Timer::stop() and setting a flag when initialization succeeds. The callback will be called by the non-realtime thread. Can anyone propose an alternative?

[wmeeusse]

The initRequest gets triggered by the spawner loading a controller. So this should only happen after the controller manager got started and gazebo finished loading the controller manager plugin. It might take a while for the first tf messages to arrive, but you should be able to just block in the initRequest call until the right tf messages arrive. So if your controller initialization is hanging, this points to gazebo using only using one single ros spin thread. You can get around this problem by giving your tf listener its own thread to receive tf messages.

However, I'm curious to hear why you need to receive tf messages in your controller. Do you need (i) to look up a fixed transform between two frames? In that case, you can directly parse the urdf in your initRequest call to get the fixed transform. Or do you need (ii) to get the state between two frames that have a moving joint between them? In that case... you have a bunch of other problems, but before going into that it might be good to hear a bit more about your use case.

[jim-rothrock]

However, I'm curious to hear why you need to receive tf messages in your controller. Do you need (i) to look up fixed transform between two frames? In that case, you can directly parse the urdf in your initRequest call to get the fixed transform.

I need to get the fixed transform from the base frame (specified by a parameter) to the steering axis of each wheel. These transforms are needed in order to compute steering angles that will point each drive axle toward the robot's center of rotation. I'm calling tf::TransformListener::lookupTransform() to get the transforms because that's how I've seen it done in all the examples I've examined. Is there some code available that works like lookupTransform(), but uses URDF data instead of tf? I can write it myself, but that sounds a problem that someone probably already solved. I didn't see anything similar to lookupTransform() in the URDF API docs.

[wmeeusse]

Okay, for that usecase using the urdf directly is definitely the right way to go. The easiest way to get something linke "lookupTransform" would be to parse the urdf into a kdl tree http://wiki.ros.org/kdl_parser, and then call the kdl api http://orocos.org/kdl to get the poses between the wheels and the base.

[mikepurvis]

@jim-rothrock Have you been satisfied with steered_wheel_base_controller? Would you consider contributing that to ros_controllers?

[jim-rothrock]

@jim-rothrock Have you been satisfied with steered_wheel_base_controller? Would you consider contributing that to ros_controllers?

I'm considering it, but will have to consult with my attorney concerning liability issues that may arise from letting parties outside of Wunderkammer Laboratory modify the code, as opposed to modifying their own forked versions.

[vincentrou]

We have made ROS controllers for ackermann and four_wheel_steering vehicles. https://github.com/romea/romea_controllers

Do not hesitate to test it and give us feedback !

This is only for indigo for now since there is a problem to simlulate PositionJointInterface on gazebo7/kinetic https://github.com/ros-simulation/gazebo_ros_pkgs/issues/479

[bmagyar]

Great! Do you guys have videos of these controllers? Perhaps a simulation package that brings up vehicles that we could use to try them? If we can get some tests & documentation set up for them we could even consider adding them to ros_controllers.

On 2 February 2017 at 17:26, vincentrou notifications@github.com wrote:

We have made ROS controllers for ackermann and four_wheel_steering vehicles. https://github.com/romea/romea_controllers

Do not hesitate to test it and give us feedback !

This is only for indigo for now since there is a problem to simlulate PositionJointInterface on gazebo7/kinetic ros-simulation/gazebo_ros_pkgs#479 https://github.com/ros-simulation/gazebo_ros_pkgs/issues/479

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/ros-controls/ros_controllers/issues/55#issuecomment-277006309, or mute the thread https://github.com/notifications/unsubscribe-auth/ADXH4ebxgLigU4f7LfsERP26jknsFdbsks5rYgPJgaJpZM4BD3TK .

[vincentrou]

Here is a vehicle that we use for now with those controllers : https://github.com/romea/robucar There is a simulation pkg if you want to test it.

There is already some basic test to validate the kinematics equations.

[vincentrou]

We are interested to integrate our four_wheel_drive_controller in ros-controls. Maybe it is better to created a new repository in ros-controls, named "vehicle_controllers" for example where we can put diff_drive_controllers, mecanum_drive_controller, four_wheel_drive_controller... What do you think ?

[bmagyar]

I like the idea of adding more controllers but I'd keep them for now in ros_controllers. It is still of a manageable size for me to release at once.

Could you create a pull request where you add four_wheel_drive_controller to ros_controllers? We can talk details over there.

[vincentrou]

Before I submit a PR. I have two package that four_wheel_steering_controller depends on (urdf_vehicle_kinematic and four_wheel_steering_msgs). For kinetic, what is better ? Releasing those two packages on my own but maybe it is a problem to depend on non official osrf package ? Or should I try to make them integrated if there is an interest ?

[bmagyar]

urdf_vehicle_kinematic seems to be a collection of utility functions. There are a couple of ways to do that:

• host the code inside four_wheel_streering_controller
• add the package to ros-controls (with some refactoring)
• refactor the controller and make the parsing self-hosted as in diff_drive_controller

four_wheel_steering_msgs: are there more of this kind for other controllers that you are using?

• add to control_msgs (feels a bit misplaced, but could work)
• create a new pkg called vehicle_msgs or something similar and add it there (this would be nice if there are more msgs we could put here)

[vincentrou]

For the urdf_vehicle_kinematic, the best could be to add the package to ros-controls because it could be use by multiples controller like the diff_drive_controller to retrieve track, wheel_radius and other kinematics.

For the four_wheel_steering_msgs, the ackermann_msgs already exist in the ros_drivers group but does not fill the need

• I can try to submit the four_wheel_steering_msgs package in ros_drivers
• create a new git repo named vehicle_msgs where the four_wheel_steering_msgs and ackermann_msgs can be placed

I have submit an issue https://github.com/ros-drivers/ackermann_msgs/issues/4 on the ackermann_msgs package to discuss the last point.

[jack-oquin]

Combining the messages in a single repo forces them to always be released together, with the same version numbers.

Given the stability of ackermann_msgs, I don't see much advantage to that.

[bmagyar]

Thanks for your input @jack-oquin , point taken! We can host four_wheel_steering_msgs here then.

As for urdf_vehicle_kinematic I'd at least rename it as urdf_vehicle_utilities or something like. On the other hand I'd generally avoid adding a new library that controllers depend on... Maybe something we could add to ros_control directly? For this we have to make sure that the code is as mature as it can be...

[vincentrou]

@jack-oquin has already create a repo in ros-drivers and I just push the package in it https://github.com/ros-drivers/four_wheel_steering_msgs

@bmagyar For the urdf_vehicle_kinematic, tell me if you prefer to create a repo in ros-controls or a pull request on the ros_control repo. It is not a very mature code and I do not see where to integrate it in ros_control so maybe it is better to create a new repo ? (like urdf_vehicle_utilities)

[bmagyar]

Could we get feedback some more please? @efernandez

[vincentrou]

@bmagyar has you said in your previous comment https://github.com/ros-controls/ros_controllers/issues/55#issuecomment-293848850 , I can host the urdf_vehicle_kinematic library in the four_wheel_streering_controller pkg. If there is interest in these library for other pkg we can create a standalone pkg later.

[efernandez]

Just to confirm, the main point of this thread now is how to integrate https://github.com/Romea/romea_controllers in ros_controllers, right? I don't want to miss any other important point of discussion here, @bmagyar

I see a lot of common things between the diff_drive_controller and multiple packages there, and I completely agree with getting things in and splitting some of the functionality. I'd just comment about the URDF stuff for now:

Most of this was originally done for the diff_drive_controller because there is/was nothing else providing that AFAIK. For some cases like isCylinder, it'd be great if some generic urdf library could provide that. Unless someone knows of any, I'd create a new repo for that. I don't really think it needs to leave in ros-controls honestly, since it's just a simple API to retrieve some fields from URDF elements, or perform some checks. However, if we want this as a library I'd have almost one word for each helper function/method:

1. Instead of euclideanOfVectors in https://github.com/Romea/romea_controllers/blob/master/urdf_vehicle_kinematic/src/urdf_vehicle_kinematic.cpp#L4 , I'd create something like tf2_geometry_msgs in https://github.com/ros/geometry2. It could be a tf2_urdf package. Once we have the conversion into a tf::Vector3 (for this particular example) we can use all the math it provides, and we won't need a method to compute the distance here, or the transform in other places. See http://docs.ros.org/diamondback/api/urdf/html/pose_8h_source.html for the three geometry-related class URDF offers. It looks like the conversion to tf::Vector3 is not supported, but I guess we could just rely on converting to tf::Transform the urdf::Pose. This is certainly minor, but IMHO it'd make things cleaner.

2. Regarding isCylinder https://github.com/Romea/romea_controllers/blob/master/urdf_vehicle_kinematic/src/urdf_vehicle_kinematic.cpp#L16, getWheelRadius https://github.com/Romea/romea_controllers/blob/master/urdf_vehicle_kinematic/src/urdf_vehicle_kinematic.cpp#L51, and similar ones, I think they should go into a new repo. For me they're just urdf helper methods, so the repo/package name should sound like that, although there's some sort of vehicle-related semantics that IMHO could be changed.

If we create a library for this, I think the methods should throw std::invalid_argument("MESSAGE") (see http://www.cplusplus.com/reference/stdexcept/invalid_argument/ for the recommended usage of this specific exception) instead of printing, which can be done on the client side.

A bigger change, that I wouldn't do now, would be to have something like getCylinder or getAsCylinder returning a urdf::Cylinder (instead of isCylinder) and then we just access the radius field. Again, I'm not saying we should do it, I'm just saying that it's better to have these functions as they're on a separate repo, and consider this API changes for a more generic interface that others could benefit from.

I refrain myself from adding more comments. If you like the idea of creating a tf2_urdf package in https://github.com/ros/geometry2, I can help with that. At the same time I'd create a new repo for the URDF stuff and update the diff_drive_controller and (optionally) https://github.com/Romea/romea_controllers to use it. This way all can be done and tested easily w/o breaking anything.

Regarding the four_wheel_steering_controller I see also a lot of common things, and we should certainly try to factorize things out. I wouldn't do that now though (I still need to sync CPR diff_drive_controller upstream). I'm OK with getting it into ros_controllers; minor changes can be discussed on a PR.

Regarding the messages it seems they're already in ros-drivers https://github.com/ros-drivers/four_wheel_steering_msgs , so I've nothing to say.

[bmagyar]

Spot on! I really like the idea of aligning with tf and urdf types although getting it into geometry2 may prove a bit tricky. What if we start hosting tf2_urdf in ros-controls in the beginning, then move it over once we are sure it is in a semi-final state. Easier to control it for us.

[efernandez]

Yes, works for me. I don't have a strong opinion on where to get things.

[mathias-luedtke]

What if we start hosting tf2_urdf in ros-controls in the beginning, then move it over once we are sure it is in a semi-final state. Easier to control it for us.

ros-controls/controller_tools?

[vincentrou]

1. I had a look to create a tf2_urdf, it works fine but I had to use tf2_eigen for Eigen::Vector3d which is the only conversion available for urdf::Vector3d. I think it is better to make the release of this new repo in geometry2 or robot_model but for now we could start in ros-controls by creating the repo tf2_urdf so I can make a PR on it

2. You can create a new repo in ros-controls named urdf_vehicle_utilities or urdf_utilities, if you want it less specific, where I can make a PR with the modified actual urdf_vehicle_kinematic pkg

[bmagyar]

https://github.com/ros-controls/urdf_utilities (I know this sounds typical) we may rename it later

[vincentrou]

@bmagyar Do you want also to create the tf2_urdf in ros-controls ? Or we leave it for now if it is just for the euclideanOfVectors function ?

[bmagyar]

I wouldn't create a new package for a single utility function. Let's roll with one but try to use tf2 & Eigen math tools as much as you can.

On 28 April 2017 at 13:55, vincentrou notifications@github.com wrote:

@bmagyar https://github.com/bmagyar Do you want also to create the tf2_urdf in ros-controls ? Or we leave it for now if it is just for the euclideanOfVectors function ?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/ros-controls/ros_controllers/issues/55#issuecomment-297990576, or mute the thread https://github.com/notifications/unsubscribe-auth/ADXH4QtRY_slPzaXrOpMQ_dgg9MEFZ84ks5r0eHYgaJpZM4BD3TK .