From 863f9dc596aeb0bdc153b2293764756bfdbf5cc6 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Mon, 5 Feb 2018 14:39:24 +0100 Subject: [PATCH 1/2] feat(omron_node): diagnostics for frequency --- omron_os32c_driver/CMakeLists.txt | 4 ++-- omron_os32c_driver/package.xml | 1 + omron_os32c_driver/src/os32c_node.cpp | 14 ++++++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/omron_os32c_driver/CMakeLists.txt b/omron_os32c_driver/CMakeLists.txt index 6be7daa..08c8cfe 100644 --- a/omron_os32c_driver/CMakeLists.txt +++ b/omron_os32c_driver/CMakeLists.txt @@ -1,13 +1,13 @@ cmake_minimum_required(VERSION 2.8.3) project(omron_os32c_driver) -find_package(catkin REQUIRED COMPONENTS odva_ethernetip roscpp sensor_msgs) +find_package(catkin REQUIRED COMPONENTS diagnostic_updater odva_ethernetip roscpp sensor_msgs) find_package(Boost 1.47 REQUIRED COMPONENTS system) catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS odva_ethernetip roscpp sensor_msgs + CATKIN_DEPENDS diagnostic_updater odva_ethernetip roscpp sensor_msgs LIBRARIES omron_os32c DEPENDS Boost ) diff --git a/omron_os32c_driver/package.xml b/omron_os32c_driver/package.xml index 6f9e8fa..7611ddb 100644 --- a/omron_os32c_driver/package.xml +++ b/omron_os32c_driver/package.xml @@ -11,6 +11,7 @@ catkin boost + diagnostic_updater odva_ethernetip roscpp sensor_msgs diff --git a/omron_os32c_driver/src/os32c_node.cpp b/omron_os32c_driver/src/os32c_node.cpp index 9b79db3..cf1858e 100644 --- a/omron_os32c_driver/src/os32c_node.cpp +++ b/omron_os32c_driver/src/os32c_node.cpp @@ -26,6 +26,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include +#include +#include #include #include "odva_ethernetip/socket/tcp_socket.h" @@ -41,6 +43,8 @@ using eip::socket::TCPSocket; using eip::socket::UDPSocket; using namespace omron_os32c_driver; +double RATE = 12.856; + int main(int argc, char *argv[]) { ros::init(argc, argv, "os32c"); @@ -57,6 +61,12 @@ int main(int argc, char *argv[]) // publisher for laserscans ros::Publisher laserscan_pub = nh.advertise("scan", 1); + // diagnostics for frequency, monitor with 0.1 tolerance + diagnostic_updater::Updater updater; + updater.setHardwareID(host); + diagnostic_updater::FrequencyStatus frequency_status(diagnostic_updater::FrequencyStatusParam(&RATE, &RATE)); + updater.add(frequency_status); + boost::asio::io_service io_service; shared_ptr socket = shared_ptr(new TCPSocket(io_service)); shared_ptr io_socket = shared_ptr(new UDPSocket(io_service, 2222)); @@ -113,6 +123,10 @@ int main(int argc, char *argv[]) laserscan_msg.header.seq++; laserscan_pub.publish(laserscan_msg); + // Tick the frequency and update the diagnostics + frequency_status.tick(); + updater.update(); + // Every tenth message received, send the keepalive message in response. // TODO: Make this time-based instead of message-count based. if (++ctr > 10) From ee2e1a308c82537f7f8a83b260323378b1b3763d Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Mon, 5 Feb 2018 17:34:36 +0100 Subject: [PATCH 2/2] Use diagnosed publisher --- omron_os32c_driver/src/os32c_node.cpp | 30 ++++++++++++++++----------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/omron_os32c_driver/src/os32c_node.cpp b/omron_os32c_driver/src/os32c_node.cpp index cf1858e..30d6898 100644 --- a/omron_os32c_driver/src/os32c_node.cpp +++ b/omron_os32c_driver/src/os32c_node.cpp @@ -26,8 +26,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include -#include -#include +#include #include #include "odva_ethernetip/socket/tcp_socket.h" @@ -42,8 +41,8 @@ using sensor_msgs::LaserScan; using eip::socket::TCPSocket; using eip::socket::UDPSocket; using namespace omron_os32c_driver; +using namespace diagnostic_updater; -double RATE = 12.856; int main(int argc, char *argv[]) { @@ -52,20 +51,28 @@ int main(int argc, char *argv[]) // get sensor config from params string host, frame_id; - double start_angle, end_angle; + double start_angle, end_angle, expected_frequency, frequency_tolerance, timestamp_min_acceptable, + timestamp_max_acceptable; ros::param::param("~host", host, "192.168.1.1"); ros::param::param("~frame_id", frame_id, "laser"); ros::param::param("~start_angle", start_angle, OS32C::ANGLE_MAX); ros::param::param("~end_angle", end_angle, OS32C::ANGLE_MIN); + ros::param::param("~expected_frequency", expected_frequency, 12.856); + ros::param::param("~frequency_tolerance", frequency_tolerance, 0.1); + ros::param::param("~timestamp_min_acceptable", timestamp_min_acceptable, -1); + ros::param::param("~timestamp_max_acceptable", timestamp_max_acceptable, -1); // publisher for laserscans ros::Publisher laserscan_pub = nh.advertise("scan", 1); - // diagnostics for frequency, monitor with 0.1 tolerance - diagnostic_updater::Updater updater; + // diagnostics for frequency + Updater updater; updater.setHardwareID(host); - diagnostic_updater::FrequencyStatus frequency_status(diagnostic_updater::FrequencyStatusParam(&RATE, &RATE)); - updater.add(frequency_status); + DiagnosedPublisher diagnosed_publisher(laserscan_pub, updater, FrequencyStatusParam(&expected_frequency, + &expected_frequency, + frequency_tolerance), + TimeStampStatusParam(timestamp_min_acceptable, + timestamp_max_acceptable)); boost::asio::io_service io_service; shared_ptr socket = shared_ptr(new TCPSocket(io_service)); @@ -118,13 +125,12 @@ int main(int argc, char *argv[]) MeasurementReport report = os32c.receiveMeasurementReportUDP(); OS32C::convertToLaserScan(report, &laserscan_msg); - // Stamp and publish message. + // Stamp and publish message diagnosed laserscan_msg.header.stamp = ros::Time::now(); laserscan_msg.header.seq++; - laserscan_pub.publish(laserscan_msg); + diagnosed_publisher.publish(laserscan_msg); - // Tick the frequency and update the diagnostics - frequency_status.tick(); + // Update diagnostics updater.update(); // Every tenth message received, send the keepalive message in response.