Handle connection failures in driver constructor (#46)

* Throw exceptions in c'tor * Update README.md * Suppress warnings * Connection check * Fix driver and docs * Remove empty spaces
fixposition · Jan 26, 2024 · 10b6b4f · 10b6b4f
1 parent d6c339f
commit 10b6b4f
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 23 deletions.
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -16,6 +16,7 @@
 #include <arpa/inet.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <stdexcept>
 
 /* PACKAGE */
 #include <fixposition_driver_lib/converter/imu.hpp>
@@ -44,7 +45,16 @@
 
 namespace fixposition {
 FixpositionDriver::FixpositionDriver(const FixpositionDriverParams& params) : params_(params) {
-    Connect();
+    // connect to the sensor
+    if (!Connect()) {
+        if (params_.fp_output.type == INPUT_TYPE::TCP) {
+            throw std::runtime_error("Unable to connect to the sensor via TCP");
+        } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) {
+            throw std::runtime_error("Unable to connect to the sensor via Serial");
+        } else {
+            throw std::runtime_error("Unable to connect to the sensor, verify configuration");
+        }
+    }
 
     // static headers
     rawdmi_.head1 = 0xaa;
@@ -63,7 +73,7 @@ FixpositionDriver::FixpositionDriver(const FixpositionDriverParams& params) : pa
 
     // initialize converters
     if (!InitializeConverters()) {
-        std::cerr << "Could not initialize output converter!\n";
+        throw std::runtime_error("Could not initialize output converter!");
     }
 }
 
@@ -121,7 +131,8 @@ void FixpositionDriver::WsCallback(const std::vector<int>& speeds) {
             send(this->client_fd_, &message[0], sizeof(message), MSG_DONTWAIT);
             break;
         case INPUT_TYPE::SERIAL:
-            write(this->client_fd_, &message[0], sizeof(message));
+            (void)!write(this->client_fd_, &message[0], sizeof(message));
+            // Suppress warning - https://stackoverflow.com/a/64407070/7944565
             break;
         default:
             std::cerr << "Unknown connection type!\n";
@@ -276,7 +287,11 @@ void FixpositionDriver::NovConvertAndPublish(const uint8_t* msg, int size) {
 }
 
 bool FixpositionDriver::CreateTCPSocket() {
-    struct sockaddr_in server_address;
+    if (client_fd_ != -1) {
+        std::cerr << "TCP connection already exists" << "\n";
+        return true;
+    }
+
     client_fd_ = socket(AF_INET, SOCK_STREAM, 0);
 
     if (client_fd_ < 0) {
@@ -286,6 +301,7 @@ bool FixpositionDriver::CreateTCPSocket() {
         std::cout << "Client created.\n";
     }
 
+    struct sockaddr_in server_address;
     server_address.sin_family = AF_INET;
     server_address.sin_addr.s_addr = INADDR_ANY;
     server_address.sin_port = htons(std::stoi(params_.fp_output.port));
@@ -301,6 +317,11 @@ bool FixpositionDriver::CreateTCPSocket() {
 }
 
 bool FixpositionDriver::CreateSerialConnection() {
+    if (client_fd_ != -1) {
+        std::cerr << "Serial connection already exists" << "\n";
+        return true;
+    }
+
     client_fd_ = open(params_.fp_output.port.c_str(), O_RDWR | O_NOCTTY);
 
     struct termios options;

diff --git a/fixposition_driver_ros1/README.md b/fixposition_driver_ros1/README.md
@@ -7,7 +7,8 @@
 -  [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page), tested with version [3.3.7](https://gitlab.com/libeigen/eigen/-/releases/3.3.7)
 -  [Boost](https://www.boost.org/), tested with version [1.65.0](https://www.boost.org/users/history/version_1_65_0.html)
 -  [CMake](https://cmake.org/)
--  [Transforms] (http://wiki.ros.org/tf)
+-  [tf](http://wiki.ros.org/tf) ROS1 library
+-  [eigen_conversions](https://wiki.ros.org/eigen_conversions) ROS1 library
 -  [Catkin](http://wiki.ros.org/catkin) for ROS1
 
 -  **[fixposition_gnss_tf](https://github.com/fixposition/fixposition_gnss_tf)**: Fixposition GNSS Transformation Lib
@@ -21,6 +22,7 @@ This driver operates as a ROS node, connecting to either a TCP or serial stream
  sudo apt update
  sudo apt install -y build-essential cmake
  sudo apt install -y libeigen3-dev
+ sudo apt install -y ros-{ROS_DISTRO}-tf ros-{ROS_DISTRO}-eigen-conversions
 ```
 
 
@@ -43,7 +45,7 @@ make sure you have sourced the setup.bash from ros:
 `/opt/ros/{ROS_DISTRO}/setup.bash`, for example
 
 ```
-source /opt/ros/melodic/setup.bash`
+source /opt/ros/melodic/setup.bash
 ```
 
 and build it with:

diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -52,14 +52,13 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       odometry_enu0_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 100)),
       eul_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/ypr", 100)),
       eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/imu_ypr", 100)) {
-    
+
     ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 100,
                                                             &FixpositionDriverNode::WsCallback, this,
                                                             ros::TransportHints().tcpNoDelay());
 
-
 
-    Connect();
+
     RegisterObservers();
 }
 
@@ -199,27 +198,27 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
     if (data.checkEpoch()) {
         // Generate new message
         fixposition_driver_ros1::NMEA msg;
-        
+
         // ROS Header
         if (data.gpzda.stamp.tow == 0.0 && data.gpzda.stamp.wno == 0) {
             msg.header.stamp = ros::Time::now();
         } else {
             msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
         }
         msg.header.frame_id = "LLH";
-        
+
         // Latitude [degrees]. Positive is north of equator; negative is south
         msg.latitude = data.gpgga.latitude;
-        
+
         // Longitude [degrees]. Positive is east of prime meridian; negative is west
         msg.longitude = data.gpgga.longitude;
-        
+
         // Altitude [m]. Positive is above the WGS 84 ellipsoid
         msg.altitude = data.gpgga.altitude;
 
         // Speed over ground [m/s]
         msg.speed = data.gprmc.speed;
-        
+
         // Course over ground [deg]
         msg.course = data.gprmc.course;
 
@@ -232,7 +231,7 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
 
         // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
         msg.mode = data.gprmc.mode;
-        
+
         // Publish message
         nmea_pub_.publish(msg);
     }

diff --git a/fixposition_driver_ros2/README.md b/fixposition_driver_ros2/README.md
@@ -21,6 +21,7 @@ This driver operates as a ROS node, connecting to either a TCP or serial stream
  sudo apt update
  sudo apt install -y build-essential cmake
  sudo apt install -y libeigen3-dev
+ sudo apt install -y libboost-date-time-dev 
 ```
 
 
@@ -34,7 +35,7 @@ fp_public_ws
 ├── src
 │   ├── fixposition_driver
 │   │   ├── fixposition_driver_lib
-│   │   ├── fixposition_driver_ros1 # will be ignore by colcon when building for ROS2
+│   │   ├── fixposition_driver_ros1 # will be ignored by colcon when building for ROS2
 │   │   ├── fixposition_driver_ros2
 │   ├── fixposition_gnss_tf
 ```

diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -59,7 +59,6 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
         params_.customer_input.speed_topic, 100,
         std::bind(&FixpositionDriverNode::WsCallback, this, std::placeholders::_1));
 
-    Connect();
     RegisterObservers();
 }
 
@@ -224,27 +223,27 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
     if (data.checkEpoch()) {
         // Generate new message
         fixposition_driver_ros2::msg::NMEA msg;
-        
+
         // ROS Header
         if (data.gpzda.stamp.tow == 0.0 && data.gpzda.stamp.wno == 0) {
             msg.header.stamp = rclcpp::Clock().now();
         } else {
             msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp);
         }
         msg.header.frame_id = "LLH";
-        
+
         // Latitude [degrees]. Positive is north of equator; negative is south
         msg.latitude = data.gpgga.latitude;
-        
+
         // Longitude [degrees]. Positive is east of prime meridian; negative is west
         msg.longitude = data.gpgga.longitude;
-        
+
         // Altitude [m]. Positive is above the WGS 84 ellipsoid
         msg.altitude = data.gpgga.altitude;
 
         // Speed over ground [m/s]
         msg.speed = data.gprmc.speed;
-        
+
         // Course over ground [deg]
         msg.course = data.gprmc.course;
 
@@ -257,7 +256,7 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
 
         // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
         msg.mode = data.gprmc.mode;
-        
+
         // Publish message
         nmea_pub_->publish(msg);
     }