41 ROS_DEBUG(
"Called ROSaicNode() constructor..");
61 ros::waitForShutdown();
62 ROS_DEBUG(
"Leaving ROSaicNode() constructor..");
72 ROS_DEBUG(
"Called configureRx() method");
80 IO.
send(
"\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D");
86 IO.
send(
"sso, all, none, none, off \x0D");
89 IO.
send(
"sno, all, none, none, off \x0D");
97 ss <<
"sgd, " <<
datum_ <<
"\x0D";
106 boost::regex_match(
device_, match, boost::regex(
"(tcp|udp)://(.+):(\\d+)"));
107 std::string proto(match[1]);
119 std::string pvt_sec_or_msec;
120 std::string rest_sec_or_msec;
123 else pvt_sec_or_msec =
"msec";
126 else rest_sec_or_msec =
"msec";
131 std::stringstream ss;
133 ss <<
"sno, Stream" << std::to_string(stream) <<
", " << rx_port <<
", GGA, " << pvt_sec_or_msec <<
134 std::to_string(rx_period_pvt) <<
"\x0D";
142 std::stringstream ss;
144 ss <<
"sno, Stream" << std::to_string(stream) <<
", " << rx_port <<
", RMC, " << pvt_sec_or_msec <<
145 std::to_string(rx_period_pvt) <<
"\x0D";
153 std::stringstream ss;
155 ss <<
"sno, Stream" << std::to_string(stream) <<
", " << rx_port <<
", GSA, " << pvt_sec_or_msec <<
156 std::to_string(rx_period_pvt) <<
"\x0D";
164 std::stringstream ss;
166 ss <<
"sno, Stream" << std::to_string(stream) <<
", " << rx_port <<
", GSV, " << rest_sec_or_msec <<
167 std::to_string(rx_period_rest) <<
"\x0D";
175 std::stringstream ss;
176 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", PVTCartesian, " << pvt_sec_or_msec <<
177 std::to_string(rx_period_pvt) <<
"\x0D";
185 std::stringstream ss;
186 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", PVTGeodetic, " << pvt_sec_or_msec <<
187 std::to_string(rx_period_pvt) <<
"\x0D";
195 std::stringstream ss;
196 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", PosCovCartesian, " << pvt_sec_or_msec <<
197 std::to_string(rx_period_pvt) <<
"\x0D";
205 std::stringstream ss;
206 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", PosCovGeodetic, " << pvt_sec_or_msec <<
207 std::to_string(rx_period_pvt) <<
"\x0D";
215 std::stringstream ss;
216 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", AttEuler, " << rest_sec_or_msec <<
217 std::to_string(rx_period_rest) <<
"\x0D";
225 std::stringstream ss;
226 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", AttCovEuler, " << rest_sec_or_msec <<
227 std::to_string(rx_period_rest) <<
"\x0D";
235 std::stringstream ss;
236 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", ChannelStatus, " << pvt_sec_or_msec <<
237 std::to_string(rx_period_pvt) <<
"\x0D";
242 ss.str(std::string());
243 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", MeasEpoch, " << pvt_sec_or_msec <<
244 std::to_string(rx_period_pvt) <<
"\x0D";
249 ss.str(std::string());
250 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", DOP, " << pvt_sec_or_msec <<
251 std::to_string(rx_period_pvt) <<
"\x0D";
256 ss.str(std::string());
257 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", VelCovGeodetic, " << pvt_sec_or_msec <<
258 std::to_string(rx_period_pvt) <<
"\x0D";
266 std::stringstream ss;
267 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", ReceiverStatus, " << rest_sec_or_msec <<
268 std::to_string(rx_period_rest) <<
"\x0D";
273 ss.str(std::string());
274 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", QualityInd, " << rest_sec_or_msec <<
275 std::to_string(rx_period_rest) <<
"\x0D";
280 ss.str(std::string());
281 ss <<
"sso, Stream" << std::to_string(stream) <<
", " << rx_port <<
", ReceiverSetup, " << rest_sec_or_msec <<
282 std::to_string(rx_period_rest) <<
"\x0D";
292 std::stringstream ss;
305 std::stringstream ss;
306 ss <<
"snts, NTR1, off \x0D";
316 else if (
mode_ ==
"Client")
319 std::stringstream ss;
327 else if (
mode_ ==
"Client-Sapcorda")
330 std::stringstream ss;
331 ss <<
"snts, NTR1, Client-Sapcorda, , , , , , , , \x0D";
339 ROS_ERROR(
"Invalid mode specified for NTRIP settings.");
348 std::stringstream ss;
356 std::stringstream ss;
357 ss <<
"sno, Stream" << std::to_string(stream) <<
", IPS1, GGA, " << pvt_sec_or_msec <<
358 std::to_string(rx_period_pvt) <<
" \x0D";
366 std::stringstream ss;
378 ROS_DEBUG(
"Leaving configureRx() method");
383 g_nh->param(
"device",
device_, std::string(
"/dev/ttyACM0"));
396 ROS_ERROR(
"Please specify a valid polling period for PVT-related SBF blocks and NMEA messages.");
403 ROS_ERROR(
"Please specify a valid polling period for PVT-unrelated SBF blocks and NMEA messages.");
407 g_nh->param(
"datum",
datum_, std::string(
"ETRS89"));
415 g_nh->param(
"ntrip_settings/mode",
mode_, std::string(
"off"));
416 g_nh->param(
"ntrip_settings/caster",
caster_, std::string());
418 g_nh->param(
"ntrip_settings/username",
username_, std::string());
419 g_nh->param(
"ntrip_settings/password",
password_, std::string());
422 g_nh->param(
"ntrip_settings/send_gga",
send_gga_, std::string(
"auto"));
424 g_nh->param(
"ntrip_settings/rtcm_version",
rtcm_version_, std::string(
"RTCMv3"));
441 ROS_DEBUG(
"Finished getROSParams() method");
447 ROS_DEBUG(
"Called initializeIO() method");
450 if (boost::regex_match(
device_, match, boost::regex(
"(tcp)://(.+):(\\d+)")))
460 std::string proto(match[1]);
468 temporary_thread.detach();
473 std::stringstream ss;
474 ss <<
"Protocol '" << proto <<
"' is unsupported.";
475 ROS_ERROR(
"%s", ss.str().c_str());
483 temporary_thread.detach();
485 ROS_DEBUG(
"Leaving initializeIO() method");
490 ROS_DEBUG(
"Called connect() method");
491 ROS_DEBUG(
"Setting ROS timer for calling reconnect() method until connection succeds");
494 ROS_DEBUG(
"Started ROS timer for calling reconnect() method until connection succeds");
496 ROS_DEBUG(
"Leaving connect() method");
503 ROS_DEBUG(
"Called reconnect() method");
507 ROS_DEBUG(
"Stopped ROS timer since successully connected.");
513 bool initialize_serial_return =
false;
516 ROS_INFO(
"Connecting serially to device %s, targeted baudrate: %u",
device_.c_str(),
baudrate_);
519 catch (std::runtime_error& e)
522 std::stringstream ss;
523 ss <<
"IO.initializeSerial() failed for device " <<
device_ <<
" due to: " << e.what();
524 ROS_ERROR(
"%s", ss.str().c_str());
527 if (initialize_serial_return)
537 bool initialize_tcp_return =
false;
543 catch (std::runtime_error& e)
546 std::stringstream ss;
547 ss <<
"IO.initializeTCP() failed for host " <<
tcp_host_ <<
" on port " <<
tcp_port_ <<
548 " due to: " << e.what();
549 ROS_ERROR(
"%s", ss.str().c_str());
552 if (initialize_tcp_return)
561 ROS_DEBUG(
"Leaving reconnect() method");
571 ROS_DEBUG(
"Called defineMessages() method");
624 ROS_ERROR(
"For a proper NavSatFix message, please set the publish/pvtgeodetic and the publish/poscovgeodetic ROSaic parameters both to true.");
632 ROS_ERROR(
"For a proper GPSFix message, please set the publish/pvtgeodetic and the publish/poscovgeodetic ROSaic parameters both to true.");
646 ROS_ERROR(
"For a proper PoseWithCovarianceStamped message, please set the publish/pvtgeodetic, publish/poscovgeodetic, publish_atteuler and publish_attcoveuler ROSaic parameters all to true.");
658 ROS_DEBUG(
"Leaving defineMessages() method");
743 boost::shared_ptr<ros::NodeHandle>
g_nh;
747 int main(
int argc,
char** argv)
749 ros::init(argc, argv,
"septentrio_gnss");
750 g_nh.reset(
new ros::NodeHandle(
"~"));
783 if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
784 ros::console::levels::Debug))
785 ros::console::notifyLoggerLevelsChanged();
std::string rtcm_version_
RTCM version for NTRIP service (if Rx does not have internet)
bool g_atteuler_has_arrived_gpsfix
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
uint32_t polling_period_pvt_
Polling period for PVT-related SBF blocks.
boost::shared_ptr< ros::NodeHandle > g_nh
std::string password_
Password for NTRIP service.
std::string g_frame_id
The frame ID used in the header of every published ROS message.
bool publish_attcoveuler_
Whether or not to publish the rosaic::AttCovEuler message.
bool g_channelstatus_has_arrived_gpsfix
For GPSFix: Whether the ChannelStatus block of the current epoch has arrived or not.
boost::mutex connection_mutex_
bool publish_gprmc_
Whether or not to publish the RMC message.
bool serial_
Whether yet-to-be-established connection to Rx will be serial or TCP.
std::map< std::string, uint32_t > g_PoseWithCovarianceStampedMap
A C++ map for keeping track of SBF blocks necessary to construct the PoseWithCovarianceStamped ROS me...
uint32_t g_leap_seconds
The number of leap seconds that have been inserted into the UTC time.
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
bool g_dop_has_arrived_gpsfix
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
std::map< std::string, uint32_t > g_GPSFixMap
A C++ map for keeping track of the SBF blocks necessary to construct the GPSFix ROS message...
boost::mutex g_response_mutex
Mutex to control changes of global variable "g_response_received".
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
uint32_t polling_period_rest_
Polling period for all other SBF blocks and NMEA messages.
bool rx_has_internet_
Whether Rx has internet or not.
bool publish_pvtcartesian_
Whether or not to publish the rosaic::PVTCartesian message.
bool publish_poscovcartesian_
Whether or not to publish the rosaic::PosCovCartesian message.
bool g_attcoveuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttCovEuler block of the current epoch has arrived or not...
CallbackMap insert(std::string message_key)
Adds a pair to the multimap "callbackmap_", with the message_key being the key.
std::string ant_type_
Antenna type, from the list returned by the command "lstAntennaInfo, Overview".
bool initializeTCP(std::string host, std::string port)
Initializes the TCP I/O.
int main(int argc, char **argv)
std::string rx_serial_port_
In case of serial communication to Rx, rx_serial_port_ specifies Rx's serial port connected to...
CallbackHandlers getHandlers() const
bool g_attcoveuler_has_arrived_gpsfix
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
void connect()
Calles the reconnect() method.
void getROSParams()
Gets the node parameters from the ROS Parameter Server, parts of which are specified in a YAML file...
uint32_t rx_input_corrections_tcp_
Rx TCP port number, e.g. 28785, on which Rx receives the corrections (can't be the same as main conne...
bool publish_poscovgeodetic_
Whether or not to publish the rosaic::PosCovGeodetic message.
bool publish_atteuler_
Whether or not to publish the rosaic::AttEuler message.
bool g_qualityind_has_arrived_diagnostics
For DiagnosticArray: Whether the QualityInd block of the current epoch has arrived or not...
float delta_n_
Marker-to-ARP offset in the northward direction.
bool g_publish_diagnostics
Whether or not to publish the diagnostic_msgs::DiagnosticArray message.
bool initializeSerial(std::string port, uint32_t baudrate=115200, std::string flowcontrol="None")
Initializes the serial port.
bool publish_pvtgeodetic_
Whether or not to publish the rosaic::PVTGeodetic message.
void send(std::string cmd)
void defineMessages()
Defines which Rx messages to read and which ROS messages to publish.
void initializeIO()
Initializes the I/O handling.
bool publish_gpgsv_
Whether or not to publish the GSV message.
bool g_publish_pose
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
uint32_t caster_port_
IP port number of NTRIP caster to connect to.
std::string tcp_port_
TCP port number.
boost::condition_variable connection_condition_
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
std::string ant_serial_nr_
Serial number of your particular antenna.
void reconnect(const ros::TimerEvent &event)
Attempts to (re)connect every reconnect_delay_s_ seconds.
uint32_t baudrate_
Baudrate.
std::string username_
Username for NTRIP service.
std::string mountpoint_
Mountpoint for NTRIP service.
std::map< std::string, uint32_t > g_NavSatFixMap
A C++ map for keeping track of the SBF blocks necessary to construct the NavSatFix ROS message...
bool g_poscovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PosCovGeodetic block of the current epoch has arrived or not.
bool g_velcovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the VelCovGeodetic block of the current epoch has arrived or not.
bool publish_gpgga_
Whether or not to publish the GGA message.
bool getROSInt(const std::string &key, U &u)
Gets an integer or unsigned integer value from the parameter server.
float reconnect_delay_s_
Delay in seconds between reconnection attempts to the connection type specified in the parameter conn...
std::string tcp_host_
Host name of TCP server.
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
std::string mode_
Type of NTRIP connection.
std::string send_gga_
Whether (and at which rate) or not to send GGA to the NTRIP caster.
bool g_poscovgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PosCovGeodetic block of the current epoch has arrived or not...
uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a uint32_t number that can be appended to eit...
bool connected_
Whether connecting to Rx was successful.
bool g_atteuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttEuler block of the current epoch has arrived or not...
bool g_measepoch_has_arrived_gpsfix
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
void configureRx()
Configures Rx: Which SBF/NMEA messages it should output and later correction settings.
bool g_publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
bool g_pvtgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
float delta_e_
Marker-to-ARP offset in the eastward direction.
std::string device_
Device port.
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
std::string rx_input_corrections_serial_
Rx serial port, e.g. USB2, on which Rx receives the corrections (can't be the same as main connection...
bool g_publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
bool g_receiverstatus_has_arrived_diagnostics
For DiagnosticArray: Whether the ReceiverStatus block of the current epoch has arrived or not...
std::string trimString(std::string str)
Removes trailing zeros from a string representing a float or double except for the first zero after t...
bool publish_gpgsa_
Whether or not to publish the GSA message.
std::string datum_
Datum to be used.
std::string caster_
Hostname or IP address of the NTRIP caster to connect to.
bool g_poscovgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the current epoch has arrived or n...
ros::Timer reconnect_timer_
Our ROS timer governing the reconnection.
std::map< std::string, uint32_t > g_DiagnosticArrayMap
A C++ map for keeping track of SBF blocks necessary to construct the DiagnosticArray ROS message...
std::string ntrip_version_
NTRIP version for NTRIP service.
This class represents the ROsaic node, to be extended..
bool g_response_received
Determines whether a command reply was received from the Rx.
bool g_pvtgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the current epoch has arrived or not...
const uint32_t g_ROS_QUEUE_SIZE
Queue size for ROS publishers.
bool g_publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
The heart of the ROSaic driver: The ROS node that represents it.
io_comm_rx::Comm_IO IO
Handles communication with the Rx.
float delta_u_
Marker-to-ARP offset in the upward direction.
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
std::string hw_flow_control_
HW flow control.