Question

我想为ROS GPSD客户端编写订户节点，该客户端正在发布主题“/ fix”的GPS坐标。我不知道究竟什么是正确的代码以及我必须在CMakeList.txt和package.xml中做出哪些更改。以下是代码

#include <ros/ros.h>
#include <sensor_msgs/NavSatStatus.h>  
#include <sensor_msgs/NavSatFix.h>
using namespace gps_common;
void callback(const sensor_msgs::NavSatFixConstPtr& fix) {
  if (fix->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
   ROS_INFO("No fix.");
   return;
}

  if (fix->header.stamp == ros::Time(0)) {
   return;
}

printf("\n Latitude = %f and Logitude = %f ",fix->latitude, fix->logitude);  

}

int main (int argc, char **argv) {
 ros::init(argc, argv, "gps_collect");
 ros::NodeHandle node;
 ros::Subscriber fix_sub = node.subscribe("fix", 10, callback);
 ros::spin();
 return 0;
 }

Answer 1

我正在使用C语言编写此代码。来源：https://github.com/felipegutierrez/raspberryExplore/blob/master/src/gps/gpsClient.c

#include <gps.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>

#include "gpsClient.h"

int runGpsStreamClient() {
    int rc;
    int count = 0;
    clock_t t;

    struct gps_data_t gps_data;
    t = clock();
    if ((rc = gps_open("localhost", "2947", &gps_data)) == -1) {
        printf("code: %d, reason: %s\n", rc, gps_errstr(rc));
        return EXIT_FAILURE;
    }
    t = clock() - t;
    double time_taken = ((double) t) / CLOCKS_PER_SEC; // in seconds
    printf("gps_open() took %f seconds to execute \n", time_taken);

    t = clock();
    gps_stream(&gps_data, WATCH_ENABLE | WATCH_JSON, NULL);
    t = clock() - t;
    time_taken = ((double) t) / CLOCKS_PER_SEC; // in seconds
    printf("gps_stream() took %f seconds to execute \n", time_taken);

    while (count < 60) {
        /* wait for 1 second to receive data */
        if (gps_waiting(&gps_data, 1000000)) {
            /* read data */
            if ((rc = gps_read(&gps_data)) == -1) {
                printf(
                        "error occurred reading gps data. code: %d, reason: %s\n",
                        rc, gps_errstr(rc));
            } else {
                /* Display data from the GPS receiver. */
                double lat = gps_data.fix.latitude;
                double lon = gps_data.fix.longitude;
                double alt = gps_data.fix.altitude;
                double speed = gps_data.fix.speed;
                double climb = gps_data.fix.climb;
                double t = gps_data.fix.time; // EDIT: Replaced tv.tv_sec with gps_data.fix.time
                int status = gps_data.status;
                int mode = gps_data.fix.mode;

                /**
                 * MODE_NOT_SEEN    0   mode update not seen yet
                 * MODE_NO_FIX      1   none
                 * MODE_2D          2   good for latitude/longitude
                 * MODE_3D          3   good for altitude/climb too
                 */
                printf("status: %d - ", status);
                printf("mode: %d - ", mode);
                printf("latitude: %f - ", lat);
                printf("longitude: %f - ", lon);
                printf("altitude: %f - ", alt);
                printf("speed: %f - ", speed);
                printf("vertical speed: %f - ", climb);
                printf("timestamp: %f - ", t);
                printf("%d:%d:%d", (int) (t / 3600), (int) (t / 60), (int) t);

                if ((status == STATUS_FIX)
                        && (mode == MODE_2D || mode == MODE_3D)
                        && !isnan(lat) && !isnan(lon)) {
                    //gettimeofday(&tv, NULL); EDIT: tv.tv_sec isn't actually the timestamp!
                    printf(" =) GPS data correctly received\n");
                } else {
                    printf(" =( NO GPS data received\n");
                }
            }
        } else {
            printf("Timeout to retrieve data from gpsd.");
        }
        count++;
        sleep(1);
    }

    /* When you are done... */
    gps_stream(&gps_data, WATCH_DISABLE, NULL);
    gps_close(&gps_data);

    return EXIT_SUCCESS;
}

ROS gpsd客户端订阅者节点（/ fix）

1 个答案: