Integrate ROVIOLI Localization mode with ROS Navigation Stack

[gerrysuomi]

I'm a newbie to maplab and I will try to elaborate my problem as detailed as possible. What I'd like to do is to integrate ROVIOLI with localization mode enabled into ROS Navigation Stack (for example, to replace the AMCL localization system). I followed the ROS Navigation tutorials (http://wiki.ros.org/navigation/Tutorials/RobotSetup) to setup the navigation stack. A global map is created with maplab in VIO mode. Then my navigation application is launched without any error. But when I try to set the initial pose of the robot in rviz with the button '2D Pose Estimate', it seems that this does not take any effect, the robot keeps staying at the same location. I checked that the topic/initialpose is published from rviz and there is data published when '2D Pose Estimate' button is clicked. But there is no subscriber of this topic in my current setup. I find out from aforementioned ROS Navigation tutorials that if AMCL is used as localization system, it is the subscriber of topic /initialpose. However, it seems that /initialpose is unique for AMCL. So I'd like to ask how to set the initial pose of robot in my navigation setup if ROVIOLI localization is used. Or more generally, how can ROVIOLI localization be integrated with ROS Navigation? I'm really puzzled by this problem and any help is highly appreciated. Thanks in advance.

Best regards, Gerry

[mfehr]

Hi @gerrysuomi There is no such subscriber at the moment.

But conceptually, if you do run ROVIOLI in localization mode, it will do global localization, so it should not need an initial guess, assuming it has the chance to successfully localize, i.e. the robot has somewhat a similar viewpoint like when he did the localization map. Until it localizes ROVIOLI will output simply the odometry estimate, which starts at the origin. Have a look at the two topics that are published, I think they are something like T_G_B/I (localized) and T_M_B/I (odom).

Now, if you want to initialize the odometry pose manually somehow, you need to add a subscriber yourself. I see two ways to implement it:

• You hook into the filter reset logic, and then just set the position of the 6DOF pose to the desired 2D position. This should work if you just use ROVIOLI without building a map, if you build a map, then this sudden jump in odometry estimate won't make sense for map building and the resulting map will be inconsistent unless you handle that as well. But this is a bit more complicated. So bottom line this could be a simple workaround unless you are building a map.
• A better way could be to treat the initial guess as what it actually is, a manual localization. So you add a subscriber and turn the 2D estimate in a 6DOF localization and feed it to the filter. This then should be treated as such and the "jump" should correctly go into the baseframe (from global to odom base frame), rather than the odometry transformation (odom base frame to robot base sensor). So have a look at how localization results are created in the ROVIOLI process and turn your click into such a localization result message and feed it into the message flow.