search

uuvsimulator / uuv_simulator

Gazebo/ROS packages for underwater robotics simulation
https://uuvsimulator.github.io/
Other
699 stars 361 forks source link

Coordinate system #165

Open narcispr opened 6 years ago

narcispr commented 6 years ago

Hi, I'm new to uuv_simulator. I was previously using UWSim. UUV simulator looks very nice but I've a question: In underwater robotics we normally use North, East, Down (NED) coordinate system while Gazebo and uuv_simulator use Z up. Is there a way to change that? Thank you!

musamarcusso commented 6 years ago

Hi @narcispr, actually Gazebo does not accept any other standard other than ENU. In the necessary plugins we have to perform the transformation when necessary. Do you have something specific that you want to represent in NED?

narcispr commented 6 years ago

Hi @musamarcusso, in general in underwater robotics NED is always used instead of ENU. I'm trying to simulate a new AUV (Girona500) but my idea is to use uuv_simulator to simulate the environment, the AUV dynamics and the sensors but the current AUV architecture COLA2 for the rest. For now, what I've done is to take the nav sensor measurements generated by the uuv_simulator and apply the following transformations in my localization filter.

I've seen that the SetReferencePose() function in the Gazebo IMU let you define the IMU in NED. Having an IMU in NED will be wonderful :)

With these transformations I can feed this navigation data to my localization filter (instead of the data comming from the real sensors) and everything works fine. However, when I look at the TF tree I've 2 independent trees: one in NED and the other in ENU.

Sorry, I'm new to both uuv_simulator and Gazebo. If you can clarify some of my questions it ill be great!

musamarcusso commented 6 years ago

Hi @narcispr, yes, I am aware that NED is used in underwater robotics. We had for a long time see what we could do best here to change and where it would be possible to change because we also used some ROS packages for other purposes with which NED was not compatible. I can check how to make the changes you proposed, I will give you a feedback to you on that soon.

musamarcusso commented 6 years ago

I have been trying to list all the spots in the package where it could be nice to have the change to NED, but I think it would be better to have both options (NED or ENU). What do you think? I say that because of the issue that ROS packages might not recognize it, so I would add an option to the plugins and ROS nodes to use one or the other. This is a problem that I have been trying to figure out for a long time, but since there are not that many applications in underwater robotics for ROS (as far as I know), there seems to be no real interest on setting this up for both Gazebo and ROS.

narcispr commented 6 years ago

It will be great to be able to configure uuv_simulator to work in ENU or NED. I've participated in several european projects working with underwater vehicles and ROS, and all the groups use NED (IST, UJI, UIB, UdG, Atlass, CMRE, IFREMER, CNR, HWU, ...). The underwater community is quite big but not very visible, you know, we are underwater :)

About other ROS packages, we have never found any problem using NED. We have used ros control, moveit!, several localization algorithms, ... Only for RViz it is recomended to add an extra TF that rotates the whole world 180º Roll, otherwise it is difficult to move the scene.

Thank you!

musamarcusso commented 6 years ago

Did you do some extra change in Gazebo to use NED seamlessly as well, in case you used Gazebo? In ROS I can see that is quite simple to configure. As far as I understand, there is an extra NED TF and all frames are transformed related to that frame instead of the usual world frame, correct?

musamarcusso commented 6 years ago

I ask because I want to see if I have to switch to NED at the input of nodes such as the thruster manager, but not when generating the TAM, since the thrust forces need to be transformed back to ENU to be applied on the Gazebo vehicle model. For the ROS nodes I can see your point, the controller interfaces have internally this transformation to and from NED already. I have seen other projects, but I didn't find a lot open-source material on simulation (like for example UWSim and other packages that use it, but since they are not coupled to Gazebo, it can be a little easier to establish the default reference frames). I would be interested to know if anyone already managed to make NED on the Gazebo side somewhat easier to be set as default. From the information I read from forums, that is not being considered by the developers.

narcispr commented 6 years ago

I never worked with Gazebo before. For Rviz we do what you say: create an extra NED TF and transform all frames to that frame instead of the usual world frame. To use the current uuv_simulator what I did was only transform the navigation sensor measurements. I didn't touch the force applied to the AUV. I don't know if someone has managed to make NED on Gazebo, however, for what I see in http://gazebosim.org/api/dev/group__gazebo__sensors.html, it seems that Gazebo can simulate sensors using NED.

musamarcusso commented 6 years ago

For the sensors it can be managed, but when applying forces and torques in Gazebo it is necessary to use Gazebo's functions that only understands either the world frame (ENU) or the body frame. Also the vehicle state that is read from Gazebo would have to be transformed as well. I tried before to add a virtual NED frame to the vehicle, but it didn't work so well. Maybe I'll start adding the option to use NED first to the sensor plugins and the ROS nodes and then see what can be done for the apply force and torque functions.

musamarcusso commented 6 years ago

Since this is quite an extensive change, I'll try to separate the task in smaller ones to make sure the whole package doesn't break. I'll get back to you about this, I have just to see where the changes are most needed to start with a few modules.

clydemcqueen commented 6 years ago

Hi, this ROS standard might prove helpful: http://www.ros.org/reps/rep-0103.html. The standard suggests using a "_ned" suffix for NED frames.

narcispr commented 6 years ago

Thank you @musamarcusso! I think that having at least the sensors will be a great improvement. Forces are mostly given in body frame isn't it? Whenever you have something to be tested let me know!

musamarcusso commented 6 years ago

Hi @narcispr,

I have prepared a pull request #190 to try and begin to solve this issue. The first step now was on how to represent the NED frame in a meaningful way for all the entities. Like @clydemcqueen said, they can be represented with the "_ned" suffix, but have to be added kind of to every module, sensors especially, to generate the data. So the solution I propose here is to create a "world_ned" that is create under the "world" frame with the proper orientation and then add the vehicles and have their odometry generated wrt world_ned. So basically

world -> world_ned -> odom -> base_link

I haven't yet used a package like robot_pose_ekf, but I started by giving each Gazebo sensor plugin the option to use world_ned instead of world. That turned out to be a little tricky. An example is the p3d sensor I had been using before to generate the Odometry topic from the standard gazebo_ros_pkgs, that does not seem to accept another TF frame and inertial reference frame (it seems to have that option, but the odometry topic is generated in the ENU world all the same). I adapted the p3d sensor and all the others in the uuv_sensor_plugins package to now use either world or world_ned. The TF for world_ned has to be generated as well, but that is done using a static publisher that adds the transform between world and world_ned to tf_static and is started with the launch files for the Gazebo worlds. Now if you start the vehicle, there is an option to set the reference frame world_frame that will set up the vehicle and all its sensors to the same reference frame. The vehicle has also a base_link_ned that is create with it. This is the first step, I am still going to change the controllers and local planner to compute reference and error in either frame as well and see that the change is easy to be extended to new modules. Let me know what you think, unfortunately I couldn't find examples regarding Gazebo simulation using NED that showed a better solution.

Best,

Musa

musamarcusso commented 6 years ago

Hi @narcispr, I have been using the solution I posted before for a while and it seems to do the job. What do you think of it? If it passes as a way to have the NED frame, please let me know, so I can close this issue.