void TinkerforgeSensors::publishNavSatFixMessage(SensorDevice *sensor)
{
  uint8_t fix, satellites_view, satellites_used;
  uint16_t pdop, hdop, vdop, epe;
  uint32_t latitude, longitude;
  uint32_t altitude, geoidal_separation;
  uint32_t course, speed;
  char ns, ew;
  if (sensor != NULL)
  {
    // get gps sensor status
    gps_get_status((GPS*)sensor->getDev(), &fix, &satellites_view, &satellites_used);

    if (fix != GPS_FIX_3D_FIX)
      return; // No valid data

    gps_get_coordinates((GPS*)sensor->getDev(), &latitude, &ns, &longitude, &ew, &pdop,
      &hdop, &vdop, &epe);
    gps_get_altitude((GPS*)sensor->getDev(), &altitude, &geoidal_separation);
    // course in deg, speed in 1/100 km/h
    gps_get_motion((GPS*)sensor->getDev(), &course, &speed);

    // generate NavSatFix message from gps sensor data
    sensor_msgs::NavSatFix gps_msg;

    // message header
    gps_msg.header.seq =  sensor->getSeq();
    gps_msg.header.stamp = ros::Time::now();
    gps_msg.header.frame_id = sensor->getFrame();
    // gps status
    gps_msg.status.status = gps_msg.status.STATUS_SBAS_FIX;
    gps_msg.status.service = gps_msg.status.SERVICE_GPS;

    gps_msg.latitude = latitude/1000000.0;
    gps_msg.longitude = longitude/1000000.0;
    gps_msg.altitude = altitude/100.0;
    gps_msg.position_covariance_type = gps_msg.COVARIANCE_TYPE_UNKNOWN;

    // publish gps msg to ros
    sensor->getPub().publish(gps_msg);
  }
}
Beispiel #2
0
void LaserTransform::publishNavSatFixMessage(ros::Publisher *pub_message)
{
  static uint32_t seq = 0;
  uint8_t fix, satellites_view, satellites_used;
  uint16_t pdop, hdop, vdop, epe;
  uint32_t latitude, longitude;
  uint32_t altitude, geoidal_separation;
  uint32_t course, speed;
  char ns, ew;
  if (is_gps_connected)
  {
    // get gps sensor status
    gps_get_status(&gps, &fix, &satellites_view, &satellites_used);

    if (fix != GPS_FIX_3D_FIX)
      return; // No valid data

    gps_get_coordinates(&gps, &latitude, &ns, &longitude, &ew, &pdop,
      &hdop, &vdop, &epe);
    gps_get_altitude(&gps, &altitude, &geoidal_separation);
    // course in deg, speed in 1/100 km/h
    gps_get_motion(&gps, &course, &speed);

    if (this->velocity > 0.0)
    {
      this->velocity_gps = (speed*100) / 3.6; // in m/s
      this->course_gps = deg2rad(course*100);
      ROS_INFO_STREAM("GPS-Velocity:" << this->velocity_gps);
      ROS_INFO_STREAM("GPS-Course:" << this->course_gps);
    }
    else
    {
      this->velocity_gps = 0.0;
      this->course_gps = 0.0;
    }

    // generate NavSatFix message from gps sensor data
    sensor_msgs::NavSatFix gps_msg;

    // message header
    gps_msg.header.seq =  seq;
    gps_msg.header.stamp = ros::Time::now();
    gps_msg.header.frame_id = "gps";
    // gps status
    gps_msg.status.status = gps_msg.status.STATUS_SBAS_FIX;
    gps_msg.status.service = gps_msg.status.SERVICE_GPS;

    gps_msg.latitude = latitude/1000000.0;
    gps_msg.longitude = longitude/1000000.0;
    gps_msg.altitude = altitude/100.0;
    gps_msg.position_covariance_type = gps_msg.COVARIANCE_TYPE_UNKNOWN;

    // log gps data if GPS_LOGFILE is true
    if (gps_log.is_open() && GPS_LOGFILE)
    {
      std::string separator = "|";
      gps_log << ros::Time::now().sec;
      gps_log << gps_log << separator;
      gps_log << gps_msg.latitude;
      gps_log << separator;
      gps_log << gps_msg.longitude;
      gps_log << separator;
      gps_log << gps_msg.altitude;
      gps_log << std::endl;
    }

    // create UTM coordinates from gps data
    // this is the only use of packages geodesy and geographic_msgs
    // if the utm conversion not longer necessary, remove also dependencies from
    // CMakeLists.txt and package.xml and includes
    geographic_msgs::GeoPoint ll;
    ll = geodesy::toMsg(gps_msg);
    geodesy::UTMPoint pt;
    geodesy::fromMsg(ll,pt);
    if (xpos == 0 && ypos == 0)
    {
      xpos = pt.easting;
      ypos = pt.northing;
    }
    else
    {
      //ROS_INFO_STREAM("COORDS_1:" << pt.easting << "::" << pt.northing);
      //ROS_INFO_STREAM("COORDS_2:" << xpos << "::" << ypos);

      //std::cout << "COORDS_3:" << (int)(xpos-pt.easting) << "::" << (int)(ypos-pt.northing) << std::endl;
      //ROS_INFO_STREAM("COORDS_3:" << (int)xpos-pt.easting << "::" << (int)ypos-pt.northing);
    }
    // publish gps msg to ros
    pub_message->publish(gps_msg);

    seq++;
  }
}