Hi,
I had some more fun with this case than I initially intended.
![DSC_0397](/uploads/default/original/1X/a2c492f1d2d4291c789715ad9b76b66bd1d8f97d.jpeg)
I’m privileged to have a bare CORE2 board
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
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.
![DSC_0395](/uploads/default/original/1X/fcdb0ba9fb22db2843cff98aa6081b3eda2915b6.jpeg)
It outputs a recurring stream of common GGA sentences that follow the NEMA standard. On UART0 and UART1.
![Screenshot from 2023-04-24 19-02-29](/uploads/default/original/1X/578e895365e40aa052b9f103877fdd5c1ccce74b.png)
Here is the code: https://github.com/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: https://github.com/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: https://github.com/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
.
![Screenshot from 2023-04-24 20-11-50](/uploads/default/original/1X/4469b119e57a30b9504160dade62283949f31f34.png)
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: https://os.mbed.com/docs/mbed-os/v6.16/apis/index.html#deprecated-apis