Go to the documentation of this file.
35 #include <unordered_map>
44 #include <diagnostic_msgs/DiagnosticArray.h>
45 #include <diagnostic_msgs/DiagnosticStatus.h>
46 #include <geometry_msgs/PoseWithCovarianceStamped.h>
47 #include <geometry_msgs/Quaternion.h>
48 #include <geometry_msgs/TwistWithCovarianceStamped.h>
49 #include <gps_common/GPSFix.h>
50 #include <nav_msgs/Odometry.h>
51 #include <sensor_msgs/Imu.h>
52 #include <sensor_msgs/NavSatFix.h>
53 #include <sensor_msgs/TimeReference.h>
55 #include <septentrio_gnss_driver/AIMPlusStatus.h>
56 #include <septentrio_gnss_driver/AttCovEuler.h>
57 #include <septentrio_gnss_driver/AttEuler.h>
58 #include <septentrio_gnss_driver/BaseVectorCart.h>
59 #include <septentrio_gnss_driver/BaseVectorGeod.h>
60 #include <septentrio_gnss_driver/BlockHeader.h>
61 #include <septentrio_gnss_driver/GALAuthStatus.h>
62 #include <septentrio_gnss_driver/MeasEpoch.h>
63 #include <septentrio_gnss_driver/MeasEpochChannelType1.h>
64 #include <septentrio_gnss_driver/MeasEpochChannelType2.h>
65 #include <septentrio_gnss_driver/PVTCartesian.h>
66 #include <septentrio_gnss_driver/PVTGeodetic.h>
67 #include <septentrio_gnss_driver/PosCovCartesian.h>
68 #include <septentrio_gnss_driver/PosCovGeodetic.h>
69 #include <septentrio_gnss_driver/RFBand.h>
70 #include <septentrio_gnss_driver/RFStatus.h>
71 #include <septentrio_gnss_driver/ReceiverTime.h>
72 #include <septentrio_gnss_driver/VectorInfoCart.h>
73 #include <septentrio_gnss_driver/VectorInfoGeod.h>
74 #include <septentrio_gnss_driver/VelCovCartesian.h>
75 #include <septentrio_gnss_driver/VelCovGeodetic.h>
77 #include <nmea_msgs/Gpgga.h>
78 #include <nmea_msgs/Gpgsa.h>
79 #include <nmea_msgs/Gpgsv.h>
80 #include <nmea_msgs/Gprmc.h>
82 #include <septentrio_gnss_driver/ExtSensorMeas.h>
83 #include <septentrio_gnss_driver/IMUSetup.h>
84 #include <septentrio_gnss_driver/INSNavCart.h>
85 #include <septentrio_gnss_driver/INSNavGeod.h>
86 #include <septentrio_gnss_driver/VelSensorSetup.h>
207 }
catch (
const std::runtime_error& ex)
210 std::string(ex.what()) +
".");
225 uint32_t defaultVal)
const
228 if ((!
pNh_->getParam(name, tempVal)) || (tempVal < 0))
246 template <
typename T>
247 bool param(
const std::string& name, T& val,
const T& defaultVal)
const
249 return pNh_->param(name, val, defaultVal);
292 template <
typename M>
298 it->second.publish(msg);
302 topicMap_.insert(std::make_pair(topic, pub));
313 if (std::isnan(loc.pose.pose.orientation.w))
320 geometry_msgs::TransformStamped transformStamped;
321 transformStamped.header.stamp = loc.header.stamp;
322 transformStamped.header.frame_id = loc.header.frame_id;
323 transformStamped.child_frame_id = loc.child_frame_id;
324 transformStamped.transform.translation.x = loc.pose.pose.position.x;
325 transformStamped.transform.translation.y = loc.pose.pose.position.y;
326 transformStamped.transform.translation.z = loc.pose.pose.position.z;
327 transformStamped.transform.rotation.x = loc.pose.pose.orientation.x;
328 transformStamped.transform.rotation.y = loc.pose.pose.orientation.y;
329 transformStamped.transform.rotation.z = loc.pose.pose.orientation.z;
330 transformStamped.transform.rotation.w = loc.pose.pose.orientation.w;
334 geometry_msgs::TransformStamped T_l_b;
347 <<
": No transform for insertion of local frame at t="
349 <<
". Exception: " << std::string(ex.what()));
358 <<
": No most recent transform for insertion of local frame. Exception: "
359 << std::string(ex.what()));
368 transformStamped.header.stamp = loc.header.stamp;
369 transformStamped.header.frame_id = loc.header.frame_id;
422 const geometry_msgs::TwistWithCovariance& twist)
428 static Eigen::Vector3d vel = Eigen::Vector3d::Zero();
429 static Eigen::Vector3d var = Eigen::Vector3d::Zero();
430 static uint64_t ctr = 0;
434 vel[0] += twist.twist.linear.x;
435 vel[1] += twist.twist.linear.y;
436 vel[2] += twist.twist.linear.z;
437 var[0] += twist.covariance[0];
438 var[1] += twist.covariance[7];
439 var[2] += twist.covariance[14];
442 if ((stamp - lastStamp) >= 495000000)
446 time_t epochSeconds = stamp / 1000000000;
447 struct tm* tm_temp = std::gmtime(&epochSeconds);
448 std::stringstream timeUtc;
449 timeUtc << std::setfill(
'0') << std::setw(2)
450 << std::to_string(tm_temp->tm_hour) << std::setw(2)
451 << std::to_string(tm_temp->tm_min) << std::setw(2)
452 << std::to_string(tm_temp->tm_sec) <<
"." << std::setw(3)
453 << std::to_string((stamp - (stamp / 1000000000) * 1000000000) /
468 else if (var[0] > 0.0)
473 std::to_string(var[0]) +
474 ". Ignoring measurement.");
479 std_x = std::to_string(1000000.0);
488 else if (var[1] > 0.0)
493 std::to_string(var[1]) +
494 ". Ignoring measurement.");
508 else if (var[2] > 0.0)
513 std::to_string(var[2]) +
514 ". Ignoring measurement.");
521 std::string velNmea =
"$PSSN,VSM," + timeUtc.str() +
"," + v_x +
"," +
522 v_y +
"," + std_x +
"," + std_y +
"," + v_z +
"," +
525 char crc = std::accumulate(velNmea.begin() + 1, velNmea.end(), 0,
526 [](
char sum,
char ch) { return sum ^ ch; });
528 std::stringstream crcss;
529 crcss << std::hex << static_cast<int32_t>(
crc);
531 velNmea +=
"*" + crcss.str() +
"\r\n";
534 vel = Eigen::Vector3d::Zero();
535 var = Eigen::Vector3d::Zero();
543 std::shared_ptr<ros::NodeHandle>
pNh_;
547 virtual void sendVelocity(
const std::string& velNmea) = 0;
551 std::unordered_map<std::string, ros::Publisher>
topicMap_;
septentrio_gnss_driver::VectorInfoGeod VectorInfoGeodMsg
tf2_ros::TransformListener tfListener_
void registerSubscriber()
Timestamp timestampFromRos(const TimestampRos &tsr)
Convert ROS timestamp to nsec timestamp.
std::string local_frame_id
Frame id of the local frame to be inserted.
Capabilities capabilities_
nav_msgs::Odometry LocalizationMsg
#define ROS_ERROR_STREAM(args)
nmea_msgs::Gpgsv GpgsvMsg
bool insert_local_frame
Wether local frame should be inserted into tf.
sensor_msgs::NavSatFix NavSatFixMsg
void log(log_level::LogLevel logLevel, const std::string &s) const
Log function to provide abstraction of ROS loggers.
bool has_heading
Wether Rx has heading.
#define ROS_FATAL_STREAM(args)
bool has_improved_vsm_handling
Wether Rx has improved VSM handling.
geometry_msgs::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg
void processTwist(Timestamp stamp, const geometry_msgs::TwistWithCovariance &twist)
septentrio_gnss_driver::BlockHeader BlockHeaderMsg
TimestampRos timestampToRos(Timestamp ts)
Convert nsec timestamp to ROS timestamp.
bool hasImprovedVsmHandling()
Check if Rx has improved VSM handling.
sensor_msgs::NavSatStatus NavSatStatusMsg
ros::Subscriber odometrySubscriber_
Odometry subscriber.
void publishMessage(const std::string &topic, const M &msg)
Publishing function.
septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg
septentrio_gnss_driver::ReceiverTime ReceiverTimeMsg
#define ROS_DEBUG_STREAM(args)
septentrio_gnss_driver::INSNavCart INSNavCartMsg
void publish(const boost::shared_ptr< M > &message) const
This class is the base class for abstraction.
sensor_msgs::TimeReference TimeReferenceMsg
bool ins_vsm_ros_variances_by_parameter
Whether or not to use variance defined by ROS parameter.
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
std::unordered_map< std::string, ros::Publisher > topicMap_
Map of topics and publishers.
septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg
septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg
septentrio_gnss_driver::GALAuthStatus GalAuthStatusMsg
#define ROS_WARN_STREAM(args)
#define ROS_WARN_STREAM_THROTTLE(period, args)
septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg
virtual void sendVelocity(const std::string &velNmea)=0
Send velocity to communication layer (virtual)
std::vector< bool > ins_vsm_ros_config
Whether or not to use individual elements of 3D velocity (v_x, v_y, v_z)
septentrio_gnss_driver::PVTCartesian PVTCartesianMsg
gps_common::GPSStatus GpsStatusMsg
#define ROS_INFO_STREAM_THROTTLE(period, args)
septentrio_gnss_driver::VelCovCartesian VelCovCartesianMsg
bool use_ros_axis_orientation
ROS axis orientation, body: front-left-up, geographic: ENU.
septentrio_gnss_driver::PosCovCartesian PosCovCartesianMsg
std::string trimDecimalPlaces(double num)
Trims decimal places to three.
septentrio_gnss_driver::RFBand RfBandMsg
bool isIns()
Check if Rx is INS.
const Settings * settings() const
septentrio_gnss_driver::AttCovEuler AttCovEulerMsg
nmea_msgs::Gpgga GpggaMsg
void setImprovedVsmHandling()
Set improved VSM handling to true.
void callbackOdometry(const nav_msgs::Odometry::ConstPtr &odo)
gps_common::GPSFix GpsFixMsg
septentrio_gnss_driver::INSNavGeod INSNavGeodMsg
bool getUint32Param(const std::string &name, uint32_t &val, uint32_t defaultVal) const
Gets an integer or unsigned integer value from the parameter server.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
#define ROS_INFO_STREAM(args)
geometry_msgs::TransformStamped TransformStampedMsg
bool hasHeading()
Check if Rx has heading.
diagnostic_msgs::DiagnosticStatus DiagnosticStatusMsg
Settings settings_
Settings.
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
tf2_ros::TransformBroadcaster tf2Publisher_
Transform publisher.
void setIsIns()
Set INS to true.
const ROSCPP_DECL std::string & getName()
Log level for ROS logging.
septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg
ros::Subscriber twistSubscriber_
Twist subscriber.
Timestamp getTime() const
Gets current timestamp.
void callbackTwist(const TwistWithCovarianceStampedMsg::ConstPtr &twist)
Time & fromNSec(uint64_t t)
Declares lower-level string utility functions used when parsing messages.
septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg
bool param(const std::string &name, T &val, const T &defaultVal) const
Gets parameter of type T from the parameter server.
std::string ins_vsm_ros_source
VSM source for INS.
std::shared_ptr< ros::NodeHandle > pNh_
Node handle pointer.
bool is_ins
Wether Rx is INS.
void setHasHeading()
Set has heading to true.
septentrio_gnss_driver::RFStatus RfStatusMsg
nmea_msgs::Gpgsa GpgsaMsg
septentrio_gnss_driver::VectorInfoCart VectorInfoCartMsg
septentrio_gnss_driver::VelSensorSetup VelSensorSetupMsg
septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg
nmea_msgs::Gprmc GprmcMsg
septentrio_gnss_driver::MeasEpoch MeasEpochMsg
tf2_ros::Buffer tfBuffer_
tf buffer
virtual ~ROSaicNodeBase()
std::vector< double > ins_vsm_ros_variances
Variances of the 3D velocity (var_x, var_y, var_z)
septentrio_gnss_driver::AIMPlusStatus AimPlusStatusMsg
geometry_msgs::Quaternion QuaternionMsg
septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg
uint32_t queueSize_
Publisher queue size.
TimestampRos lastTfStamp_
Last tf stamp.
septentrio_gnss_driver::IMUSetup IMUSetupMsg
void publishTf(const LocalizationMsg &loc)
Publishing function for tf.
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout) const
geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg
diagnostic_msgs::DiagnosticArray DiagnosticArrayMsg
septentrio_gnss_driver::AttEuler AttEulerMsg