GPS sensor (ROSbot 2.0)

Hi!

I try to edit the ROSbot 2.0 firmware to use GPS.

I add the lib from mbed web.
NEO-6M GPS : GPS - read gps data | Mbed

and i connect the two gps as sown in figure below.

After that, I edit the firmware code in in main.cpp like below.

#include <GPS.h>

#define GPS1_RX SENS4_PIN3
#define GPS1_TX SENS4_PIN4
#define GPS1_BaudRATE 9600

#define GPS2_RX SENS5_PIN3
#define GPS2_TX SENS5_PIN4
#define GPS2_BaudRATE 9600

sensor_msgs::NavSatFix gps_msg1;
sensor_msgs::NavSatFix gps_msg2;

ros::Publisher *gps_pub1;
ros::Publisher *gps_pub2;

tatic void initGPSPublisher1()
{
    gps_pub1 = new ros::Publisher("gps1", &gps_msg1);
    nh.advertise(*gps_pub1);
}

static void initGPSPublisher2()
{
    gps_pub2 = new ros::Publisher("gps2", &gps_msg2);
    nh.advertise(*gps_pub2);
}



...
int main()
{
    GPS gps1(GPS1_TX, GPS1_RX, GPS1_BaudRATE);
    GPS gps2(GPS2_TX, GPS2_RX, GPS2_BaudRATE);
    
    initGPSPublisher1();
    initGPSPublisher2();

    if (true) // gps1
        {
            
            gps_msg1.latitude = gps1.latitude;
            gps_msg1.longitude = gps1.longitude;

            if (nh.connected())
                gps_pub1->publish(&gps_msg1);

        }

        if (true) // gps2
        {
            gps_msg2.latitude = gps2.latitude;
            gps_msg2.longitude = gps2.longitude;

            if (nh.connected())
                gps_pub2->publish(&gps_msg2);
        }
...
}

The whole code are upload in github link.

By using this code, I flash the ROSbot 2.0.
However, when I run the rosbot_ekf all.launch file.
I chouldn’t have GPS sensor signal and error msg is came.
image
image

How can i solve this problem?

Hi,
Checking in progress, but I am almost sure it’s a problem with your GPS lib.

More soon.

Thank you for the answer.

I think it’s a GPS lib problem or GPS module connection problem.

In order to check in which part of the problem, there was no problem when I was arbitrarily substituting 5 into the GPS topic as shown in the code below.

        if (true) // gps1
        {
            gps_msg1.latitude = 5;
            gps_msg1.longitude = 5;

            gps_msg1.status.status = 5;

            if (nh.connected())
                gps_pub1->publish(&gps_msg1);

        }

But if I put the delay like the code below, the same error occurred. Why do you have a problem when you put this delay?

        if (true) // gps1
        {
            gps_msg1.latitude = 5;
            gps_msg1.longitude = 5;

            gps_msg1.status.status = 5;

            if (nh.connected())
                gps_pub1->publish(&gps_msg1);

            wait(1.0);

        }

Thank you.

Hi,
I had some more fun with this case than I initially intended.

I’m privileged to have a bare CORE2 board :wink: so I ran all my tests and example using it instead of ROSbot 2.0. It should make no difference. So, I kinda solved your problem. however, there is a catch.

Yes, the problem was the badly written GPS library that you added to your code. It uses a blocking API and was designed to crash the program if there are too many characters in the input :man_facepalming: ROSbot’s firmware is a multi-threaded application, so you should avoid using code that blocks execution for long amounts of time.

Also, you were using the wrong pins for the UARTs. In my example, I use UARTs accessible on ROSbot’s back panel (hExt and hSens3). Finally, in my example I significantly increased stack size of the main thread.

Pico

I used Raspberry Pico running micropython to simulate two GPS serial inputs.

It outputs a recurring stream of common GGA sentences that follow the NEMA standard. On UART0 and UART1.

