ROS SITL missing GNSS topic

[Georacer]

Hello everyone,

First-off, I tried to post the question in the Discuss forum, but the registration link is broken and the new post editor is kind of broken as well, popping weird messages. So here goes:

I did a fresh build from master for rosflight and tried out the ROS SITL. I launched Gazebo as well as the rosflight_io node.

I can get the simulation up and running and can get a list of topics:

/airspeed
/attitude
/attitude/euler
/attitude_correction
/aux_command
/baro
/clock
/command
/fixedwing/RC
/fixedwing/truth/NED
/fixedwing/truth/NWU
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu/data
/imu/temperature
/magnetometer
/output_raw
/rc_raw
/rosflight_errors
/rosout
/rosout_agg
/sonar
/status
/unsaved_params
/version
george@george-

All listed topics publish at the correct rates, so things must be at least kind of right, BUT:

There are no gnss and navsat_fix/* topics published. Is that intensional?

The AP should publish such data since:

rosservice call /param_get STRM_GNSS
exists: True
value: 1000.0

Am I doing something wrong?

---

Side question 1: I'm wondering what is the reasoning behind setting GNSS stream rate to such a high value (1000Hz) when the attitude msg is streamed at 200Hz

Side question 2: Is it still the recommended (and only) way to use a GPS sensor to attach it to the onboard (companion) computer in NMEA mode?

Thanks for your time!

[superjax]

Thanks for posting! I am so sorry about the discuss site. We have had a nightmare getting our hosting provider to play nice, and have plans to migrate to something new. This is a fine place to post questions.

1 - So, that is actually a little misleading, and we should definitely document that better. We aren't sending GPS at 1000 Hz, we are actually checking if we have received a new GPS message and need to send it at 1000Hz. GNSS is handled differently than the other sensors, and this is a hack to get around some of it's oddities. @BillThePlatypus, is there a way that we could unify this API?

void Sensors::update_other_sensors()
{
  switch (next_sensor_to_update_)
  {
  case GNSS:
    if (rf_.board_.gnss_present() && rf_.board_.gnss_has_new_data())
    {
      data_.gnss_present = true;
      data_.gnss_new_data = true;
      rf_.board_.gnss_update();
      this->data_.gnss_data = rf_.board_.gnss_read();
      this->data_.gnss_raw = rf_.board_.gnss_raw_read();
    }
    break;
...

the gnss_has_new_data() is sensor dependent, but I think that in Gazebo it returns true at 5 or 10 Hz.

2 - Unfortunately, yes. Development on GNSS integration directly in the firmware is happening in the airbourne driver layer, mainly by @BillThePlatypus. We actually merged it into master around May, but have since pulled it out because of some problems with hot-swapping the sensor.

If I understand correctly, we recently had some successful tests of this functionality, but @BillThePlatypus would know the most about this.

[Georacer]

Okay... so is there any way to have simulated GNSS signals during SITL?

[BillThePlatypus]

GNSS pass-through in hardware currently isn't working, but it should work fine in simulation. One thing that I notice is weird is that most of your sensor information is not in the /fixedwing/ namespace. How are you launching the simulation? Have you tried roslaunch rosplane_sim rosflight_sil.launch?

[Georacer]

I used this launchfile:

<!-- Launches a fixed-wing SIL vehicle in Gazebo -->

<launch>
  <arg name="mav_name"            value="fixedwing"/>

  <arg name="color"               default="White"/>
  <arg name="x"                   default="0"/>
  <arg name="y"                   default="0"/>
  <arg name="z"                   default="0.1"/>
  <arg name="yaw"                 default="0"/>
  <arg name="paused"              default="false"/>
  <arg name="gui"                 default="true"/>
  <arg name="verbose"             default="false"/>
  <arg name="debug"               default="false"/>

  <include file="$(find rosflight_sim)/launch/base.launch">
    <arg name="mav_name" value="$(arg mav_name)"/>
    <arg name="color" value="$(arg color)"/>
    <arg name="x" value="$(arg x)"/>
    <arg name="y" value="$(arg y)"/>
    <arg name="z" value="$(arg z)"/>
    <arg name="yaw" value="$(arg yaw)"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="verbose" value="$(arg verbose)"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <node name="rosflight_io" pkg="rosflight" type="rosflight_io" output="screen">
    <param name="udp" value="true"/>
  </node>

  <!-- Requires joystick to be present
  <node name="rc_joy" pkg="rosflight_joy" type="rc_joy">
    <remap from="RC" to="fixedwing/RC"/>
  </node>
    -->

</launch>

[Georacer]

It was directly launched from ROSFlight launch files, not ROSPlane, which prepends the mav_name prefix as the namespace of rosflight.io.

[Georacer]

Okay, I found the reason no GNSS messages were published. I was using the ROSFlight xacro plane file which doesn't declare any Gazebo plugins. Instead, I guess you are usually using the ROSPlane which DOES declare the Gazebo GPS plugin in its fixedwing xacro file.

I guess an update for both xacro fixedwing and multirotor files is in order for ROSFlight

[Georacer]

Are you okay with a PR adding a rosflight-plugins dependency on rosflight to employ the Gazebo plugins in the plain rosflight SITL?

[BillThePlatypus]

The GPS plugin is meant to simulate a GNSS receiver plugged into the on board computer over USB. I'm looking at the rosflight_sim sil_board.cpp, and I see that it is programmed as if no GNSS reciever is plugged into the board.

bool SIL_Board::gnss_present() { return false; }
void SIL_Board::gnss_update() {}

rosflight_firmware::GNSSData SIL_Board::gnss_read()
{
    rosflight_firmware::GNSSData out;
    return out;
}

@superjax, do we want to change this behavior?

[superjax]

@BillThePlatypus Yeah, let's change this to mimic the behavior we are planning on.

[superjax]

@Georacer, sure! That would be awesome! Thank you.

[Georacer]

I created PR #110, which is a shortcut, using the GPS plugin from rosflight_plugins, in the same manner as ROSPlane does.

If you want to incorporate the GPS functionality in the rosflight_sil_plugin directly, you should abolish the GPS plugin as redundant. But this is you executive choice.

[Georacer]

After we review the PR, I'd like to continue the discussion on GPS usage, which I don't understand.

1. As we have established the F4 doesn't read the GPS directly, ok
2. Once the GPS is connected to the companion computer, how is one supposed to read it? The rosflight_utils/libnmea_navsat_driver only publishes a rosflight_msgs/GPS.msg message which doesn't exist.
3. The ROSPlane algorithms just expect to find a navsat_compat/ pair of topics, without generating them.
4. Am I just supposed to create a node reading the GPS and publish the navsatfix/ or gnss topic myself?

Thanks

[superjax]

@BillThePlatypus, @engband What is the current method for doing this?

[superjax]

Here's what I think. We should just sit down and actually finish GPS integration in SITL, that's the plan going forwards so I think it makes sense to do that. This should be easy, (we can just copy the logic from rosflight_plugins into the STIL plugin and it will be the most like our expect hardware setup.)

The fundamental problem is that the navsatfix message is not a good way to fuse GPS. GNSS receivers actually measure ECEF position and velocity. They don't natively measure LLA and ground speed/heading, despite them commonly supplying us that information. There is an internal conversion to Geodetic they do, but we should really be doing that ourselves so we don't have weird linearization errors from their conversions. Furthermore, most state estimation algorithms I know depend on linear algebra. Having measurements be an actual vector (as opposed to heading and LLA angles) makes things much more efficient and reduces linearization error.

Anyway, because of that, we switched a bunch of our stuff over to the new GNSS message, but a lot of it got left behind. I'm going to give myself a task to fix this now. It's mainly just that someone needs to sit down and push a bunch of changes to make everything agree again.

For legacy reasons, I think it makes sense to continue publishing the navsatfix message, even if it isn't used by us anywhere.

Okay, so here is the plan: [x] - rosflight_io publishes GNSS, navsat (already done) [ PR ] - rosflight_sim supplies gnss information to be sent by rosflight_io @superjax [ ] - firmware supplies gnss information to be sent by rosflight_io @BillThePlatypus [x] - roscopter subscribes directly to GNSS @mmmfarrell [ PR ] - ros_plane subscribes to GNSS and converts it to its internal type before fusing it. @superjax [PR ] - remove libnmea_navsat_driver and replace it with a stripped-down version of 'UBLOX_read' that publishes rosflight_msgs/GNSS @superjax @matthewk-rydalch

This plan removes support for NMEA. Does your use case depend on non-UBLOX receivers?

Everything but the firmware integration should be pretty quick. Is there anything else that I am missing?

[Georacer]

Although the information path for real and simulated flight is still not clear to me, I trust you have a better handle in this than I do. I would just like to add than you keep in mind that by deprecating the NMEA driver and using only uBlox ECEF information, you might find yourself in want for NMEA in the future. Ardupilot had abolished it but re-instated later.

But granted, the goal to integrate ECEF directly in the firmware is an understandable one and a good reason to veer away from generic NMEA.

[plusk01]

fyi @goromal

[superjax]

Yeah, I understand that. If we need NMEA in the future, we can bring it back. For now it makes things simpler because the UBX NAV-ECEFPOS and NAV-ECEFVEL give us exactly what we want! We used to use some skytraq receivers, but we haven't used those in a long time.

The path looks like this:

   Until Firmware            Planned Long-Term                     Simulation
Integration Complete              Use Case

      +-------+                  +-------+                    +----------------+
      |       |                  |       |                    |                |
      |  GPS  |                  |  GPS  |                    |     gazebo     |
      |       |                  |       |                    |                |
      +---+---+                  +---+---+                    +--------+-------+
          |                          |                                 |
          |UBX (USB)                 |UBX  (UART)                      | STIL plugin
          |                          v                                 v
          |                    +-----+----+                   +--------+-------+
          v                    |          |                   |                |
 +--------+------+             | Firmware |                   | rosflight_sim  |
 |               |             |          |                   |                |
 |  new UBLOX    |             +----+-----+                   +--------+-------+
 |  driver       |                  |                                  |
 |               |                  |mavlink (USB)                     |mavlink (UDP)
 +------+--------+                  v                                  v
        |                   +-------+------+                   +-------+------+
        |                   |              |                   |              |
        |                   | rosflight_io |                   | rosflight_io |
        |                   |              |                   |              |
        |                   +------+-------+                   +------+-------+
        |                          |                                  |
        |rosflight_msgs/GNSS       |rosflight_msgs/GNSS               |rosflight_msgs/GNSS
        |                          v                                  v
 +------v------+            +------+------+                    +------+------+
 |             |            |             |                    |             |
 | ros_copter/ |            | ros_copter/ |                    | ros_copter/ |
 |  ros_plane  |            |  ros_plane  |                    |  ros_plane  |
 |             |            |             |                    |             |
 +-------------+            +-------------+                    +-------------+

[Georacer]

Ah, so GPS information doesn't actually get passed (yet) through ROSFlight, but is directly caught by ROSPlane and fused accordingly. It's just as I suspected, but the sanity check is reassuring. Thanks!

Also, cool graphics!

[mmmfarrell]

The ROScopter portion of this is already done on my branch ‘outdoor’ that will be merged in after final flight tests next week.

ROScopter will subscribe to the UBLOX ‘PosVelEcef’ message or the inertial sense ‘GPS’ message and convert it to the ‘rosflight_msgs/GNSS’ message or just directly subscribe to the ‘rosflight_msgs/GNSS’ message.

On Thu, Oct 17, 2019 at 9:38 AM George Zogopoulos-Papaliakos < notifications@github.com> wrote:

Ah, so GPS information doesn't actually get passed (yet) through ROSFlight, but is directly caught by ROSPlane and fused accordingly. It's just as I suspected, but the sanity check is reassuring. Thanks!

Also, cool graphics!

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/rosflight/rosflight/issues/109?email_source=notifications&email_token=AFCDMPYOIPYUFQZDAU4TKGTQPCIILA5CNFSM4JAPFBG2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOEBQXXPY#issuecomment-543259583, or unsubscribe https://github.com/notifications/unsubscribe-auth/AFCDMP3DVMV3J6AF3SJERNDQPCIILANCNFSM4JAPFBGQ .

[BillThePlatypus]

As of a few weeks ago, ROSplane uses NavSatFix and TwistStamped messages. This means that if your GPS reciever supports NMEA, you can use the standard NMEA navsat driver. Perhaps that will switch to ECEF in the future.

I think that it may be a good idea to add a node that converts NavsatFix and TwistStamped to ROSflight GNSS messages. This would improve compatibility with third party nodes.

[superjax]

I just created to PRs to address some of these issues. the UBX parser is about 80% done, but I don't know if I'll have time to work on this weekend. We'll see.

[bsutherland333]

I believe everything in this issue has already been implemented, so I'm going to close it.