Merge pull request #211 from jhu-dvrk/rc-2.3.1

merge rc 2.3.1

CHANGELOG.md

@@ -1,6 +1,22 @@
 Change log
 ==========
 
+2.3.1 (2025-01-17)
+==================
+
+* API changes:
+  * The dvrk_console_json will close all the relays for the user. You shouldn't need to use `qlacommand -c close-relays` anymore!  This can be overriden in the console JSON config.
+  * All arms are re-homed and the encoder preload on the controllers are ignored. It's a bit slower but prevents some issues with potentiometer drift. This can be overriden in the console JSON config.
+* Deprecated features:
+  * All site specific directories under `share` have been removed. Repositories with the existing files have been created under https://github.com/orgs/dvrk-config/repositories.  Let Anton know if you need access to your site's repository.
+* New features:
+  * Added more arm_from_ros
+  * Added script to reconnect all bluetooth for SUJ Si
+* Bug fixes:
+  * Added missing include for Ubuntu 24.04
+  * Fixed bug when using -I
+
+
 2.3.0 (2024-08-30)
 ==================
 

core/CMakeLists.txt

@@ -1,5 +1,5 @@
 #
-# (C) Copyright 2009-2024 Johns Hopkins University (JHU), All Rights Reserved.
+# (C) Copyright 2009-2025 Johns Hopkins University (JHU), All Rights Reserved.
 #
 # --- begin cisst license - do not edit ---
 #
@@ -10,7 +10,7 @@
 # --- end cisst license ---
 
 cmake_minimum_required(VERSION 3.10)
-project (sawIntuitiveResearchKitCore VERSION 2.3.0)
+project (sawIntuitiveResearchKitCore VERSION 2.3.1)
 
 find_package (cisst REQUIRED)
 include (${CISST_USE_FILE})

core/applications/CMakeLists.txt

@@ -20,6 +20,7 @@ add_subdirectory (s-potentiometers-calibration)
 add_subdirectory (config-generator)
 add_subdirectory (sd-card-updater)
 add_subdirectory (remove-logs)