Here is the code: GitHub - byq77/pico-gps-sim

I didn’t have any GPS sensors at hand so this was the only way.

GPS ROS example

Here is the code of the GPS library I wrote: GitHub - byq77/mbed6-gps-lib. The GPS library has a similar API to the one you used but mine is non-blocking. It uses a circular buffer and interrupts to read bytes from the input stream.

And the code of the example: GitHub - byq77/core2-pio-mbed6-gps-example

You can test example right away, the binary is in the release. You can flash the hex file using e.g. core2-flasher.

The example code is quite simple:

#include "mbed.h"
#include "gps.h"
#include "ros.h"
#include "sensor_msgs/NavSatFix.h"

#define GPS1_TX SENS3_PIN3
#define GPS1_RX SENS3_PIN4

#define GPS2_TX EXT_PIN7
#define GPS2_RX EXT_PIN6

constexpr std::chrono::milliseconds GPS_UPDATE_DT{100ms};
constexpr std::chrono::milliseconds SPIN_DELAY{10ms};

class RosGps : private ros::NodeHandle
{
public:

    RosGps()
    {
        // initialise ros node
        initNode();

        _gps1_msg.header.frame_id = "gps1";
        _gps2_msg.header.frame_id = "gps2";

        // advertise publisher
        advertise(_gps1_pub);
        advertise(_gps2_pub);
    };

    /**
     * @brief Convert raw longitude and latitude data to ros format
     * 
     * @param value raw longitude or latitude
     * @param nsew n,s,e,w
     * @return converted value 
     */
    double convertToRosSatPos(double value, char nsew)
    {
        switch(nsew)
        {
            case 'n':
            case 'N':
            case 'e':
            case 'E':
                return value;
            default:
                return (-1)*value;
        }
    }

    /**
     * @brief Run the node.
     */
    void spin()
    {
        auto time = Kernel::Clock::now();
        auto gps_check_time = time + GPS_UPDATE_DT;

        while (1)
        {
            time = Kernel::Clock::now();

            if(time >= gps_check_time)
            {
                if(_gps1.update())
                {
                    loginfo("GPS1 sample received");
                    _gps1_msg.latitude = convertToRosSatPos(_gps1.latitude, _gps1.ns);
                    _gps1_msg.longitude = convertToRosSatPos(_gps1.longitude, _gps1.ew);
                    sensor_msgs::NavSatStatus nav_sat_status;
                    nav_sat_status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
                    nav_sat_status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; 
                    _gps1_msg.status = nav_sat_status;

                    _gps1_pub.publish(&_gps1_msg);
                }

                if(_gps2.update())
                {
                    loginfo("GPS2 sample received");
                    _gps2_msg.latitude = convertToRosSatPos(_gps2.latitude, _gps2.ns);
                    _gps2_msg.longitude = convertToRosSatPos(_gps2.longitude, _gps2.ew);
                    sensor_msgs::NavSatStatus nav_sat_status;
                    nav_sat_status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
                    nav_sat_status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; 
                    _gps2_msg.status = nav_sat_status;

                    _gps2_pub.publish(&_gps2_msg);
                }

                gps_check_time+=GPS_UPDATE_DT;
            }

            spinOnce();
            ThisThread::sleep_until(time + SPIN_DELAY);
        }
    };

private:
    DigitalOut _sens_power_{SENS_POWER_ON, 1}; // POWER SENSOR ON

    DigitalOut _gps1_comm_indicator_{LED1, 0};
    DigitalOut _gps2_comm_indicator_{LED2, 0};

    GPS _gps1{GPS1_TX, GPS1_RX};
    GPS _gps2{GPS2_TX, GPS2_RX};

    sensor_msgs::NavSatFix _gps1_msg{};
    sensor_msgs::NavSatFix _gps2_msg{};

    ros::Publisher _gps1_pub{"gps1", &_gps1_msg};
    ros::Publisher _gps2_pub{"gps2", &_gps2_msg};
};

