From fa3a5ecdf03cc3853216b815fabc5ec8f8f53fdb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 16 Feb 2024 19:26:15 +0100 Subject: [PATCH] Add support for VINT networkhub This is a port of #127 to ROS2. Closes #135. --- .../phidgets_accelerometer/accelerometer_ros_i.hpp | 2 ++ phidgets_accelerometer/src/accelerometer_ros_i.cpp | 14 ++++++++++++++ .../phidgets_analog_inputs/analog_inputs_ros_i.hpp | 2 ++ phidgets_analog_inputs/src/analog_inputs_ros_i.cpp | 14 ++++++++++++++ .../analog_outputs_ros_i.hpp | 2 ++ .../src/analog_outputs_ros_i.cpp | 14 ++++++++++++++ .../digital_inputs_ros_i.hpp | 2 ++ .../src/digital_inputs_ros_i.cpp | 14 ++++++++++++++ .../digital_outputs_ros_i.hpp | 2 ++ .../src/digital_outputs_ros_i.cpp | 14 ++++++++++++++ .../include/phidgets_gyroscope/gyroscope_ros_i.hpp | 2 ++ phidgets_gyroscope/src/gyroscope_ros_i.cpp | 14 ++++++++++++++ .../high_speed_encoder_ros_i.hpp | 2 ++ .../src/high_speed_encoder_ros_i.cpp | 14 ++++++++++++++ .../phidgets_magnetometer/magnetometer_ros_i.hpp | 2 ++ phidgets_magnetometer/src/magnetometer_ros_i.cpp | 14 ++++++++++++++ .../include/phidgets_motors/motors_ros_i.hpp | 2 ++ phidgets_motors/src/motors_ros_i.cpp | 14 ++++++++++++++ .../include/phidgets_spatial/spatial_ros_i.hpp | 2 ++ phidgets_spatial/src/spatial_ros_i.cpp | 14 ++++++++++++++ .../phidgets_temperature/temperature_ros_i.hpp | 2 ++ phidgets_temperature/src/temperature_ros_i.cpp | 14 ++++++++++++++ 22 files changed, 176 insertions(+) diff --git a/phidgets_accelerometer/include/phidgets_accelerometer/accelerometer_ros_i.hpp b/phidgets_accelerometer/include/phidgets_accelerometer/accelerometer_ros_i.hpp index 9f917f25..e6d9e04d 100644 --- 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 index d185882f..85363fa4 100644 --- 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 index 2fa43ca2..aece3f37 100644 --- 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 index e63683fa..a8ae64ba 100644 --- 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 index cd428a5e..9b9878bc 100644 --- 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> out_subs_; rclcpp::Service::SharedPtr out_srv_; + std::string server_name_; + std::string server_ip_; bool setSrvCallback( const std::shared_ptr req, diff --git a/phidgets_analog_outputs/src/analog_outputs_ros_i.cpp b/phidgets_analog_outputs/src/analog_outputs_ros_i.cpp index 6fa24b4f..da61cf12 100644 --- 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 index 6dbf4d7e..abc929f1 100644 --- 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 index 6afe7166..cf6124c3 100644 --- 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 index 21ced612..4b026d4a 100644 --- 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> out_subs_; rclcpp::Service::SharedPtr out_srv_; + std::string server_name_; + std::string server_ip_; void setSrvCallback( const std::shared_ptr diff --git a/phidgets_digital_outputs/src/digital_outputs_ros_i.cpp b/phidgets_digital_outputs/src/digital_outputs_ros_i.cpp index 990b5b55..273073ec 100644 --- 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 index a1b854f5..d5a38973 100644 --- 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 index 7a63b67f..48a2a7d2 100644 --- 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 index a151f31e..aad8b9d2 100644 --- 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 index 77f1a1dc..1468bf29 100644 --- 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 index 81f8b65a..cbaa78e4 100644 --- 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 index ed7aec3c..eca4e9b9 100644 --- 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 index 27454bce..7a900ef2 100644 --- 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 index 67115e01..e6d1311c 100644 --- 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 index a754d4ed..572a41c1 100644 --- 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 index d797bea8..c232f410 100644 --- 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 index 9211a4a2..34b43d99 100644 --- 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 index 92fb9f56..17793f67 100644 --- 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 "