+add_subdirectory (suj-bluetooth-reset)
 
 # create a list of required cisst libraries
 set (REQUIRED_CISST_LIBRARIES

core/applications/s-potentiometers-calibration/SPotentiometersCalibration.cpp

@@ -19,6 +19,7 @@ no warranty.  The complete license can be found in license.txt and
 // system
 #include <iostream>
 #include <limits>
+#include <memory>
 
 // cisst/saw
 #include <cisstCommon/cmnPath.h>

core/applications/suj-bluetooth-reset/CMakeLists.txt

@@ -0,0 +1,41 @@
+#
+# (C) Copyright 2023-2024 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 ---
+
+cmake_minimum_required (VERSION 3.10)
+project (sawIntuitiveResearchKitSUJBluetoothReset VERSION 2.3.0)
+
+# find cisst and make sure the required libraries have been compiled
+find_package (cisst 1.3.0 REQUIRED ${REQUIRED_CISST_LIBRARIES})
+
+if (cisst_FOUND_AS_REQUIRED)
+
+  # load cisst configuration
+  include (${CISST_USE_FILE})
+
+  # catkin/ROS paths
+  cisst_set_output_path ()
+
+  set (sawIntuitiveResearchKitSUJBluetoothReset_FILES
+    ${sawIntuitiveResearchKitSUJBluetoothReset_SOURCE_DIR}/dvrk-suj-bluetooth-reset.py)
+
+  add_custom_target (sawIntuitiveResearchKitSUJBluetoothReset ALL)
+  foreach (_file ${sawIntuitiveResearchKitSUJBluetoothReset_FILES})
+    add_custom_command (TARGET sawIntuitiveResearchKitSUJBluetoothReset PRE_BUILD
+      COMMAND ${CMAKE_COMMAND} -E
+      copy ${_file} ${EXECUTABLE_OUTPUT_PATH})
+  endforeach ()
+
+  install (
+    FILES ${sawIntuitiveResearchKitSUJBluetoothReset_FILES}
+    COMPONENT sawIntuitiveResearchKit-Applications
+    DESTINATION bin)
+
+endif (cisst_FOUND_AS_REQUIRED)

core/applications/suj-bluetooth-reset/dvrk-suj-bluetooth-reset.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+#
+# Script used to disconnect and reconnect the SUJ Si Arduino BLE
+#
+
+import os
+import re
+import json
+import subprocess
+import time
+import argparse
+from argparse import Namespace
+
+def load_json_dvrk(file_path:str)->dict:
+    '''
+    Load json files from dVRK repository
+    :param file_path: json file path
+    :return: a dictionary with loaded json file content
+    '''
+    with open(file_path) as f:
+        data = f.read()
+        data = re.sub("//.*?\n", "", data)
+        data = re.sub("/\\*.*?\\*/", "", data)
+        obj = data[data.find('{'): data.rfind('}') + 1]
+        jsonObj = json.loads(obj)
+    return jsonObj
+
+
+def suj_bluetooth_reset(args: Namespace)->None:
+    '''
+    Reset the dVRK SUJ bluetooth connections
+    '''
+    suj_file_path = args.file_path
+    data = load_json_dvrk(suj_file_path)
+
+    address_list = []
+    address_list.append(data['base-arduino-mac'])
+
+    num_arms = len(data['arms'])
+
+    for i_arm in range(num_arms):
+        address_list.append(data['arms'][i_arm]['arduino-mac'])
+
+    for addr in address_list:
+        print(f"Disconnecting and reconnecting {addr}, timeout is 30 seconds")
+        try:
+            res_disconnect = subprocess.run(['bluetoothctl', 'disconnect', addr],
+                                            capture_output = True, text = True,
+                                            timeout = 30)
+            print(res_disconnect.stdout)
+        except subprocess.TimeoutExpired:
+            print('Disconnect failed, timeout')
+
+        time.sleep(1.0)
+        try:
+            res_connect = subprocess.run(['bluetoothctl', 'connect', addr],
+                                         capture_output = True, text = True,
+                                         timeout = 30)
+            print(res_connect.stdout)
+        except subprocess.TimeoutExpired:
+            print('Connect failed, timeout')
+
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-f', '--file_path', required = True,
+                        help = 'path to dvrk SUJ Si JSON config file')
+
+    args = parser.parse_args()
+    suj_bluetooth_reset(args)
+
+    print('SUJ Bluetooth reset complete!')

core/components/CMakeLists.txt

@@ -1,5 +1,5 @@
 #
-# (C) Copyright 2011-2024 Johns Hopkins University (JHU), All Rights Reserved.
+# (C) Copyright 2011-2025 Johns Hopkins University (JHU), All Rights Reserved.
 #
 # --- begin cisst license - do not edit ---
 #
@@ -10,7 +10,7 @@
 # --- end cisst license ---
 
 cmake_minimum_required (VERSION 3.10)
