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);
}
}
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++;
}
}