Add support for VINT networkhub (#172)

mintar · web-flow · commit 6a6b81ea9ab4 · 2024-03-11T13:04:23.000+01:00
This is a port of #127 to ROS2. Closes #135.
diff --git a/phidgets_accelerometer/include/phidgets_accelerometer/accelerometer_ros_i.hpp b/phidgets_accelerometer/include/phidgets_accelerometer/accelerometer_ros_i.hpp
@@ -61,6 +61,8 @@ class AccelerometerRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     rclcpp::Time ros_time_zero_;
     bool synchronize_timestamps_{true};
diff --git a/phidgets_accelerometer/src/accelerometer_ros_i.cpp b/phidgets_accelerometer/src/accelerometer_ros_i.cpp
@@ -90,6 +90,20 @@ AccelerometerRosI::AccelerometerRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(
         get_logger(),
         "Connecting to Phidgets Accelerometer serial %d, hub port %d ...",
diff --git a/phidgets_analog_inputs/include/phidgets_analog_inputs/analog_inputs_ros_i.hpp b/phidgets_analog_inputs/include/phidgets_analog_inputs/analog_inputs_ros_i.hpp
@@ -61,6 +61,8 @@ class AnalogInputsRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     void publishLatest(int index);
 
diff --git a/phidgets_analog_inputs/src/analog_inputs_ros_i.cpp b/phidgets_analog_inputs/src/analog_inputs_ros_i.cpp
@@ -67,6 +67,20 @@ AnalogInputsRosI::AnalogInputsRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(
         get_logger(),
         "Connecting to Phidgets AnalogInputs serial %d, hub port %d ...",
diff --git a/phidgets_analog_outputs/include/phidgets_analog_outputs/analog_outputs_ros_i.hpp b/phidgets_analog_outputs/include/phidgets_analog_outputs/analog_outputs_ros_i.hpp
@@ -68,6 +68,8 @@ class AnalogOutputsRosI final : public rclcpp::Node
     std::vector<std::unique_ptr<AnalogOutputSetter>> out_subs_;
 
     rclcpp::Service<phidgets_msgs::srv::SetAnalogOutput>::SharedPtr out_srv_;
+    std::string server_name_;
+    std::string server_ip_;
 
     bool setSrvCallback(
         const std::shared_ptr<phidgets_msgs::srv::SetAnalogOutput::Request> req,
diff --git a/phidgets_analog_outputs/src/analog_outputs_ros_i.cpp b/phidgets_analog_outputs/src/analog_outputs_ros_i.cpp
@@ -49,6 +49,20 @@ AnalogOutputsRosI::AnalogOutputsRosI(const rclcpp::NodeOptions& options)
     int serial_num =
         this->declare_parameter("serial", -1);  // default open any device
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     int hub_port = this->declare_parameter(
         "hub_port", 0);  // only used if the device is on a VINT hub_port
 
diff --git a/phidgets_digital_inputs/include/phidgets_digital_inputs/digital_inputs_ros_i.hpp b/phidgets_digital_inputs/include/phidgets_digital_inputs/digital_inputs_ros_i.hpp
@@ -59,6 +59,8 @@ class DigitalInputsRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     void publishLatest(int index);
 
diff --git a/phidgets_digital_inputs/src/digital_inputs_ros_i.cpp b/phidgets_digital_inputs/src/digital_inputs_ros_i.cpp
@@ -65,6 +65,20 @@ DigitalInputsRosI::DigitalInputsRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(
         get_logger(),
         "Connecting to Phidgets DigitalInputs serial %d, hub port %d ...",
diff --git a/phidgets_digital_outputs/include/phidgets_digital_outputs/digital_outputs_ros_i.hpp b/phidgets_digital_outputs/include/phidgets_digital_outputs/digital_outputs_ros_i.hpp
@@ -68,6 +68,8 @@ class DigitalOutputsRosI final : public rclcpp::Node
     std::vector<std::unique_ptr<DigitalOutputSetter>> out_subs_;
 
     rclcpp::Service<phidgets_msgs::srv::SetDigitalOutput>::SharedPtr out_srv_;
+    std::string server_name_;
+    std::string server_ip_;
 
     void setSrvCallback(
         const std::shared_ptr<phidgets_msgs::srv::SetDigitalOutput::Request>
diff --git a/phidgets_digital_outputs/src/digital_outputs_ros_i.cpp b/phidgets_digital_outputs/src/digital_outputs_ros_i.cpp
@@ -48,6 +48,20 @@ DigitalOutputsRosI::DigitalOutputsRosI(const rclcpp::NodeOptions& options)
 
     RCLCPP_INFO(get_logger(), "Starting Phidgets Digital Outputs");
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     int serial_num =
         this->declare_parameter("serial", -1);  // default open any device
 
diff --git a/phidgets_gyroscope/include/phidgets_gyroscope/gyroscope_ros_i.hpp b/phidgets_gyroscope/include/phidgets_gyroscope/gyroscope_ros_i.hpp
@@ -63,6 +63,8 @@ class GyroscopeRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     rclcpp::Time ros_time_zero_;
     bool synchronize_timestamps_{true};
diff --git a/phidgets_gyroscope/src/gyroscope_ros_i.cpp b/phidgets_gyroscope/src/gyroscope_ros_i.cpp
@@ -91,6 +91,20 @@ GyroscopeRosI::GyroscopeRosI(const rclcpp::NodeOptions &options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(get_logger(),
                 "Connecting to Phidgets Gyroscope serial %d, hub port %d ...",
                 serial_num, hub_port);
diff --git a/phidgets_high_speed_encoder/include/phidgets_high_speed_encoder/high_speed_encoder_ros_i.hpp b/phidgets_high_speed_encoder/include/phidgets_high_speed_encoder/high_speed_encoder_ros_i.hpp
@@ -76,6 +76,8 @@ class HighSpeedEncoderRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     /// Publish the latest state for all encoder channels:
     void publishLatest();
diff --git a/phidgets_high_speed_encoder/src/high_speed_encoder_ros_i.cpp b/phidgets_high_speed_encoder/src/high_speed_encoder_ros_i.cpp
@@ -70,6 +70,20 @@ HighSpeedEncoderRosI::HighSpeedEncoderRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(get_logger(),
                 "Connecting to Phidgets Encoders serial %d, hub port %d ...",
                 serial_num, hub_port);
diff --git a/phidgets_magnetometer/include/phidgets_magnetometer/magnetometer_ros_i.hpp b/phidgets_magnetometer/include/phidgets_magnetometer/magnetometer_ros_i.hpp
@@ -60,6 +60,8 @@ class MagnetometerRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     rclcpp::Time ros_time_zero_;
     bool synchronize_timestamps_{true};
diff --git a/phidgets_magnetometer/src/magnetometer_ros_i.cpp b/phidgets_magnetometer/src/magnetometer_ros_i.cpp
@@ -87,6 +87,20 @@ MagnetometerRosI::MagnetometerRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     // compass correction params (see
     // http://www.phidgets.com/docs/1044_User_Guide)
     this->declare_parameter("cc_mag_field", rclcpp::PARAMETER_DOUBLE);
diff --git a/phidgets_motors/include/phidgets_motors/motors_ros_i.hpp b/phidgets_motors/include/phidgets_motors/motors_ros_i.hpp
@@ -76,6 +76,8 @@ class MotorsRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     void publishLatestDutyCycle(int index);
     void publishLatestBackEMF(int index);
diff --git a/phidgets_motors/src/motors_ros_i.cpp b/phidgets_motors/src/motors_ros_i.cpp
@@ -65,6 +65,20 @@ MotorsRosI::MotorsRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(get_logger(),
                 "Connecting to Phidgets Motors serial %d, hub port %d ...",
                 serial_num, hub_port);
diff --git a/phidgets_spatial/include/phidgets_spatial/spatial_ros_i.hpp b/phidgets_spatial/include/phidgets_spatial/spatial_ros_i.hpp
@@ -64,6 +64,8 @@ class SpatialRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     rclcpp::Time ros_time_zero_;
     bool synchronize_timestamps_{true};
diff --git a/phidgets_spatial/src/spatial_ros_i.cpp b/phidgets_spatial/src/spatial_ros_i.cpp
@@ -162,6 +162,20 @@ SpatialRosI::SpatialRosI(const rclcpp::NodeOptions &options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     // compass correction params (see
     // http://www.phidgets.com/docs/1044_User_Guide)
     this->declare_parameter("cc_mag_field", rclcpp::PARAMETER_DOUBLE);
diff --git a/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.hpp b/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.hpp
@@ -55,6 +55,8 @@ class TemperatureRosI final : public rclcpp::Node
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
+    std::string server_name_;
+    std::string server_ip_;
 
     void publishLatest();
 
diff --git a/phidgets_temperature/src/temperature_ros_i.cpp b/phidgets_temperature/src/temperature_ros_i.cpp
@@ -63,6 +63,20 @@ TemperatureRosI::TemperatureRosI(const rclcpp::NodeOptions& options)
         throw std::runtime_error("Publish rate must be <= 1000");
     }
 
+    this->declare_parameter("server_name",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    this->declare_parameter("server_ip",
+                            rclcpp::ParameterType::PARAMETER_STRING);
+    if (this->get_parameter("server_name", server_name_) &&
+        this->get_parameter("server_ip", server_ip_))
+    {
+        PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "",
+                             0);
+
+        RCLCPP_INFO(get_logger(), "Using phidget server %s at IP %s",
+                    server_name_.c_str(), server_ip_.c_str());
+    }
+
     RCLCPP_INFO(get_logger(),
                 "Connecting to Phidgets Temperature serial %d, hub port %d, "
                 "thermocouple "