-project (sawIntuitiveResearchKit VERSION 2.3.0)
+project (sawIntuitiveResearchKit VERSION 2.3.1)
 
 set (REQUIRED_CISST_LIBRARIES
   cisstCommon
@@ -22,7 +22,7 @@ set (REQUIRED_CISST_LIBRARIES
   cisstRobot
   cisstNumerical)
 
-find_package (cisst 1.3.0 REQUIRED ${REQUIRED_CISST_LIBRARIES})
+find_package (cisst 1.3.1 REQUIRED ${REQUIRED_CISST_LIBRARIES})
 
 if (cisst_FOUND)
 

core/components/code/mtsIntuitiveResearchKitSUJSi.cpp

@@ -784,11 +784,13 @@ void mtsIntuitiveResearchKitSUJSi::get_robot_data(void)
             if (sarm->update_raw_pots()) {
 
                 // first joint comes from base arduino
-                if (m_base_arduino) {
+                if (m_base_arduino && m_base_arduino->m_connected) {
                     sarm->m_voltages[0].at(0) = m_base_arduino->m_raw_pots.Row(0).at(sarm->m_base_arduino_pot_index);
                     sarm->m_voltages[1].at(0) = m_base_arduino->m_raw_pots.Row(1).at(sarm->m_base_arduino_pot_index);
+                } else {
+                    sarm->m_voltages[0].at(0) = 0.0;
+                    sarm->m_voltages[1].at(0) = 0.0;
                 }
-
                 // last 3 to 4 joints come from this arm's arduino
                 size_t joints_to_copy = sarm->m_nb_joints - 1;
                 sarm->m_voltages[0].Ref(joints_to_copy, 1) = sarm->m_raw_pots.Row(0).Ref(joints_to_copy);

core/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKit.h

@@ -19,10 +19,19 @@ no warranty.  The complete license can be found in license.txt and
 #ifndef _mtsIntuitiveResearchKit_h
 #define _mtsIntuitiveResearchKit_h
 
+#include <cstdint>
 #include <cisstCommon/cmnUnits.h>
 #include <cisstCommon/cmnConstants.h>
 #include <sawIntuitiveResearchKit/sawIntuitiveResearchKitExport.h>
 
+#define NOT_COPYABLE(TypeName)                          \
+    TypeName (TypeName const &) = delete;               \
+    TypeName & operator = (TypeName const &) = delete;
+
+#define NOT_MOVEABLE( TypeName )                    \
+    TypeName (TypeName &&) = delete;                \
+    TypeName & operator = (TypeName &&) = delete;
+
 namespace mtsIntuitiveResearchKit {
 
     const std::string DefaultInstallationDirectory = "/usr/share/sawIntuitiveResearchKit/share";

core/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKitArm.h

@@ -520,7 +520,7 @@ class CISST_EXPORT mtsIntuitiveResearchKitArm: public mtsTaskPeriodic
     // homing
     bool m_encoders_biased_from_pots = false; // encoders biased from pots
     bool m_encoders_biased = false; // encoder might have to be biased on joint limits (MTM roll)
-    bool m_re_home = false; // force re-biasing encoder even if values are found on FPGA
+    bool m_re_home = true; // force re-biasing encoder even if values are found on FPGA
     bool m_homing_goes_to_zero = false;
     bool m_homing_bias_encoder_requested = false;
     double m_homing_timer;

core/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKitConsole.h

@@ -88,6 +88,9 @@ class CISST_EXPORT mtsIntuitiveResearchKitConsole: public mtsTaskFromSignal
             const std::string & name,
             const std::string & ioComponentName);
 
+        NOT_COPYABLE(Arm);
+        NOT_MOVEABLE(Arm);
+
         /*! Create a new PID component and connect it to the proper RobotIO
           interface.  If the period in seconds is zero, the PID will be tied to
           IO using the ExecIn/ExecOut interfaces. */
@@ -186,6 +189,9 @@ class CISST_EXPORT mtsIntuitiveResearchKitConsole: public mtsTaskFromSignal
 
         TeleopECM(const std::string & name);
 
+        NOT_COPYABLE(TeleopECM);
+        NOT_MOVEABLE(TeleopECM);
+
         /*! Create and configure the robot arm. */
         void ConfigureTeleop(const TeleopECMType type,
                              const double & periodInSeconds,
@@ -212,6 +218,9 @@ class CISST_EXPORT mtsIntuitiveResearchKitConsole: public mtsTaskFromSignal
                   const std::string & nameMTM,
                   const std::string & namePSM);
 
+        NOT_COPYABLE(TeleopPSM);
+        NOT_MOVEABLE(TeleopPSM);
+
         /*! Create and configure the robot arm. */
         void ConfigureTeleop(const TeleopPSMType type,
                              const double & periodInSeconds,
@@ -380,7 +389,7 @@ class CISST_EXPORT mtsIntuitiveResearchKitConsole: public mtsTaskFromSignal
     struct {
         mtsFunctionVoid close_all_relays;
     } IO;
-    bool m_close_all_relays_from_config = false;
+    bool m_close_all_relays_from_config = true;
 
     mtsInterfaceProvided * mInterface = nullptr;
     struct {

core/components/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>saw_intuitive_research_kit</name>
-  <version>2.3.0</version>
+  <version>2.3.1</version>
   <description>
   sawIntuitiveResearchKit
   </description>

ros/dvrk_arms_from_ros/components/CMakeLists.txt

@@ -1,5 +1,5 @@
 #
-# (C) Copyright 2020-2024 Johns Hopkins University (JHU), All Rights Reserved.
+# (C) Copyright 2020-2025 Johns Hopkins University (JHU), All Rights Reserved.
 #
 # --- begin cisst license - do not edit ---
 #
@@ -10,7 +10,7 @@
 # --- end cisst license ---
 
 cmake_minimum_required (VERSION 3.10)
-project (dvrk_arms_from_ros VERSION 2.3.0)
+project (dvrk_arms_from_ros VERSION 2.3.1)
 
 
 # first test for ROS1
@@ -59,7 +59,7 @@ set (REQUIRED_CISST_LIBRARIES
   cisstParameterTypes
   )
 
-find_package (cisst 1.3.0 REQUIRED ${REQUIRED_CISST_LIBRARIES})
+find_package (cisst 1.3.1 REQUIRED ${REQUIRED_CISST_LIBRARIES})
 
 if (cisst_FOUND_AS_REQUIRED)
 
@@ -77,8 +77,10 @@ if (cisst_FOUND_AS_REQUIRED)
   add_library (dvrk_arm_from_ros
     include/dvrk_arm_from_ros.h
     include/dvrk_psm_from_ros.h
+    include/dvrk_mtm_from_ros.h
     src/dvrk_arm_from_ros.cpp
-    src/dvrk_psm_from_ros.cpp)
+    src/dvrk_psm_from_ros.cpp
+    src/dvrk_mtm_from_ros.cpp)
 
   # target_link_libraries (
   #  dvrk_arm_from_ros

ros/dvrk_arms_from_ros/components/include/dvrk_arm_from_ros.h

@@ -35,7 +35,7 @@ class dvrk_arm_from_ros: public mts_ros_crtk_bridge_required
     ~dvrk_arm_from_ros(){}
 
     virtual void Init(void);
-    void Configure(const std::string & CMN_UNUSED(filename));
+    void Configure(const std::string & CMN_UNUSED(filename)) override;
 
 protected:
 };

ros/dvrk_arms_from_ros/components/include/dvrk_mtm_from_ros.h

@@ -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-01-13
+
+  (C) Copyright 2020-2024 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 _dvrk_mtm_from_ros_h
+#define _dvrk_mtm_from_ros_h
+
+#include <dvrk_arm_from_ros.h>
+
+class dvrk_mtm_from_ros: public dvrk_arm_from_ros
+{
+    CMN_DECLARE_SERVICES(CMN_DYNAMIC_CREATION_ONEARG, CMN_LOG_ALLOW_DEFAULT);
+
+public:
+    typedef mtsROSBridge BaseType;
+
+    dvrk_mtm_from_ros(const std::string & componentName,
+                      cisst_ral::node_ptr_t _node_handle,
+                      const double periodInSeconds);
+    dvrk_mtm_from_ros(const mtsTaskPeriodicConstructorArg & arg);
+    ~dvrk_mtm_from_ros() {}
+
+    void InitMTM(void);
+
+protected:
+};
+
+CMN_DECLARE_SERVICES_INSTANTIATION(dvrk_mtm_from_ros);
+
+#endif // _dvrk_mtm_from_ros_h

ros/dvrk_arms_from_ros/components/package.xml

@@ -1,6 +1,6 @@
 <package format="3">
   <name>dvrk_arms_from_ros</name>
-  <version>2.3.0</version>
+  <version>2.3.1</version>
   <description>
   sawIntuitiveResearchKit compatible arm from ROS topics
   </description>