49 #include <geometry_msgs/TransformStamped.h> 53 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DSystemState.h> 54 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DSatellites.h> 55 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DDetailSatellites.h> 56 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DLocalMagneticField.h> 57 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DUTMPosition.h> 58 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DECEFPos.h> 59 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DNorthSeekingInitStatus.h> 60 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DOdometerState.h> 61 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DRawGNSS.h> 62 #include <kvh_geo_fog_3d_msgs/KvhGeoFog3DRawSensors.h> 65 #include "sensor_msgs/Imu.h" 66 #include "sensor_msgs/NavSatFix.h" 67 #include "sensor_msgs/NavSatStatus.h" 68 #include "sensor_msgs/MagneticField.h" 69 #include "nav_msgs/Odometry.h" 70 #include "geometry_msgs/Quaternion.h" 71 #include "geometry_msgs/Vector3.h" 72 #include "geometry_msgs/Vector3Stamped.h" 73 #include "geometry_msgs/TwistWithCovarianceStamped.h" 125 int filterVehicleType;
126 if (_node.
getParam(
"filterVehicleType", filterVehicleType))
157 int main(
int argc,
char **argv)
159 ros::init(argc, argv,
"kvh_geo_fog_3d_driver");
169 std::map<packet_id_e, ros::Publisher> kvhPubMap{
196 ros::Publisher odomSpeedPub = node.
advertise<geometry_msgs::TwistWithCovarianceStamped>(
"odom/encoder_vehicle_velocity", 1);
201 ros::Publisher velBodyTwistFLUPub = node.
advertise<geometry_msgs::TwistWithCovarianceStamped>(
"imu/vel_flu", 1);
202 ros::Publisher velBodyTwistFRDPub = node.
advertise<geometry_msgs::TwistWithCovarianceStamped>(
"imu/vel_frd", 1);
211 typedef std::pair<packet_id_e, int> freqPair;
234 ROS_ERROR(
"Unable to get init options. Exiting.");
239 if ((errorCode = kvhDriver.
Init(initOptions.
port, packetRequest, initOptions)) < 0)
241 ROS_ERROR(
"Unable to initialize driver. Error Code %d", errorCode);
408 PublishOdomNED(odomPubNED, systemStatePacket, utmPosPacket, eulStdDevPack, bodyVelocityPacket);
409 PublishOdomENU(odomPubENU, systemStatePacket, utmPosPacket, eulStdDevPack, bodyVelocityPacket);
566 diagnostics.
broadcast(diagnostic_msgs::DiagnosticStatus::WARN,
"Shutting down the KVH driver");
void UpdateSystemStatus(diagnostic_updater::DiagnosticStatusWrapper &stat)
void PublishOdomENU(ros::Publisher &, system_state_packet_t, utm_position_packet_t, euler_orientation_standard_deviation_packet_t, body_velocity_packet_t)
KVH Geo Fog 3D driver class header.
union system_state_packet_t::@1 filter_status
void PublishMagField(ros::Publisher &, raw_sensors_packet_t)
Driver worker class for the KVH Geo Fog 3D.
global variables used to store packet information.
void PublishIMUSensorRawFLU(ros::Publisher &, raw_sensors_packet_t)
void PublishOdomSpeed(ros::Publisher &, system_state_packet_t, odometer_state_packet_t, double, double, bool)
void PublishUtmPosition(ros::Publisher &, utm_position_packet_t)
void setHardwareID(const std::string &hwid)
void PublishSystemState(ros::Publisher &, system_state_packet_t)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void PublishIMU_RPY_ENU(ros::Publisher &, system_state_packet_t)
void PublishOdomNED(ros::Publisher &, system_state_packet_t, utm_position_packet_t, euler_orientation_standard_deviation_packet_t, body_velocity_packet_t)
void add(const std::string &name, TaskFunction f)
int Once()
Do a single data read from the device.
int AddPacket(packet_id_e)
void PublishVelENUTwist(ros::Publisher &, system_state_packet_t, velocity_standard_deviation_packet_t)
void PublishRawNavSatFix(ros::Publisher &, system_state_packet_t, raw_gnss_packet_t)
void PublishSatellites(ros::Publisher &, satellites_packet_t)
void SetupUpdater(diagnostic_updater::Updater *_diagnostics, mitre::KVH::DiagnosticsContainer *_diagContainer)
void PublishRawGnss(ros::Publisher &, raw_gnss_packet_t)
void PublishEcefPosition(ros::Publisher &, ecef_position_packet_t)
bool velocityHeadingEnabled
float standard_deviation[3]
bool atmosphericAltitudeEnabled
bool getParam(const std::string &key, std::string &s) const
int GetPacket(packet_id_e _packetId, T &_packet)
Retrieves the requested packets that are currently stored. Use PacketIsUpdated and SetPacketUpdated t...
std::vector< std::pair< packet_id_e, uint16_t > > KvhPacketRequest
bool motionAnalysisEnabled
Contains diagnostic information published out the ROS diagnostics topic.
int RequestPacket(packet_id_e)
This function is used to request packets that you only want once or that cannot be requested through ...
int Init(const std::string &_port, KvhPacketRequest &_packetsRequested)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void PublishSatellitesDetailed(ros::Publisher &, detailed_satellites_packet_t)
void PublishIMU_ENU(ros::Publisher &, system_state_packet_t, euler_orientation_standard_deviation_packet_t)
void PublishKvhOdometerState(ros::Publisher &, odometer_state_packet_t)
bool PacketIsUpdated(packet_id_e)
Use this function to determine if new packet data has arrived since the last time you checked...
void PublishIMURawFLU(ros::Publisher &, system_state_packet_t)
void SetSystemStatus(uint16_t _status)
void PublishIMU_RPY_NED_DEG(ros::Publisher &, system_state_packet_t)
int SetPacketUpdated(packet_id_e, bool)
Use this function to set that the packet has been updated (though the driver will usually do that its...
void PublishIMU_NED(ros::Publisher &, system_state_packet_t, euler_orientation_standard_deviation_packet_t)
void PublishVelNEDTwist(ros::Publisher &, system_state_packet_t, velocity_standard_deviation_packet_t)
#define ROS_INFO_STREAM(args)
void PublishIMU_RPY_NED(ros::Publisher &, system_state_packet_t)
void PublishVelBodyTwistFRD(ros::Publisher &, system_state_packet_t, body_velocity_packet_t, velocity_standard_deviation_packet_t)
uint8_t filterVehicleType
void PublishIMURaw(ros::Publisher &, system_state_packet_t)
void PublishLocalMagnetics(ros::Publisher &, local_magnetics_packet_t)
void PublishOdomState(ros::Publisher &, odometer_state_packet_t, double)
bool reversingDetectionEnabled
void PublishNorthSeekingStatus(ros::Publisher &, north_seeking_status_packet_t)
void UpdateFilterStatus(diagnostic_updater::DiagnosticStatusWrapper &stat)
void broadcast(int lvl, const std::string msg)
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
int Cleanup()
Cleanup and close our connections.
int GetInitOptions(ros::NodeHandle &_node, kvh::KvhInitOptions &_initOptions)
#define ROS_DEBUG_THROTTLE(period,...)
void SetFilterStatus(uint16_t _status)
union system_state_packet_t::@0 system_status
void PublishVelBodyTwistFLU(ros::Publisher &, system_state_packet_t, body_velocity_packet_t, velocity_standard_deviation_packet_t)
void PublishIMU_RPY_ENU_DEG(ros::Publisher &, system_state_packet_t)
void PublishIMUSensorRaw(ros::Publisher &, raw_sensors_packet_t)
double odometerVelocityCovariance
void PublishRawSensors(ros::Publisher &, raw_sensors_packet_t)
void PublishNavSatFix(ros::Publisher &, system_state_packet_t)