int main()
{
    RosGps test;
    test.spin();

    return 0;
}

It publishes to the two topics: gps1 and gps2.

This is how you can increase a size of the main thread stack size.

"rtos.main-thread-stack-size": 10240,

Round-up

I hope the example I created will help you develop your specific application. It is simple and I tried to partially document the code, so it should be self-explanatory. Anyway, I intended to leave something that would help others with a similar problems (adding a custom sensor to the ROSbot firmware).

Lastly, there is a catch. I wrote the example and the library for Mbed Os 6 and the ROSbot ros1 firmware is using an older version - v5.15. There are no plans to update it to Mbed OS 6, at least not for the ros1 version. You should be able with minimal effort to use the library directly in the ROSbot’s ros1 firmware, though. The changes are mainly around using C++14. Here you can learn about the differences between Mbed versions: Full API list - API references and tutorials | Mbed OS 6 Documentation

1 Like

Thank you so much for your help. I will try applying your example.
I will try running the example and will get back to reply.

I flashed the Core2 board using the example code to receive GPS data from a single GPS sensor. The GPS sensor is connected in SEN3 pins. I commented out the gps2 section of the code.

#include "mbed.h"
#include "gps.h"
#include "ros.h"
#include "sensor_msgs/NavSatFix.h"


#define GPS1_TX SENS3_PIN3
#define GPS1_RX SENS3_PIN4

// #define GPS2_TX EXT_PIN7
// #define GPS2_RX EXT_PIN6

constexpr std::chrono::milliseconds GPS_UPDATE_DT{100ms};
constexpr std::chrono::milliseconds SPIN_DELAY{10ms};

class RosGps : private ros::NodeHandle
{
public:

    RosGps()
    {
        // initialise ros node
        initNode();

        _gps1_msg.header.frame_id = "gps1";
        // _gps2_msg.header.frame_id = "gps2";

        // advertise publisher
        advertise(_gps1_pub);
        // advertise(_gps2_pub);
    };

    /**
     * @brief Convert raw longitude and latitude data to ros format
     * 
     * @param value raw longitude or latitude
     * @param nsew n,s,e,w
     * @return converted value 
     */
    double convertToRosSatPos(double value, char nsew)
    {
        switch(nsew)
        {
            case 'n':
            case 'N':
            case 'e':
            case 'E':
                return value;
            default:
                return (-1)*value;
        }
    }

    /**
     * @brief Run the node.
     */
    void spin()
    {
        auto time = Kernel::Clock::now();
        auto gps_check_time = time + GPS_UPDATE_DT;

        while (1)
        {
            time = Kernel::Clock::now();

            if(time >= gps_check_time)
            {
                if(_gps1.update())
                {
                    loginfo("GPS1 sample received");
                    _gps1_msg.latitude = convertToRosSatPos(_gps1.latitude, _gps1.ns);
                    _gps1_msg.longitude = convertToRosSatPos(_gps1.longitude, _gps1.ew);
                    sensor_msgs::NavSatStatus nav_sat_status;
                    nav_sat_status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
                    nav_sat_status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; 
                    _gps1_msg.status = nav_sat_status;

                    _gps1_pub.publish(&_gps1_msg);
                }

                // if(_gps2.update())
                // {
                //     loginfo("GPS2 sample received");
                //     _gps2_msg.latitude = convertToRosSatPos(_gps2.latitude, _gps2.ns);
                //     _gps2_msg.longitude = convertToRosSatPos(_gps2.longitude, _gps2.ew);
                //     sensor_msgs::NavSatStatus nav_sat_status;
                //     nav_sat_status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
                //     nav_sat_status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; 
                //     _gps2_msg.status = nav_sat_status;

                //     _gps2_pub.publish(&_gps2_msg);
                // }

                gps_check_time+=GPS_UPDATE_DT;
            }

            spinOnce();
            ThisThread::sleep_until(time + SPIN_DELAY);
        }
    };

private:
    DigitalOut _sens_power_{SENS_POWER_ON, 1}; // POWER SENSOR ON

