When starting the GPS Ros Node the reading from the Raspberry Serial Port most of the time works but sometimes after rebooting, it does not read the data correctly and spills out the same char again and again(always a "?"). Only after recompiling or restarting the node, it starts working again.
int main(int argc, char **argv)
{
int fd;
ros::init(argc, argv, "talker");
ros::NodeHandle n;
gps_node::gps_raw gps_data;
ros::Publisher chatter_pub = n.advertise<gps_node::gps_raw>("gps_raw", 100);
ros::Rate loop_rate(1000);
if ((fd = serialOpen ("/dev/ttyAMA0", 115200)) < 0)
{
fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
}
if (wiringPiSetup () == -1)
{
fprintf (stdout, "Unable to start wiringPi: %s\n", strerror (errno)) ;
}
char input = 0;
while (ros::ok())
{
while (serialDataAvail (fd))
{
input = serialGetchar (fd);
ROS_INFO_STREAM(input);
NazaDecoder.decode(input);
gps_data.gps_sats = round(NazaDecoder.getNumSat());
gps_data.lat = NazaDecoder.getLat();
gps_data.lon = NazaDecoder.getLon();
gps_data.heading = round(NazaDecoder.getHeadingNc());
gps_data.alt = NazaDecoder.getGpsAlt();
chatter_pub.publish(gps_data);
ros::spinOnce();
loop_rate.sleep();
}
}
return 0;
}
I found a working solution, by reopening the serial port by failure.
if (wiringPiSetup () == -1)
{
fprintf (stdout, "Unable to start wiringPi: %s\n", strerror (errno)) ;
}
REINIT:if ((fd = serialOpen ("/dev/ttyAMA0", 115200)) < 0)
{
fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
}
int input = 0;
while (ros::ok())
{
while (serialDataAvail (fd))
{
input = serialGetchar (fd);
NazaDecoder.decode(input);
gps_data.gps_sats = round(NazaDecoder.getNumSat());
gps_data.lat = NazaDecoder.getLat();
gps_data.lon = NazaDecoder.getLon();
gps_data.heading = round(NazaDecoder.getHeadingNc());
gps_data.alt = NazaDecoder.getGpsAlt();
chatter_pub.publish(gps_data);
ros::spinOnce();
loop_rate.sleep();
if(input==-1){
goto REINIT;
}
}
}
return 0;