Added dvrk_hrsv_widget: a simple application that creates two Qt base…

…d widgets to be dragged on the HRSV (stereo display) so the operator can see the status of the dVRK (operator present/clutch/camera and state of active PSM teleop)
jhu-dvrk · Mar 13, 2020 · c7d75e2 · c7d75e2
1 parent fe3eb44
commit c7d75e2
Show file tree

Hide file tree

Showing 5 changed files with 351 additions and 0 deletions.
diff --git a/dvrk_hrsv_widget/CMakeLists.txt b/dvrk_hrsv_widget/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required (VERSION 2.8.3)
+project (dvrk_hrsv_widget)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package (catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+  )
+
+# find Qt
+find_package(Qt5Widgets)
+find_package(Qt5Core REQUIRED)
+
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+include_directories (
+  ${catkin_INCLUDE_DIRS}
+  ${dvrk_hrsv_widget_SOURCE_DIR}/include
+  ${dvrk_hrsv_widget_BINARY_DIR}/include
+)
+
+catkin_package ()
+
+add_executable (hrsv_widget
+  include/hrsv_widget.h
+  src/hrsv_widget.cpp
+  src/main.cpp)
+
+target_link_libraries (
+  hrsv_widget
+  Qt5::Widgets
+  ${catkin_LIBRARIES})
diff --git a/dvrk_hrsv_widget/include/hrsv_widget.h b/dvrk_hrsv_widget/include/hrsv_widget.h
@@ -0,0 +1,87 @@
+/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
+/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */
+
+/*
+  Author(s):  Anton Deguet
+  Created on: 2020-03-13
+
+  (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
+
+--- begin cisst license - do not edit ---
+
+This software is provided "as is" under an open source license, with
+no warranty.  The complete license can be found in license.txt and
+http://www.cisst.org/cisst/license.txt.
+
+--- end cisst license ---
+*/
+
+#ifndef _hrsv_widget_h
+#define _hrsv_widget_h
+
+#include <QWidget>
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <sensor_msgs/Joy.h>
+#include <diagnostic_msgs/KeyValue.h>
+
+class QLabel;
+
+class hrsv_widget: public QWidget
+{
+    Q_OBJECT;
+
+public:
+    hrsv_widget(ros::NodeHandle * nodeHandle);
+    ~hrsv_widget();
+
+    void setupUi(void);
+    void setupROS(void);
+
+private slots:
+    void timerEvent(QTimerEvent *);
+
+protected:
+    ros::NodeHandle * mNodeHandle;
+
+    QLabel * mLeftLabel;
+    QLabel * mRightLabel;
+    QLabel * mMainLabel;
+
+    struct {
+        std::string PSM;
+        std::string Status;
+    } mLeft, mRight;
+
+    bool mClutch = false;
+    bool mOperatorPresent = false;
+    std::string mCameraState;
+
+    ros::Subscriber mClutchSubscriber;
+    void ClutchCallback(const sensor_msgs::Joy & message);
+
+    ros::Subscriber mOperatorPresentSubscriber;
+    void OperatorPresentCallback(const sensor_msgs::Joy & message);
+
+    ros::Subscriber mCameraStateSubscriber;
+    void CameraStateCallback(const std_msgs::String & message);
+
+    ros::Subscriber mPSMSelectedCallback;
+    void PSMSelectedCallback(const diagnostic_msgs::KeyValue & message);
+
+    // all possible callbacks
+    ros::Subscriber
+        mMTMRPSM1Subscriber,
+        mMTMRPSM2Subscriber,
+        mMTMRPSM3Subscriber,
+        mMTMLPSM1Subscriber,
+        mMTMLPSM2Subscriber,
+        mMTMLPSM3Subscriber;
+    void MTMRPSMCallback(const std_msgs::String & message);
+    void MTMLPSMCallback(const std_msgs::String & message);
+
+    void UpdateLabels(void);
+};
+
+#endif // _hrsv_widget_h
diff --git a/dvrk_hrsv_widget/package.xml b/dvrk_hrsv_widget/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>dvrk_hrsv_widget</name>
+  <version>0.0.0</version>
+  <description>dVRK HRSV widget</description>
+
+  <maintainer email="[email protected]">Anton Deguet</maintainer>
+
+  <license>BSd</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>
diff --git a/dvrk_hrsv_widget/src/hrsv_widget.cpp b/dvrk_hrsv_widget/src/hrsv_widget.cpp
@@ -0,0 +1,158 @@
+/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
+/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */
+
+/*
+  Author(s):  Anton Deguet
+  Created on: 2020-03-13
+
+  (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
+
+--- begin cisst license - do not edit ---
+
+This software is provided "as is" under an open source license, with
+no warranty.  The complete license can be found in license.txt and
+http://www.cisst.org/cisst/license.txt.
+
+--- end cisst license ---
+*/
+
+// system include
+#include <iostream>
+
+#include <hrsv_widget.h>
+
+#include <QHBoxLayout>
+#include <QLabel>
+
+hrsv_widget::hrsv_widget(ros::NodeHandle * nodeHandle):
+    mNodeHandle(nodeHandle)
+{
+    setupUi();
+    startTimer(50); // 50 ms
+    setupROS();
+}
+
+hrsv_widget::~hrsv_widget(void)
+{
+    // stopTimer();
+}
+
+void hrsv_widget::setupUi(void)
+{
+    QHBoxLayout * layout = new QHBoxLayout();
+    this->setLayout(layout);
+
+    mLeftLabel = new QLabel("--------------------------");
+    mLeftLabel->setAlignment(Qt::AlignLeft);
+    layout->addWidget(mLeftLabel);
+
+    mMainLabel = new QLabel("-----------------------------------------------------------------------");
+    mMainLabel->setAlignment(Qt::AlignCenter);
+    layout->addWidget(mMainLabel);
+
+    mRightLabel = new QLabel("-------------------------");
+    mRightLabel->setAlignment(Qt::AlignRight);
+    layout->addWidget(mRightLabel);
+}
+
+void hrsv_widget::setupROS(void)
+{
+    mClutchSubscriber = mNodeHandle->subscribe("footpedals/clutch", 100,
+                                               &hrsv_widget::ClutchCallback, this);
+    mOperatorPresentSubscriber = mNodeHandle->subscribe("footpedals/operatorpresent", 100,
+                                                        &hrsv_widget::OperatorPresentCallback, this);
+    mCameraStateSubscriber = mNodeHandle->subscribe("MTML_MTMR_ECM/current_state", 100,
+                                                    &hrsv_widget::CameraStateCallback, this);
+
+    mPSMSelectedCallback = mNodeHandle->subscribe("console/teleop/teleop_psm_selected", 100,
+                                                  &hrsv_widget::PSMSelectedCallback, this);
+
+    mMTMRPSM1Subscriber = mNodeHandle->subscribe("MTMR_PSM1/current_state", 100,
+                                                 &hrsv_widget::MTMRPSMCallback, this);
+    mMTMRPSM2Subscriber = mNodeHandle->subscribe("MTMR_PSM2/current_state", 100,
+                                                 &hrsv_widget::MTMRPSMCallback, this);
+    mMTMRPSM3Subscriber = mNodeHandle->subscribe("MTMR_PSM3/current_state", 100,
+                                                 &hrsv_widget::MTMRPSMCallback, this);
+
+    mMTMLPSM1Subscriber = mNodeHandle->subscribe("MTML_PSM1/current_state", 100,
+                                                 &hrsv_widget::MTMLPSMCallback, this);
+    mMTMLPSM2Subscriber = mNodeHandle->subscribe("MTML_PSM2/current_state", 100,
+                                                 &hrsv_widget::MTMLPSMCallback, this);
+    mMTMLPSM3Subscriber = mNodeHandle->subscribe("MTML_PSM3/current_state", 100,
+                                                 &hrsv_widget::MTMLPSMCallback, this);
+}
+
+void hrsv_widget::timerEvent(QTimerEvent *)
+{
+    ros::spinOnce();
+}
+
+void hrsv_widget::ClutchCallback(const sensor_msgs::Joy & message)
+{
+    if (message.buttons.size() != 1) {
+        return;
+    }
+    mClutch = (message.buttons[0] == 1);
+    UpdateLabels();
+}
+
+void hrsv_widget::OperatorPresentCallback(const sensor_msgs::Joy & message)
+{
+    if (message.buttons.size() != 1) {
+        return;
+    }
+    mOperatorPresent = (message.buttons[0] == 1);
+    UpdateLabels();
+}
+
+void hrsv_widget::CameraStateCallback(const std_msgs::String & message)
+{
+    mCameraState = message.data;
+    UpdateLabels();
+}
+
+void hrsv_widget::PSMSelectedCallback(const diagnostic_msgs::KeyValue & message)
+{
+    if (message.key == "MTMR") {
+        mRight.PSM = message.value;
+    } else if (message.key == "MTML") {
+        mLeft.PSM = message.value;
+    }
+    UpdateLabels();
+}
+
+void hrsv_widget::MTMRPSMCallback(const std_msgs::String & message)
+{
+    mRight.Status = message.data;
+    UpdateLabels();
+}
+
+void hrsv_widget::MTMLPSMCallback(const std_msgs::String & message)
+{
+    mLeft.Status = message.data;
+    UpdateLabels();
+}
+
+void hrsv_widget::UpdateLabels(void)
+{
+    std::string message;
+    message = mLeft.PSM + ": " + mLeft.Status;
+    mLeftLabel->setText(QString(message.c_str()));
+
+    message = mRight.PSM + ": " + mRight.Status;
+    mRightLabel->setText(QString(message.c_str()));
+
+    message = "";
+    if (mOperatorPresent) {
+        message.append("Operator");
+    } else {
+        message.append("No operator");
+    }
+    if (mClutch) {
+        message.append(":Clutched");
+    }
+    if (mCameraState == "ENABLED") {
+        message.append(":Camera");
+    }
+    mMainLabel->setText(message.c_str());
+}
diff --git a/dvrk_hrsv_widget/src/main.cpp b/dvrk_hrsv_widget/src/main.cpp
@@ -0,0 +1,44 @@
+/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
+/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */
+
+/*
+  Author(s):  Anton Deguet
+  Created on: 2020-03-13
+
+  (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
+
+--- begin cisst license - do not edit ---
+
+This software is provided "as is" under an open source license, with
+no warranty.  The complete license can be found in license.txt and
+http://www.cisst.org/cisst/license.txt.
+
+--- end cisst license ---
+*/
+
+#include <hrsv_widget.h>
+
+#include <signal.h>
+#include <QApplication>
+#include <ros/ros.h>
+
+void signalHandler(int)
+{
+    QCoreApplication::exit(0);
+}
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "hrsv_widget");
+    QApplication application(argc, argv);
+
+    ros::NodeHandle nodeHandle;
+    hrsv_widget leftWidget(&nodeHandle);
+    hrsv_widget rightWidget(&nodeHandle);
+
+    leftWidget.show();
+    rightWidget.show();
+
+    signal(SIGINT, signalHandler);
+    return application.exec();
+}