From 2c1583bbae2b7d090fa8e4a79d732bb5022f3853 Mon Sep 17 00:00:00 2001 From: James Haley Date: Thu, 20 Jan 2022 19:37:51 -0500 Subject: [PATCH 1/2] Adding remote server access to temperature --- .../include/phidgets_temperature/temperature_ros_i.h | 2 ++ phidgets_temperature/launch/temperature.launch | 6 ++++++ phidgets_temperature/src/temperature_ros_i.cpp | 10 ++++++++++ 3 files changed, 18 insertions(+) diff --git a/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h b/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h index 8eb26da5..ffeb4e1d 100644 --- a/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h +++ b/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h @@ -56,6 +56,8 @@ class TemperatureRosI final void timerCallback(const ros::TimerEvent& event); ros::Timer timer_; int publish_rate_; + std::string server_name_; + std::string server_ip_; void publishLatest(); diff --git a/phidgets_temperature/launch/temperature.launch b/phidgets_temperature/launch/temperature.launch index ec370f74..9cc03b32 100644 --- a/phidgets_temperature/launch/temperature.launch +++ b/phidgets_temperature/launch/temperature.launch @@ -29,6 +29,12 @@ # optional param publish_rate, defaults to 0 + + # optional param server_name, required to locate remote network servers + + + # optional param server_ip, required to locate remote network servers + diff --git a/phidgets_temperature/src/temperature_ros_i.cpp b/phidgets_temperature/src/temperature_ros_i.cpp index edc1c7ae..f9ea2982 100644 --- a/phidgets_temperature/src/temperature_ros_i.cpp +++ b/phidgets_temperature/src/temperature_ros_i.cpp @@ -67,6 +67,16 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) { publish_rate_ = 0; } + + if (nh_private.getParam("server_name", server_name_) + && nh_private.getParam("server_ip", server_ip_)){ + + PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "", 0); + + ROS_INFO( + "Using phidget server %s at IP %s", + server_name_.c_str(), server_ip_.c_str()); + } ROS_INFO( "Connecting to Phidgets Temperature serial %d, hub port %d, " From 4750d8c713d1bdac657b246db0c95aab1f8f9328 Mon Sep 17 00:00:00 2001 From: James Haley <2vh@ornl.gov> Date: Fri, 8 Apr 2022 18:29:36 -0400 Subject: [PATCH 2/2] Added channel parameter and sensor_msgs topic --- .../include/phidgets_api/temperature.h | 2 +- phidgets_api/src/temperature.cpp | 4 +- phidgets_temperature/README.md | 4 +- .../phidgets_temperature/temperature_ros_i.h | 2 + .../launch/temperature.launch | 8 +++- .../src/temperature_ros_i.cpp | 44 +++++++++++-------- 6 files changed, 40 insertions(+), 24 deletions(-) diff --git a/phidgets_api/include/phidgets_api/temperature.h b/phidgets_api/include/phidgets_api/temperature.h index 6a61b987..191aaeca 100644 --- a/phidgets_api/include/phidgets_api/temperature.h +++ b/phidgets_api/include/phidgets_api/temperature.h @@ -51,7 +51,7 @@ class Temperature final PHIDGET22_NO_COPY_NO_MOVE_NO_ASSIGN(Temperature) explicit Temperature(int32_t serial_number, int hub_port, - bool is_hub_port_device, + bool is_hub_port_device, int channel, std::function temperature_handler); ~Temperature(); diff --git a/phidgets_api/src/temperature.cpp b/phidgets_api/src/temperature.cpp index 5e1bbfdb..970629a4 100644 --- a/phidgets_api/src/temperature.cpp +++ b/phidgets_api/src/temperature.cpp @@ -38,7 +38,7 @@ namespace phidgets { Temperature::Temperature(int32_t serial_number, int hub_port, - bool is_hub_port_device, + bool is_hub_port_device, int channel, std::function temperature_handler) : temperature_handler_(temperature_handler) { @@ -51,7 +51,7 @@ Temperature::Temperature(int32_t serial_number, int hub_port, helpers::openWaitForAttachment( reinterpret_cast(temperature_handle_), serial_number, - hub_port, is_hub_port_device, 0); + hub_port, is_hub_port_device, channel); ret = PhidgetTemperatureSensor_setOnTemperatureChangeHandler( temperature_handle_, TemperatureChangeHandler, this); diff --git a/phidgets_temperature/README.md b/phidgets_temperature/README.md index eb355334..5a6cda66 100644 --- a/phidgets_temperature/README.md +++ b/phidgets_temperature/README.md @@ -5,12 +5,14 @@ This is the ROS driver for Phidgets temperature. The various topics, services, Topics ------ -* `/temperature` (`std_msgs/Float64`) - The current temperature in degrees Celsius. +* `/temperature_{channel}` (`sensor_msgs/Temperature`) - The current temperature in degrees Celsius, with timestamp and sequence. +* `/temperature` (`std_msgs/Float64`) - The current temperature in degrees Celsius. Preserved for legacy. Parameters ---------- * `serial` (int) - The serial number of the phidgets gyroscope to connect to. If -1 (the default), connects to any gyroscope phidget that can be found. * `hub_port` (int) - The phidgets VINT hub port to connect to. Only used if the gyroscope phidget is connected to a VINT hub. Defaults to 0. +* `channel` (int) - The thermocouple reader channel (physical port). Multiple channels are launched in seperate nodelets. Defaults to 0. * `thermocouple_type` (int) - The type of thermocouple that is connected. Allowed values are 0 (for not a thermocouple, the default), 1 (for a J-Type), 2 (for a K-Type), 3 (for an E-Type), or 4 (for a T-Type). See https://www.phidgets.com/docs/Thermocouple_Primer for more information. * `data_interval_ms` (int) - The number of milliseconds between acquisitions of data on the device. Defaults to 250 ms. * `publish_rate` (int) - How often the driver will publish data on the ROS topic. If 0 (the default), it will publish every time there is an update from the device (so at the `data_interval_ms`). If positive, it will publish the data at that rate regardless of the acquisition interval. diff --git a/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h b/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h index ffeb4e1d..708f50aa 100644 --- a/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h +++ b/phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h @@ -53,6 +53,7 @@ class TemperatureRosI final ros::NodeHandle nh_; ros::NodeHandle nh_private_; ros::Publisher temperature_pub_; + ros::Publisher temperature_sensor_pub_; void timerCallback(const ros::TimerEvent& event); ros::Timer timer_; int publish_rate_; @@ -60,6 +61,7 @@ class TemperatureRosI final std::string server_ip_; void publishLatest(); + void publishLatestTemperature(); void temperatureChangeCallback(double temperature); }; diff --git a/phidgets_temperature/launch/temperature.launch b/phidgets_temperature/launch/temperature.launch index 9cc03b32..42442c41 100644 --- a/phidgets_temperature/launch/temperature.launch +++ b/phidgets_temperature/launch/temperature.launch @@ -7,7 +7,7 @@ - #### Gyroscope Driver ##################################################### + #### Driver ############################################################### --> + # optional param channel, defaults to 0, used if multiple reader ports are available + # Launch seperate nodelets for seperate channels + + # optional param thermocouple_type, defaults is 0 # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms) - + # optional param publish_rate, defaults to 0 diff --git a/phidgets_temperature/src/temperature_ros_i.cpp b/phidgets_temperature/src/temperature_ros_i.cpp index 564f17fe..015f07ad 100644 --- a/phidgets_temperature/src/temperature_ros_i.cpp +++ b/phidgets_temperature/src/temperature_ros_i.cpp @@ -33,6 +33,7 @@ #include #include +#include #include "phidgets_temperature/temperature_ros_i.h" @@ -53,6 +54,11 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) { hub_port = 0; // only used if the device is on a VINT hub_port } + int channel; + if (!nh_private.getParam("channel", channel)) + { + channel = 0; // for multi-input temperature devices + } int thermocouple_type; if (!nh_private.getParam("thermocouple_type", thermocouple_type)) { @@ -61,7 +67,7 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) int data_interval_ms; if (!nh_private.getParam("data_interval_ms", data_interval_ms)) { - data_interval_ms = 500; + data_interval_ms = 250; } if (!nh_private.getParam("publish_rate", publish_rate_)) { @@ -78,21 +84,10 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) server_name_.c_str(), server_ip_.c_str()); } - if (nh_private.getParam("server_name", server_name_) - && nh_private.getParam("server_ip", server_ip_)){ - - PhidgetNet_addServer(server_name_.c_str(), server_ip_.c_str(), 5661, "", 0); - ROS_INFO( - "Using phidget server %s at IP %s", - server_name_.c_str(), server_ip_.c_str()); - } - - ROS_INFO( - "Connecting to Phidgets Temperature serial %d, hub port %d, " - "thermocouple " - "type %d ...", - serial_num, hub_port, thermocouple_type); + "Connecting to Phidgets Temperature serial %d, hub port %d, channel %d, " + "thermocouple type %d ...", + serial_num, hub_port, channel, thermocouple_type); // We take the mutex here and don't unlock until the end of the constructor // to prevent a callback from trying to use the publisher before we are @@ -102,13 +97,14 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) try { temperature_ = std::make_unique( - serial_num, hub_port, false, + serial_num, hub_port, false, channel, std::bind(&TemperatureRosI::temperatureChangeCallback, this, std::placeholders::_1)); ROS_INFO("Connected"); - temperature_->setDataInterval(data_interval_ms); + try {temperature_->setDataInterval(data_interval_ms);} + catch (...){ROS_WARN("Temperature data interval on channel %d not set",channel);} if (thermocouple_type != 0) { @@ -122,6 +118,8 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) } temperature_pub_ = nh_.advertise("temperature", 1); + temperature_sensor_pub_ = nh_.advertise( + "temperature_"+std::to_string(channel), 1); got_first_data_ = false; @@ -138,18 +136,27 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) } } -void TemperatureRosI::publishLatest() +void TemperatureRosI::publishLatest() // Preserved for legacy { std_msgs::Float64 msg; msg.data = last_temperature_reading_; temperature_pub_.publish(msg); } +void TemperatureRosI::publishLatestTemperature() +{ + sensor_msgs::Temperature msg; + msg.header.stamp = ros::Time::now(); + msg.temperature = last_temperature_reading_; + temperature_sensor_pub_.publish(msg); +} + void TemperatureRosI::timerCallback(const ros::TimerEvent& /* event */) { std::lock_guard lock(temperature_mutex_); if (got_first_data_) { + publishLatestTemperature(); publishLatest(); } } @@ -166,6 +173,7 @@ void TemperatureRosI::temperatureChangeCallback(double temperature) if (publish_rate_ <= 0) { + publishLatestTemperature(); publishLatest(); } }