    DigitalOut _gps1_comm_indicator_{LED1, 0};
    // DigitalOut _gps2_comm_indicator_{LED2, 0};

    GPS _gps1{GPS1_TX, GPS1_RX};
    // GPS _gps2{GPS2_TX, GPS2_RX};

    sensor_msgs::NavSatFix _gps1_msg{};
    // sensor_msgs::NavSatFix _gps2_msg{};

    ros::Publisher _gps1_pub{"gps1", &_gps1_msg};
    // ros::Publisher _gps2_pub{"gps2", &_gps2_msg};
};

int main()
{
    RosGps test;
    test.spin();

    return 0;
}

I check the ROS topic that the ‘gps1’ topic was generated. However, when I tried to echo the topic, it seemed like the topic was not being published.

image

What could be the problem?

Hello @chokings

The lack of messages may be due to hardware problems.

  1. Are you sure you connected the GPS sensor correctly?
  2. Is the sensor powered?
  3. Is the sensor working at all? Have you tested it with other hardware?
  4. Is the sensor configured to give the exact NEMA sentence that the library supports?

To check if these serial pins on the Rear Panel are working, you can, for example, do a loopback, i.e. connect the TX of one to the RX of the other and send “Hello World”, or better, send this NEMA sentence, so that the library will work.

Best regards
Jan Brzyk

Hello @JanBrzyk

  1. Are you sure you connected the GPS sensor correctly?
    I connected the GPS sensor to sen3pin as shown in the picture below.

  2. Is the sensor powered?
    As mentioned in my previous response, I connected it to the 5V pin and confirmed that the GPS LED turned on.

  3. Is the sensor working at all? Have you tested it with other hardware?
    I confirmed that the GPS is working properly when connected to an Arduino UNO board.

  4. Is the sensor configured to give the exact NEMA sentence that the library supports?
    I confirmed that the GPS module being used supports NMEA, UBX, and RTCM protocols, and was able to receive GPS data in NMEA protocol with Arduino.

Thank you.

Hi,
If you used Gps-neo-6m Arduino Library and Tutorial to test your sensor with Arduino you can see that it supports by default a completely different NEMA sentence than my library. I wrote mine to support GPGGA sentences like the library you used in your initial code. I did say that I do not have that particular sensor to test. You have to check what type of sentence your sensor outputs and adjust the library to support that sentence.

1 Like

Thank you for your advice. I will check it out.

Hi,
I have verified that my GPS can receive messages in the GPGGA format. I used the code from the link below to perform the verification with Arduino.

As a result, it was confirmed that the GPS data contains GPGGA, as shown in the figure below.

After that, I wrote a code to receive GPS data by incorporating the libraries you provided to apply to ROSbot 2.0. The entire code is available at the link below.

        if (true) // gps1
        {
            gps1.update();
            gps_msg1.latitude = gps1.latitude;
            gps_msg1.longitude = gps1.longitude;

            // gps_msg1.status.status = gps1.nsats;

            

            if (nh.connected())
                gps_pub1->publish(&gps_msg1);

            
        }

However, after uploading the code and checking the rostopic, it repeats as shown in the figure below.

What other parts need to be modified?

Thank you.

Hello @chokings

Unfortunately, I have to say that we are unable to continue troubleshooting your external hardware example, as this is not a technical issue with the ROSbots.

However, I encourage the entire community to join the discussion!

Best regards
Jan Brzyk

Yo @chokings,
The update() method returns true if it is able to successfully read a GPGGA message so if you do not check what is returned the GPS fields might have initial values (zeros).

I made a slight modification to my example and lib. I modify my lib to react to the serial message stream even if messages are not terminated with a carriage return character (that could be a problem) I also added a method that returns the NEMA message string.

2 Likes