sensor_msgs/Temperature and multi-channel readers

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<void(double)> temperature_handler);
 
  ~Temperature();

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<void(double)> temperature_handler)
  : temperature_handler_(temperature_handler)
 {
@@ -51,7 +51,7 @@ Temperature::Temperature(int32_t serial_number, int hub_port,
 
  helpers::openWaitForAttachment(
  reinterpret_cast<PhidgetHandle>(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);

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.

phidgets_temperature/include/phidgets_temperature/temperature_ros_i.h

@@ -53,13 +53,15 @@ 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_;
  std::string server_name_;
  std::string server_ip_;
 
  void publishLatest();
+ void publishLatestTemperature();
 
  void temperatureChangeCallback(double temperature);
 };

phidgets_temperature/launch/temperature.launch

@@ -7,7 +7,7 @@
  <node pkg="nodelet" type="nodelet" name="temperature_manager"
  args="manager" output="screen" />
 
- #### Gyroscope Driver #####################################################
+ #### Driver ###############################################################
 
  <node pkg="nodelet" type="nodelet" name="PhidgetsTemperatureNodelet"
  args="load phidgets_temperature/PhidgetsTemperatureNodelet temperature_manager"
@@ -21,11 +21,15 @@
  # optional param hub_port, used if connected to a VINT hub
  <!-- <param name="hub_port" value="0"/> -->
 
+ # optional param channel, defaults to 0, used if multiple reader ports are available
+ # Launch seperate nodelets for seperate channels 
+ <!-- <param name="channel" value="0"/> -->
+
  # optional param thermocouple_type, defaults is 0
  <!-- <param name="thermocouple_type" value="0" /> -->
 
  # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms)
- <param name="data_interval_ms" value="4"/>
+ <param name="data_interval_ms" value="250"/>
 
  # optional param publish_rate, defaults to 0
  <!-- <param name="publish_rate" value="0" /> -->

phidgets_temperature/src/temperature_ros_i.cpp

@@ -33,6 +33,7 @@
 
 #include <ros/ros.h>
 #include <std_msgs/Float64.h>
+#include <sensor_msgs/Temperature.h>
 
 #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,11 +84,10 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private)
  server_ip_.c_str());
  }
 
- ROS_INFO(
- "Connecting to Phidgets Temperature serial %d, hub port %d, "
- "thermocouple "
- "type %d ...",
- serial_num, hub_port, thermocouple_type);
+ ROS_INFO(
+ "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
@@ -92,13 +97,14 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private)
  try
  {
  temperature_ = std::make_unique<Temperature>(
- 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)
  {
@@ -112,6 +118,8 @@ TemperatureRosI::TemperatureRosI(ros::NodeHandle nh, ros::NodeHandle nh_private)
  }
 
  temperature_pub_ = nh_.advertise<std_msgs::Float64>("temperature", 1);
+ temperature_sensor_pub_ = nh_.advertise<sensor_msgs::Temperature>(
+ "temperature_"+std::to_string(channel), 1);
 
  got_first_data_ = false;
 
@@ -128,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<std::mutex> lock(temperature_mutex_);
  if (got_first_data_)
  {
+ publishLatestTemperature();
  publishLatest();
  }
 }
@@ -156,6 +173,7 @@ void TemperatureRosI::temperatureChangeCallback(double temperature)
 
  if (publish_rate_ <= 0)
  {
+ publishLatestTemperature();
  publishLatest();
  }
 }