From 9e7c4572e0ba348796ba8b84224e9242b5c3e42d Mon Sep 17 00:00:00 2001 From: Gary Edwards Date: Mon, 11 Mar 2024 12:23:23 +0000 Subject: [PATCH] Add support for Phidgets Humidity sensors (#173) I have done a find and replace on the phidgets_temperature package to get the Phidgets Humidity sensor working. Tested with a `HUM1001_0`. --- phidgets_api/CMakeLists.txt | 1 + phidgets_api/include/phidgets_api/humidity.h | 68 ++++++++ phidgets_api/src/humidity.cpp | 102 ++++++++++++ phidgets_humidity/CMakeLists.txt | 53 +++++++ phidgets_humidity/README.md | 15 ++ .../phidgets_humidity/humidity_ros_i.h | 69 ++++++++ .../phidgets_humidity_nodelet.h | 52 +++++++ phidgets_humidity/launch/humidity.launch | 37 +++++ phidgets_humidity/package.xml | 25 +++ .../phidgets_humidity_nodelet.xml | 9 ++ phidgets_humidity/src/humidity_ros_i.cpp | 147 ++++++++++++++++++ .../src/phidgets_humidity_nodelet.cpp | 52 +++++++ 12 files changed, 630 insertions(+) create mode 100644 phidgets_api/include/phidgets_api/humidity.h create mode 100644 phidgets_api/src/humidity.cpp create mode 100644 phidgets_humidity/CMakeLists.txt create mode 100644 phidgets_humidity/README.md create mode 100644 phidgets_humidity/include/phidgets_humidity/humidity_ros_i.h create mode 100644 phidgets_humidity/include/phidgets_humidity/phidgets_humidity_nodelet.h create mode 100644 phidgets_humidity/launch/humidity.launch create mode 100644 phidgets_humidity/package.xml create mode 100644 phidgets_humidity/phidgets_humidity_nodelet.xml create mode 100644 phidgets_humidity/src/humidity_ros_i.cpp create mode 100644 phidgets_humidity/src/phidgets_humidity_nodelet.cpp diff --git a/phidgets_api/CMakeLists.txt b/phidgets_api/CMakeLists.txt index 2b1a14a5..bb29116c 100644 --- a/phidgets_api/CMakeLists.txt +++ b/phidgets_api/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(phidgets_api src/accelerometer.cpp src/encoder.cpp src/encoders.cpp src/gyroscope.cpp + src/humidity.cpp src/ir.cpp src/magnetometer.cpp src/motor.cpp diff --git a/phidgets_api/include/phidgets_api/humidity.h b/phidgets_api/include/phidgets_api/humidity.h new file mode 100644 index 00000000..00eb0d87 --- /dev/null +++ b/phidgets_api/include/phidgets_api/humidity.h @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PHIDGETS_API_HUMIDITY_H +#define PHIDGETS_API_HUMIDITY_H + +#include + +#include + +#include "phidgets_api/phidget22.h" + +namespace phidgets { + +class Humidity final +{ + public: + PHIDGET22_NO_COPY_NO_MOVE_NO_ASSIGN(Humidity) + + explicit Humidity(int32_t serial_number, int hub_port, + bool is_hub_port_device, + std::function humidity_handler); + + ~Humidity(); + + double getHumidity() const; + + void setDataInterval(uint32_t interval_ms) const; + + void humidityChangeHandler(double humidity) const; + + private: + std::function humidity_handler_; + PhidgetHumiditySensorHandle humidity_handle_; + static void HumidityChangeHandler( + PhidgetHumiditySensorHandle humidity_handle, void *ctx, + double humidity); +}; + +} // namespace phidgets + +#endif // PHIDGETS_API_HUMIDITY_H diff --git a/phidgets_api/src/humidity.cpp b/phidgets_api/src/humidity.cpp new file mode 100644 index 00000000..f7d9cb3a --- /dev/null +++ b/phidgets_api/src/humidity.cpp @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include + +#include "phidgets_api/phidget22.h" +#include "phidgets_api/humidity.h" + +namespace phidgets { + +Humidity::Humidity(int32_t serial_number, int hub_port, bool is_hub_port_device, + std::function humidity_handler) + : humidity_handler_(humidity_handler) +{ + PhidgetReturnCode ret = PhidgetHumiditySensor_create(&humidity_handle_); + if (ret != EPHIDGET_OK) + { + throw Phidget22Error("Failed to create HumiditySensor handle", ret); + } + + helpers::openWaitForAttachment( + reinterpret_cast(humidity_handle_), serial_number, + hub_port, is_hub_port_device, 0); + + ret = PhidgetHumiditySensor_setOnHumidityChangeHandler( + humidity_handle_, HumidityChangeHandler, this); + if (ret != EPHIDGET_OK) + { + throw Phidget22Error("Failed to set change handler for Humidity", ret); + } +} + +Humidity::~Humidity() +{ + PhidgetHandle handle = reinterpret_cast(humidity_handle_); + helpers::closeAndDelete(&handle); +} + +double Humidity::getHumidity() const +{ + double current_humidity; + PhidgetReturnCode ret = + PhidgetHumiditySensor_getHumidity(humidity_handle_, ¤t_humidity); + if (ret != EPHIDGET_OK) + { + throw Phidget22Error("Failed to get humidity", ret); + } + return current_humidity; +} + +void Humidity::setDataInterval(uint32_t interval_ms) const +{ + PhidgetReturnCode ret = + PhidgetHumiditySensor_setDataInterval(humidity_handle_, interval_ms); + if (ret != EPHIDGET_OK) + { + throw Phidget22Error("Failed to set data interval", ret); + } +} + +void Humidity::humidityChangeHandler(double humidity) const +{ + humidity_handler_(humidity); +} + +void Humidity::HumidityChangeHandler( + PhidgetHumiditySensorHandle /* humidity_handle */, void *ctx, + double humidity) +{ + ((Humidity *)ctx)->humidityChangeHandler(humidity); +} + +} // namespace phidgets diff --git a/phidgets_humidity/CMakeLists.txt b/phidgets_humidity/CMakeLists.txt new file mode 100644 index 00000000..be11d874 --- /dev/null +++ b/phidgets_humidity/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.5.1) +cmake_policy(SET CMP0048 NEW) +project(phidgets_humidity) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# High level of warnings: +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(catkin REQUIRED COMPONENTS nodelet phidgets_api roscpp roslaunch std_msgs) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES phidgets_humidity + CATKIN_DEPENDS nodelet phidgets_api roscpp std_msgs +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(phidgets_humidity src/humidity_ros_i.cpp) +add_library(phidgets_humidity_nodelet src/phidgets_humidity_nodelet.cpp) + +add_dependencies(phidgets_humidity ${catkin_EXPORTED_TARGETS}) +add_dependencies(phidgets_humidity_nodelet ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(phidgets_humidity ${catkin_LIBRARIES}) +target_link_libraries(phidgets_humidity_nodelet ${catkin_LIBRARIES} phidgets_humidity) + +install(TARGETS phidgets_humidity phidgets_humidity_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(FILES phidgets_humidity_nodelet.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +roslaunch_add_file_check(launch) diff --git a/phidgets_humidity/README.md b/phidgets_humidity/README.md new file mode 100644 index 00000000..35658bdc --- /dev/null +++ b/phidgets_humidity/README.md @@ -0,0 +1,15 @@ +Phidgets humidity ROS driver +============================ + +This is the ROS driver for Phidgets humidity sensor. The various topics, services, and parameters that the node operates with are listed below. + +Topics +------ +* `/humidity` (`std_msgs/Float64`) - The current relative humidity in %. + +Parameters +---------- +* `serial` (int) - The serial number of the phidgets humidity to connect to. If -1 (the default), connects to any humidity phidget that can be found. +* `hub_port` (int) - The phidgets VINT hub port to connect to. Only used if the humidity phidget is connected to a VINT hub. Defaults to 0. +* `data_interval_ms` (int) - The number of milliseconds between acquisitions of data on the device. Defaults to 500 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_humidity/include/phidgets_humidity/humidity_ros_i.h b/phidgets_humidity/include/phidgets_humidity/humidity_ros_i.h new file mode 100644 index 00000000..3750245e --- /dev/null +++ b/phidgets_humidity/include/phidgets_humidity/humidity_ros_i.h @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PHIDGETS_HUMIDITY_HUMIDITY_ROS_I_H +#define PHIDGETS_HUMIDITY_HUMIDITY_ROS_I_H + +#include +#include + +#include + +#include "phidgets_api/humidity.h" + +namespace phidgets { + +class HumidityRosI final +{ + public: + explicit HumidityRosI(ros::NodeHandle nh, ros::NodeHandle nh_private); + + private: + std::unique_ptr humidity_; + std::mutex humidity_mutex_; + double last_humidity_reading_; + bool got_first_data_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + ros::Publisher humidity_pub_; + void timerCallback(const ros::TimerEvent& event); + ros::Timer timer_; + int publish_rate_; + std::string server_name_; + std::string server_ip_; + + void publishLatest(); + + void humidityChangeCallback(double humidity); +}; + +} // namespace phidgets + +#endif // PHIDGETS_HUMIDITY_HUMIDITY_ROS_I_H diff --git a/phidgets_humidity/include/phidgets_humidity/phidgets_humidity_nodelet.h b/phidgets_humidity/include/phidgets_humidity/phidgets_humidity_nodelet.h new file mode 100644 index 00000000..67aa166e --- /dev/null +++ b/phidgets_humidity/include/phidgets_humidity/phidgets_humidity_nodelet.h @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PHIDGETS_HUMIDITY_PHIDGETS_HUMIDITY_NODELET_H +#define PHIDGETS_HUMIDITY_PHIDGETS_HUMIDITY_NODELET_H + +#include + +#include + +#include "phidgets_humidity/humidity_ros_i.h" + +namespace phidgets { + +class PhidgetsHumidityNodelet : public nodelet::Nodelet +{ + public: + virtual void onInit(); + + private: + std::unique_ptr humidity_; +}; + +} // namespace phidgets + +#endif // PHIDGETS_HUMIDITY_PHIDGETS_HUMIDITY_NODELET_H diff --git a/phidgets_humidity/launch/humidity.launch b/phidgets_humidity/launch/humidity.launch new file mode 100644 index 00000000..58f95355 --- /dev/null +++ b/phidgets_humidity/launch/humidity.launch @@ -0,0 +1,37 @@ + + + + + #### Nodelet manager ###################################################### + + + + #### Humidity Driver ##################################################### + + + + # See README.md for more information on the parameters + + # optional param serial, default is -1 + + + # optional param hub_port, used if connected to a VINT hub + + + # supported data rates: 50 ms - 60,000 ms + + + # 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_humidity/package.xml b/phidgets_humidity/package.xml new file mode 100644 index 00000000..282feab7 --- /dev/null +++ b/phidgets_humidity/package.xml @@ -0,0 +1,25 @@ + + + phidgets_humidity + 1.0.8 + Driver for the Phidgets Humidity devices + + Martin Günther + Chris Lalancette + + BSD + + Gary Edwards + + catkin + + nodelet + phidgets_api + roscpp + roslaunch + std_msgs + + + + + diff --git a/phidgets_humidity/phidgets_humidity_nodelet.xml b/phidgets_humidity/phidgets_humidity_nodelet.xml new file mode 100644 index 00000000..2e407a26 --- /dev/null +++ b/phidgets_humidity/phidgets_humidity_nodelet.xml @@ -0,0 +1,9 @@ + + + + + Phidgets Humidity nodelet publisher. + + + diff --git a/phidgets_humidity/src/humidity_ros_i.cpp b/phidgets_humidity/src/humidity_ros_i.cpp new file mode 100644 index 00000000..454235e0 --- /dev/null +++ b/phidgets_humidity/src/humidity_ros_i.cpp @@ -0,0 +1,147 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include +#include + +#include "phidgets_humidity/humidity_ros_i.h" + +namespace phidgets { + +HumidityRosI::HumidityRosI(ros::NodeHandle nh, ros::NodeHandle nh_private) + : nh_(nh), nh_private_(nh_private) +{ + ROS_INFO("Starting Phidgets Humidity"); + + int serial_num; + if (!nh_private_.getParam("serial", serial_num)) + { + serial_num = -1; // default open any device + } + int hub_port; + if (!nh_private.getParam("hub_port", hub_port)) + { + hub_port = 0; // only used if the device is on a VINT hub_port + } + int data_interval_ms; + if (!nh_private.getParam("data_interval_ms", data_interval_ms)) + { + data_interval_ms = 500; + } + if (!nh_private.getParam("publish_rate", publish_rate_)) + { + 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 Humidity serial %d, hub port %d, ", + serial_num, hub_port); + + // 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 + // finished setting up. + std::lock_guard lock(humidity_mutex_); + + try + { + humidity_ = std::make_unique( + serial_num, hub_port, false, + std::bind(&HumidityRosI::humidityChangeCallback, this, + std::placeholders::_1)); + + ROS_INFO("Connected"); + + humidity_->setDataInterval(data_interval_ms); + + } catch (const Phidget22Error& err) + { + ROS_ERROR("Humidity: %s", err.what()); + throw; + } + + humidity_pub_ = nh_.advertise("humidity", 1); + + got_first_data_ = false; + + if (publish_rate_ > 0) + { + timer_ = nh_.createTimer(ros::Duration(1.0 / publish_rate_), + &HumidityRosI::timerCallback, this); + } else + { + // If we're not publishing at a fixed rate, publish the first data + } +} + +void HumidityRosI::publishLatest() +{ + std_msgs::Float64 msg; + msg.data = last_humidity_reading_; + humidity_pub_.publish(msg); +} + +void HumidityRosI::timerCallback(const ros::TimerEvent& /* event */) +{ + std::lock_guard lock(humidity_mutex_); + if (got_first_data_) + { + publishLatest(); + } +} + +void HumidityRosI::humidityChangeCallback(double humidity) +{ + std::lock_guard lock(humidity_mutex_); + last_humidity_reading_ = humidity; + + if (!got_first_data_) + { + got_first_data_ = true; + } + + if (publish_rate_ <= 0) + { + publishLatest(); + } +} + +} // namespace phidgets diff --git a/phidgets_humidity/src/phidgets_humidity_nodelet.cpp b/phidgets_humidity/src/phidgets_humidity_nodelet.cpp new file mode 100644 index 00000000..f03cf033 --- /dev/null +++ b/phidgets_humidity/src/phidgets_humidity_nodelet.cpp @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include +#include + +#include "phidgets_humidity/phidgets_humidity_nodelet.h" +#include "phidgets_humidity/humidity_ros_i.h" + +typedef phidgets::PhidgetsHumidityNodelet PhidgetsHumidityNodelet; + +PLUGINLIB_EXPORT_CLASS(PhidgetsHumidityNodelet, nodelet::Nodelet) + +void PhidgetsHumidityNodelet::onInit() +{ + NODELET_INFO("Initializing Phidgets Humidity Nodelet"); + + // TODO: Do we want the single threaded or multithreaded NH? + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_private = getMTPrivateNodeHandle(); + + humidity_ = std::make_unique(nh, nh_private); +}