initial package cleanup

Reviewed-on: #1
Co-authored-by: QuentinLin <qlin1806@gmail.com>
Co-committed-by: QuentinLin <qlin1806@gmail.com>

.gitignore

@@ -1,3 +1,5 @@
 .idea
 CMakeLists.txt.user
-cmake-build*
+cmake-build*
+build/
+**/__pycache__/

CMakeLists.txt

@@ -1,33 +1,68 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.0.2)
 project(sas_robot_driver_franka)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 #add_compile_options(-std=c++11)
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+add_compile_options(-Werror=return-type)
-    add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
 
 
-find_package(pybind11 REQUIRED)
+#Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view.
-find_package(ament_cmake REQUIRED)
+file(GLOB_RECURSE EXTRA_FILES */*)
-find_package(rclcpp REQUIRED)
+#add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
-find_package(geometry_msgs REQUIRED)
+add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
-find_package(std_msgs REQUIRED)
+
-find_package(std_srvs REQUIRED)
+## Find catkin macros and libraries
-find_package(sas_common REQUIRED)
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-find_package(sas_core REQUIRED)
+## is used, also find other catkin packages
-find_package(sas_msgs REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
-find_package(sas_conversions REQUIRED)
+        roscpp
-find_package(Eigen3 REQUIRED)
+        rospy
-find_package(tf2_ros REQUIRED)
+        std_msgs
-find_package(tf2 REQUIRED)
+        tf2_ros
-find_package(sas_robot_driver REQUIRED)
+        tf2
+        sas_common
+        sas_clock
+        sas_robot_driver
+        sas_patient_side_manager
+        message_generation
+        pybind11_catkin
+)
+
+
+add_service_files(
+        DIRECTORY srv
+        FILES
+        Move.srv
+        Grasp.srv
+)
+
+add_message_files(
+        DIRECTORY msg
+        FILES
+        GripperState.msg
+)
+
+catkin_python_setup()
+
+generate_messages(
+        DEPENDENCIES
+        std_msgs
+
+)
+
+
+catkin_package(
+        INCLUDE_DIRS include
+        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin message_runtime std_msgs
+)
+
 find_package(Franka REQUIRED)
-find_package(pinocchio REQUIRED)
+find_package(Eigen3 REQUIRED)
-find_package(sas_robot_driver_franka_interfaces REQUIRED)  # Add this line
+find_package(yaml-cpp REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
 
-## To correctly find and link with QT
+# To correctly find and link with QT
 set(CMAKE_PREFIX_PATH $ENV{QT_PATH})
 set(CMAKE_AUTOMOC ON)
 set(CMAKE_AUTORCC ON)
@@ -37,76 +72,15 @@ if (CMAKE_VERSION VERSION_LESS "3.7.0")
 endif ()
 find_package(Qt5 COMPONENTS Widgets REQUIRED)
 
-##### CPP LIBRARY initial #####
+find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets)
-include_directories(
+find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets)
-        include
-        constraints_manager/include
-)
-install(
-        DIRECTORY include/
-        DESTINATION include
-)
 
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(Eigen3 rclcpp geometry_msgs std_msgs
-        sas_common sas_core sas_conversions sas_robot_driver_franka_interfaces
-        tf2_ros tf2 sas_robot_driver)
-
-
-##### CPP LIBRARY initial end  #####
-
-##### CPP LIBRARY ROBOT_DYNAMICS #####
-
-add_library(${PROJECT_NAME}_robot_dynamics SHARED
-        src/robot_dynamics/qros_robot_dynamics_client.cpp
-        src/robot_dynamics/qros_robot_dynamics_server.cpp
-)
-
-ament_target_dependencies(${PROJECT_NAME}_robot_dynamics
-        rclcpp geometry_msgs std_msgs
-        sas_common sas_core sas_conversions
-        tf2_ros tf2 sas_robot_driver)
-
-
-target_include_directories(${PROJECT_NAME}_robot_dynamics
-        PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>)
-
-
-target_link_libraries(${PROJECT_NAME}_robot_dynamics
-        -ldqrobotics
-        Eigen3::Eigen
-)
-
-install(
-        TARGETS ${PROJECT_NAME}_robot_dynamics # ${PROJECT_NAME}
-        EXPORT export_${PROJECT_NAME} #export_${PROJECT_NAME}
-        LIBRARY DESTINATION lib
-        #ARCHIVE DESTINATION lib
-        RUNTIME DESTINATION bin
-        INCLUDES DESTINATION include
-)
-
-##END## CPP LIBRARY ROBOT_DYNAMICS #####
-
-##### CPP LIBRARY Motion Generation  #####
 
 add_library(MotionGenerator src/generator/motion_generator.cpp)
-target_link_libraries(MotionGenerator Franka::Franka pinocchio::pinocchio Eigen3::Eigen)
+target_link_libraries(MotionGenerator Franka::Franka)
+
 
 add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp)
-target_link_libraries(ConstraintsManager Eigen3::Eigen)
-install(
-        DIRECTORY constraints_manager/include/
-        DESTINATION include/constraints_manager
-)
-target_include_directories(ConstraintsManager
-        PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/constraints_manager/include>
-        $<INSTALL_INTERFACE:include/constraints_manager>)
-
-
 
 add_library(QuadraticProgramMotionGenerator src/generator/quadratic_program_motion_generator.cpp)
 target_link_libraries(QuadraticProgramMotionGenerator
@@ -114,21 +88,14 @@ target_link_libraries(QuadraticProgramMotionGenerator
         dqrobotics
         ConstraintsManager)
 
-
 add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp)
 target_link_libraries(CustomMotionGeneration
         qpOASES
         dqrobotics
         ConstraintsManager)
 
-##### CPP LIBRARY Motion Generation end  #####
-
-
-##### CPP LIBRARY franka low level interface  #####
-
 add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
 target_link_libraries(robot_interface_franka Franka::Franka
-        pinocchio::pinocchio
         dqrobotics
         MotionGenerator
         ConstraintsManager
@@ -137,163 +104,149 @@ target_link_libraries(robot_interface_franka Franka::Franka
 
 add_library(robot_interface_hand src/hand/robot_interface_hand.cpp)
 target_link_libraries(robot_interface_hand Franka::Franka
-        pinocchio::pinocchio
         dqrobotics)
 
 
-##### CPP LIBRARY franka low level interface end  #####
+############
+## Build ###
+############
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+
+
+include_directories(
+        include
+        src/
+        src/robot_dynamics
+        src/hand
+        src/joint
+        ${catkin_INCLUDE_DIRS}
+        constraints_manager/include
+)
+
+
+add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp)
+target_link_libraries(qros_robot_dynamics_provider
+        ${catkin_LIBRARIES}
+        dqrobotics)
+
+add_library(qros_robot_dynamics_interface src/robot_dynamics/qros_robot_dynamics_interface.cpp)
+target_link_libraries(qros_robot_dynamics_interface
+        ${catkin_LIBRARIES}
+        dqrobotics)
+
+add_dependencies(qros_robot_dynamics_interface ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(qros_robot_dynamics_interface ${catkin_EXPORTED_TARGETS})
+
 
-##### SAS Franka Robot Driver #####
 add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
 target_link_libraries(sas_robot_driver_franka
-        ${PROJECT_NAME}_robot_dynamics
+        qros_robot_dynamics_provider
         dqrobotics
         dqrobotics-interface-json11
         robot_interface_franka)
 
-##### SAS Franka Robot Driver end  #####
-##### qros hand effector driver #####
-
 add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
-
-ament_target_dependencies(qros_effector_driver_franka_hand
-        rclcpp sas_common sas_core std_srvs
-        sas_robot_driver_franka_interfaces
-)
-
-#ament_target_dependencies(${PROJECT_NAME}_robot_dynamics
-#        rclcpp geometry_msgs std_msgs
-#        sas_common sas_core sas_conversions
-#        tf2_ros tf2 sas_robot_driver)
-
 target_link_libraries(qros_effector_driver_franka_hand
         dqrobotics
-        Franka::Franka
+        Franka::Franka)
-        pinocchio::pinocchio
+add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS})
-)
+add_dependencies(qros_effector_driver_franka_hand ${catkin_EXPORTED_TARGETS})
 
-#target_include_directories(qros_effector_driver_franka_hand
+add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
-#        PUBLIC
+target_link_libraries(sas_robot_driver_coppelia
-#        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/interface>
-#        $<INSTALL_INTERFACE:include/interface>)
-
-##### qros hand effector driver end  #####
-
-
-##### SAS Franka Robot Driver Node #####
-
-add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp)
-ament_target_dependencies(sas_robot_driver_franka_node
-        rclcpp sas_common sas_core
-        sas_robot_driver)
-target_link_libraries(sas_robot_driver_franka_node sas_robot_driver_franka)
-
-install(TARGETS
-        sas_robot_driver_franka_node
-        DESTINATION lib/${PROJECT_NAME})
-
-
-##### SAS Franka Robot Hand Driver Node #####
-add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
-ament_target_dependencies(sas_robot_driver_franka_node
-        rclcpp sas_common sas_core)
-target_link_libraries(sas_robot_driver_franka_hand_node
-        qros_effector_driver_franka_hand)
-
-install(TARGETS
-        sas_robot_driver_franka_hand_node
-        DESTINATION lib/${PROJECT_NAME})
-
-##### Coppelia Mirror Node #####
-
-add_executable(sas_robot_driver_franka_coppelia_node
-    src/sas_robot_driver_coppelia_node.cpp
-    src/coppelia/sas_robot_driver_coppelia.cpp
-)
-ament_target_dependencies(sas_robot_driver_franka_coppelia_node
-        rclcpp sas_common sas_core sas_conversions sas_robot_driver
-)
-
-target_link_libraries(sas_robot_driver_franka_coppelia_node
         dqrobotics
         dqrobotics-interface-json11
-        dqrobotics-interface-vrep
+        dqrobotics-interface-vrep)
-)
+
-install(TARGETS
+add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-        sas_robot_driver_franka_coppelia_node
+add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-        DESTINATION lib/${PROJECT_NAME})
+add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+
+add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp)
+target_link_libraries(sas_robot_driver_coppelia_node
+        sas_robot_driver_coppelia
+        ${catkin_LIBRARIES})
+
+add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp)
+target_link_libraries(sas_robot_driver_franka_node
+        sas_robot_driver_franka
+        ${catkin_LIBRARIES})
+
+
+add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
+target_link_libraries(sas_robot_driver_franka_hand_node
+        qros_effector_driver_franka_hand
+        ${catkin_LIBRARIES})
 
-##### JuanEmika
 
 add_executable(JuankaEmika
         qt/configuration_window/main.cpp
         qt/configuration_window/mainwindow.cpp
         qt/configuration_window/mainwindow.ui
 )
-ament_target_dependencies(sas_robot_driver_franka_node
-        rclcpp sas_common sas_core
-        sas_robot_driver)
 
 target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
         dqrobotics
+        ${catkin_LIBRARIES}
         robot_interface_franka
 )
 
+
+#####################################################################################
+# python binding
+include_directories(
+        include/sas_robot_driver_franka/robot_dynamic
+)
+pybind_add_module(_qros_robot_dynamic SHARED
+        src/robot_dynamics/qros_robot_dynamic_py.cpp src/robot_dynamics/qros_robot_dynamics_interface.cpp src/robot_dynamics/qros_robot_dynamics_provider.cpp
+)
+add_dependencies(_qros_robot_dynamic ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND)
+# https://github.com/pybind/pybind11/issues/387
+target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics)
+
+
 if (QT_VERSION_MAJOR EQUAL 6)
     qt_finalize_executable(JuankaEmika)
 endif ()
 
-install(TARGETS
+install(TARGETS ${PROJECT_NAME}
-        JuankaEmika
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        DESTINATION lib/${PROJECT_NAME})
-
-
-##### PYBIND11 LIBRARY ROBOT_DYNAMICS #####
-ament_python_install_package(${PROJECT_NAME})
-
-pybind11_add_module(_qros_franka_robot_dynamics_py SHARED
-        src/robot_dynamics/qros_robot_dynamics_py.cpp
-        src/robot_dynamics/qros_robot_dynamics_client.cpp
-        src/robot_dynamics/qros_robot_dynamics_server.cpp
 )
 
-target_include_directories(_qros_franka_robot_dynamics_py
+install(TARGETS sas_robot_driver_franka_node
-        PUBLIC
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot_dynamics>
-        $<INSTALL_INTERFACE:include/robot_dynamics>)
-
-target_compile_definitions(_qros_franka_robot_dynamics_py PRIVATE IS_SAS_PYTHON_BUILD)
-# https://github.com/pybind/pybind11/issues/387
-target_link_libraries(_qros_franka_robot_dynamics_py PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
-
-install(TARGETS _qros_franka_robot_dynamics_py
-        DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
 )
-##END## PYBIND11 LIBRARY ROBOT_DYNAMICS #####
 
+install(TARGETS sas_robot_driver_coppelia_node
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
-install(
+install(TARGETS sas_robot_driver_franka_hand_node
-        DIRECTORY include/
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-        DESTINATION include
+)
+
+install(TARGETS qros_robot_dynamics_provider
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(TARGETS qros_robot_dynamics_interface
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h"
+        #   PATTERN ".svn" EXCLUDE
 )
 
 
-
+install(TARGETS _qros_robot_dynamic
-
+        LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
-##### LAUNCH FILES #####
-
-install(DIRECTORY
-        launch
-        DESTINATION share/${PROJECT_NAME}/
 )
 
-##END## LAUNCH FILES #####
-
-ament_package()
-
-
-
-
-
-
-

include/sas_robot_driver_franka/generator/custom_motion_generation.h

@@ -32,13 +32,13 @@
 
 #pragma once
 #include <array>
-#include <eigen3/Eigen/Core>
+#include <Eigen/Core>
-#include <eigen3/Eigen/Dense>
+#include <Eigen/Dense>
 #include <vector>
 #include <iostream>
 #include <dqrobotics/solvers/DQ_QPOASESSolver.h>
 #include <memory>
-#include <constraints_manager.h>
+#include "constraints_manager.h"
 
 
 using namespace DQ_robotics;

include/sas_robot_driver_franka/generator/motion_generator.h

@@ -4,8 +4,8 @@
 
 #include <array>
 
-#include <eigen3/Eigen/Core>
+#include <Eigen/Core>
-#include <eigen3/Eigen/Dense>
+#include <Eigen/Dense>
 #include <franka/control_types.h>
 #include <franka/duration.h>
 #include <franka/robot.h>

include/sas_robot_driver_franka/generator/quadratic_program_motion_generator.h

@@ -1,7 +1,7 @@
 #pragma once
 #include <array>
-#include <eigen3/Eigen/Core>
+#include <Eigen/Core>
-#include <eigen3/Eigen/Dense>
+#include <Eigen/Dense>
 #include <vector>
 #include <iostream>
 #include <dqrobotics/solvers/DQ_QPOASESSolver.h>

include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h

@@ -1,6 +1,6 @@
 #pragma once
 /*
-# Copyright (c) 2024 Quenitin Lin
+# Copyright (c) 2023 Juan Jose Quiroz Omana
 #
 #    This file is part of sas_robot_driver_franka.
 #
@@ -28,66 +28,63 @@
 #
 # ################################################################
 */
-
+#include <ros/ros.h>
-#include <rclcpp/rclcpp.hpp>
+#include <ros/node_handle.h>
-#include <rclcpp/node.hpp>
+#include <sas_common/sas_common.h>
-// #include <sas_common/sas_common.hpp>
+#include <sas_conversions/sas_conversions.h>
-#include <sas_conversions/sas_conversions.hpp>
+#include <geometry_msgs/WrenchStamped.h>
-#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <geometry_msgs/Transform.h>
-#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <tf2_ros/transform_listener.h>
-#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2/time.h>
+#include <geometry_msgs/Pose.h>
-// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <std_msgs/Header.h>
-#include <geometry_msgs/msg/pose.hpp>
-#include <std_msgs/msg/header.hpp>
 #include <dqrobotics/DQ.h>
 
 #define BUFFER_DURATION_DEFAULT_S 2.0   // 2 second
 
-using namespace rclcpp;
 namespace qros {
 
 using namespace DQ_robotics;
 
-class RobotDynamicsClient {
+class RobotDynamicInterface {
 private:
-    std::shared_ptr<Node> node_;
+    unsigned int seq_ = 0;
 
-    std::string topic_prefix_;
+    std::string node_prefix_;
     std::string child_frame_id_;
     std::string parent_frame_id_;
 
-    Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr subscriber_cartesian_stiffness_;
+    ros::Subscriber subscriber_cartesian_stiffness_;
-    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    tf2_ros::Buffer tf_buffer_;
-    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+    tf2_ros::TransformListener tf_listener_;
 
 
-    static geometry_msgs::msg::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ;
+    static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ;
 
     Vector3d last_stiffness_force_;
     Vector3d last_stiffness_torque_;
     DQ last_stiffness_frame_pose_;
 
-    void _callback_cartesian_stiffness(const geometry_msgs::msg::WrenchStamped &msg);
+    void _callback_cartesian_stiffness(const geometry_msgs::WrenchStampedConstPtr &msg);
 
-    static DQ _geometry_msgs_transform_to_dq(const geometry_msgs::msg::Transform& transform);
+    static DQ _geometry_msgs_transform_to_dq(const geometry_msgs::Transform& transform);
 
 public:
-    RobotDynamicsClient() = delete;
+    RobotDynamicInterface() = delete;
-    RobotDynamicsClient(const RobotDynamicsClient&) = delete;
+    RobotDynamicInterface(const RobotDynamicInterface&) = delete;
-// #ifdef BUILD_PYBIND
+#ifdef BUILD_PYBIND
-//     explicit RobotDynamicsClient(const std::string& node_prefix):RobotDynamicsClient(sas::common::get_static_node_handle(),node_prefix){}
+    explicit RobotDynamicInterface(const std::string& node_prefix):RobotDynamicInterface(sas::common::get_static_node_handle(),node_prefix){}
-// #endif
+#endif
-    explicit RobotDynamicsClient(const std::shared_ptr<Node> &node,  const std::string& topic_prefix="GET_FROM_NODE");
+    explicit RobotDynamicInterface(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
+    RobotDynamicInterface(ros::NodeHandle& publisher_nodehandle, ros::NodeHandle& subscriber_nodehandle, const std::string& node_prefix=ros::this_node::getName());
 
     VectorXd get_stiffness_force();
     VectorXd get_stiffness_torque();
     DQ get_stiffness_frame_pose();
 
     bool is_enabled() const;
-    std::string get_topic_prefix() const {return topic_prefix_;}
+    std::string get_topic_prefix() const {return node_prefix_;}
 
 };
 

include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h

@@ -1,6 +1,6 @@
 #pragma once
 /*
-# Copyright (c) 2024 Quenitin Lin
+# Copyright (c) 2023 Juan Jose Quiroz Omana
 #
 #    This file is part of sas_robot_driver_franka.
 #
@@ -28,60 +28,60 @@
 #
 # ################################################################
 */
-#include <rclcpp/rclcpp.hpp>
+#include <ros/ros.h>
-#include <rclcpp/node.hpp>
+#include <ros/node_handle.h>
-// #include <sas_common/sas_common.hpp>
+#include <sas_common/sas_common.h>
-#include <sas_conversions/sas_conversions.hpp>
+#include <sas_conversions/sas_conversions.h>
-#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <geometry_msgs/WrenchStamped.h>
-#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/Transform.h>
-#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/TransformStamped.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros//static_transform_broadcaster.h>
-#include <geometry_msgs/msg/pose.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <std_msgs/msg/header.hpp>
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/Header.h>
 #include <dqrobotics/DQ.h>
 
 #define REDUCE_TF_PUBLISH_RATE 10
 #define WORLD_FRAME_ID "world"
-using namespace rclcpp;
 
 namespace qros {
 
 using namespace DQ_robotics;
 
-class RobotDynamicsServer {
+class RobotDynamicProvider {
 private:
     unsigned int seq_ = 0;
-    std::shared_ptr<Node> node_;
 
-    std::string topic_prefix_;
+    std::string node_prefix_;
     std::string child_frame_id_;
     std::string parent_frame_id_;
 
-    rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr publisher_cartesian_stiffness_;
+    ros::Publisher publisher_cartesian_stiffness_;
-    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    tf2_ros::TransformBroadcaster tf_broadcaster_;
-    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_base_tf_broadcaster_;
+    tf2_ros::StaticTransformBroadcaster static_base_tf_broadcaster_;
 
-    DQ world_to_base_tf_ = DQ(0);
+    DQ world_to_base_tf_;
 
-    static geometry_msgs::msg::Transform _dq_to_geometry_msgs_transform(const DQ& pose);
+    static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose);
 
     void _publish_base_static_tf();
 
 public:
-    RobotDynamicsServer() = delete;
+    RobotDynamicProvider() = delete;
-    RobotDynamicsServer(const RobotDynamicsServer&) = delete;
+    RobotDynamicProvider(const RobotDynamicProvider&) = delete;
-// #ifdef BUILD_PYBIND
+#ifdef BUILD_PYBIND
-    // explicit RobotDynamicsServer(const std::string& node_prefix):RobotDynamicsServer(sas::common::get_static_node_handle(),node_prefix){}
+    explicit RobotDynamicProvider(const std::string& node_prefix):RobotDynamicProvider(sas::common::get_static_node_handle(),node_prefix){}
-// #endif
+#endif
-    explicit RobotDynamicsServer(const std::shared_ptr<Node> &node,  const std::string& topic_prefix="GET_FROM_NODE");
+    explicit RobotDynamicProvider(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
+    RobotDynamicProvider(ros::NodeHandle& publisher_nodehandle, ros::NodeHandle& subscriber_nodehandle, const std::string& node_prefix=ros::this_node::getName());
 
     void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque);
 
     void set_world_to_base_tf(const DQ& world_to_base_tf);
 
     bool is_enabled() const;
-    std::string get_topic_prefix() const {return topic_prefix_;}
+    std::string get_topic_prefix() const {return node_prefix_;}
 
 };
 

include/sas_robot_driver_franka/robot_interface_franka.h

@@ -40,12 +40,12 @@
 #include <franka/robot.h>
 #include <franka/gripper.h>
 #include <franka/exception.h>
-#include <sas_robot_driver_franka/generator/motion_generator.h>
+#include "generator/motion_generator.h"
-#include <sas_robot_driver_franka/generator/quadratic_program_motion_generator.h>
-#include <sas_robot_driver_franka/generator/custom_motion_generation.h>
 #include <thread>
+#include "generator/quadratic_program_motion_generator.h"
 #include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
 #include <atomic>
+#include "generator/custom_motion_generation.h"
 
 using namespace DQ_robotics;
 using namespace Eigen;

include/sas_robot_driver_franka/sas_robot_driver_franka.h

@@ -40,16 +40,18 @@
 
 #include <dqrobotics/DQ.h>
 
-#include <sas_core/sas_robot_driver.hpp>
+#include <sas_robot_driver/sas_robot_driver.h>
-#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
+#include "robot_interface_franka.h"
-#include <rclcpp/rclcpp.hpp>
+#include <ros/common.h>
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_clock/sas_clock.h>
+#include <dqrobotics/utils/DQ_Math.h>
+#include <ros/this_node.h>
+#include <rosconsole/macros_generated.h>
 
 using namespace DQ_robotics;
 using namespace Eigen;
 
-struct RobotInterfaceFranka::FrankaInterfaceConfiguration;  // Forward declaration
-
 namespace sas
 {
 
@@ -61,19 +63,18 @@ struct RobotDriverFrankaConfiguration
     double speed;
     RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
     DQ robot_reference_frame = DQ(0);
+    bool automatic_error_recovery = false;
 };
 
 
-class RobotDriverFranka: public sas::RobotDriver
+class RobotDriverFranka: public RobotDriver
 {
 private:
-    std::shared_ptr<Node> node_;
-
     RobotDriverFrankaConfiguration configuration_;
 
     std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
 
-    std::shared_ptr<qros::RobotDynamicsServer> robot_dynamic_provider_sptr_;
+    qros::RobotDynamicProvider* robot_dynamic_provider_;
 
     //Joint positions
     VectorXd joint_positions_;
@@ -100,8 +101,7 @@ public:
     ~RobotDriverFranka();
 
     RobotDriverFranka(
-        const std::shared_ptr<Node> &node,
+        qros::RobotDynamicProvider* robot_dynamic_provider,
-        const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider,
         const RobotDriverFrankaConfiguration& configuration,
         std::atomic_bool* break_loops
         );
@@ -113,7 +113,7 @@ public:
     VectorXd get_joint_velocities() override;
     void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
 
-    VectorXd get_joint_torques() override;
+    VectorXd get_joint_forces() override;
 
     void connect() override;
     void disconnect() override;

launch/sas_robot_driver_franka_example.py

@@ -1,45 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-import os
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='sas_robot_driver_franka',
-            executable='sas_robot_driver_franka_node',
-            name='arm3',
-            parameters=[{
-                "robot_ip_address": "172.16.0.4",
-                "thread_sampling_time_sec": 0.004,
-                # "thread_sampling_time_nsec": 4000000,
-                "q_min": [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895],
-                "q_max": [2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895],
-                "force_upper_limits_scaling_factor": 4.0,
-                "upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0],
-                "upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0],
-                "robot_mode": "VelocityControl",
-                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
-            }],
-            output="screen"
-        ),
-        Node(
-            package='sas_robot_driver_franka',
-            executable='sas_robot_driver_franka_coppelia_node',
-            name='arm3_coppelia',
-            parameters=[{
-                "thread_sampling_time_sec": 0.008,
-                "vrep_ip": os.environ["VREP_IP"],
-                "vrep_port": 20012,
-                "vrep_dynamically_enabled": True,
-                "using_real_robot": True,
-                "vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
-                                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"],
-                "robot_topic_prefix": "/arm3",
-                "robot_mode": "VelocityControl",
-                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
-            }],
-            output="screen"
-        ),
-
-    ])

launch/sas_robot_driver_franka_hand_example.py

@@ -1,24 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-import os
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='sas_robot_driver_franka',
-            executable='sas_robot_driver_franka_hand_node',
-            name='arm3_hand',
-            parameters=[{
-                "robot_ip_address": "172.16.0.2",
-                # "thread_sampling_time_nsec": 20000000,  # 20ms , 50Hz
-                "thread_sampling_time_sec": 0.02,
-                "default_force": 2.0,
-                "default_speed": 0.07,
-                "default_epsilon_inner": 0.007,
-                "default_epsilon_outer": 0.007,
-            }],
-            output="screen"
-        ),
-
-    ])

launch/sas_robot_driver_franka_simulation_example.py

@@ -1,27 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-import os
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='sas_robot_driver_franka',
-            executable='sas_robot_driver_franka_coppelia_node',
-            name='arm3_coppelia',
-            parameters=[{
-                "thread_sampling_time_sec": 0.008,
-                "vrep_ip": os.environ["VREP_IP"],
-                "vrep_port": 20012,
-                "vrep_dynamically_enabled": True,
-                "using_real_robot": False,
-                "vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
-                                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"],
-                "robot_topic_prefix": "/arm3",
-                "robot_mode": "VelocityControl",
-                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
-            }],
-            output="screen"
-        ),
-
-    ])

msg/GripperState.msg

@@ -0,0 +1,5 @@
+float32 width
+float32 max_width
+bool is_grasped
+uint16 temperature
+uint64 duration_ms

package.xml

@@ -1,32 +1,94 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
-<package format="3">
   <name>sas_robot_driver_franka</name>
   <version>0.0.0</version>
   <description>The sas_driver_franka package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="moonshot@todo.todo">moonshot</maintainer>
 
-  <license>LGPLv3</license>
-  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>rclcpp</depend>
+  <!-- One license tag required, multiple allowed, one license per tag -->
-  <depend>rclpy</depend>
+  <!-- Commonly used license strings: -->
-  <depend>tf2_ros</depend>
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <depend>tf2</depend>
+  <license>TODO</license>
-  <depend>std_msgs</depend>
-  <depend>sas_core</depend>
-  <depend>sas_robot_driver</depend>
-  <depend>sas_common</depend>
-  <depend>sas_robot_driver_franka_interface</depend>
 
 
-  <test_depend>ament_lint_auto</test_depend>
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <test_depend>ament_lint_common</test_depend>
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <test_depend>ament_copyright</test_depend>
+  <!-- Example: -->
-  <test_depend>ament_flake8</test_depend>
+  <!-- <url type="website">http://wiki.ros.org/sas_driver_franka</url> -->
-  <test_depend>ament_pep257</test_depend>
 
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sas_clock</build_depend>
+  <build_depend>sas_robot_driver</build_depend>
+  <build_depend>sas_common</build_depend>
+  <build_depend>sas_patient_side_manager</build_depend>
+  <build_depend>pybind11_catkin</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sas_clock</build_export_depend>
+  <build_export_depend>sas_robot_driver</build_export_depend>
+  <build_export_depend>sas_common</build_export_depend>
+  <build_export_depend>sas_patient_side_manager</build_export_depend>
+  <build_export_depend>pybind11_catkin</build_export_depend>
+
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sas_clock</exec_depend>
+  <exec_depend>sas_robot_driver</exec_depend>
+  <exec_depend>sas_common</exec_depend>
+  <exec_depend>sas_patient_side_manager</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <build_type>ament_cmake</build_type>
+    <!-- Other tools can request additional information be placed here -->
+
   </export>
-</package>
+</package>

qt/configuration_window/mainwindow.h

@@ -7,7 +7,7 @@
 
 #include <QMainWindow>
 #include "qspinbox.h"
-#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
+#include <sas_robot_driver_franka/robot_interface_franka.h>
 
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }

sas_robot_driver_franka/__init__.py

@@ -1,229 +0,0 @@
-"""
-"""
-from typing import Union
-
-from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamicsClient, RobotDynamicsServer
-
-import rclpy
-from rclpy.node import Node
-from rclpy.client import Client
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
-from sas_robot_driver_franka_interfaces.srv import Move, Grasp
-from std_srvs.srv import Trigger
-from sas_robot_driver_franka_interfaces.msg import GripperState
-import os
-import threading
-from queue import Queue
-import time
-import struct
-
-MOVE_TOPIC_SUFFIX = "move"
-GRASP_TOPIC_SUFFIX = "grasp"
-STATUS_TOPIC_SUFFIX = "gripper_status"
-
-
-class FrankaGripperInterface:
-    """
-    Non blocking interface to control the Franka gripper
-    """
-
-    def __init__(self, node: Node, robot_prefix):
-        self.last_message = None
-        self.robot_prefix = robot_prefix
-        self.node = node
-        self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX))
-        self._moving = False
-        self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
-        self._grasping = False
-        self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing"))
-        self._homing = False
-        self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
-                                                          self._status_callback, 10)
-
-        self.service_future: Union[rclpy.Future, None] = None
-
-        # gripper state
-        self.state_width = None
-        self.state_max_width = None
-        self.state_temperature = None
-        self.state_is_grasped = None
-        self.spin_handler = self._default_spin_handler
-
-    def _default_spin_handler(self):
-        rclpy.spin_once(self.node)
-
-    def _is_service_ready(self, service: Client):
-        try:
-            # self.node.get_logger().info("Waiting for service: " + service.service_name)
-            ret = service.wait_for_service(timeout_sec=0.1)
-            return ret
-        except Exception as e:
-            self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
-            return False
-
-    def is_enabled(self):
-        if self.state_width is None:
-            return False
-        if not self._is_service_ready(self.move_service):
-            return False
-        if not self._is_service_ready(self.grasp_service):
-            return False
-        return True
-
-    def _status_callback(self, msg: GripperState):
-        self.state_width = msg.width
-        self.state_max_width = msg.max_width
-        self.state_temperature = msg.temperature
-        self.state_is_grasped = msg.is_grasped
-    def home(self):
-        """
-        Reinitialize the gripper
-        :return: None
-        """
-        if self.is_busy():
-            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._home()
-
-    def move(self, width, speed=0):
-        """
-        Move the gripper to a specific width
-        :param width: width in meters
-        :param speed: speed in meters per second
-        :return: None
-        """
-        if self.is_busy():
-            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._move(width, speed)
-
-    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
-        """
-        Grasp an object with the gripper
-        :param width:
-        :param force:
-        :param speed:
-        :param epsilon_inner:
-        :param epsilon_outer:
-        :return:
-        """
-        if self.is_busy():
-            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
-
-    def get_max_width(self):
-        """ Get the maximum width of the gripper """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_max_width
-
-    def get_width(self):
-        """ Get the current width of the gripper """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_width
-
-    def get_temperature(self):
-        """ Get the temperature of the gripper """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_temperature
-
-    def is_grasped(self):
-        """ Check if an object is grasped """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_is_grasped
-
-    def is_moving(self):
-        """ Check if the gripper is currently moving """
-        return self._moving
-
-    def is_grasping(self):
-        """ Check if the gripper is currently grasping """
-        return self._grasping
-
-    def is_busy(self):
-        """ Check if the gripper is currently moving or grasping """
-        return self._moving or self._grasping or self.service_future is not None
-
-    def is_done(self):
-        """ Check if the gripper is done moving or grasping """
-        if not self.is_busy():
-            self.node.get_logger().warn("Gripper is not moving or grasping")
-            return False
-        else:
-            if self.service_future is not None:
-                if self.service_future.done():
-                    return True
-                return False
-            else:
-                return True
-
-    def get_result(self):
-        """
-        Get the result of the last action
-        :return:
-        """
-        if self.service_future is not None:
-            if self.service_future.done():
-                response = self.service_future.result()
-                if isinstance(response, Move.Response):
-                    self._moving = False
-                elif isinstance(response, Grasp.Response):
-                    self._grasping = False
-                elif isinstance(response, Trigger.Response):
-                    self._homing = False
-                else:
-                    raise Exception("Invalid response type")
-                self.service_future = None
-                self.last_message = hasattr(response, "message") and response.message or ""
-                return response.success
-            else:
-                raise Exception("No result available")
-        else:
-            raise Exception("No result available")
-    def get_last_message(self):
-        return self.last_message
-
-    def wait_until_done(self):
-        """
-        Wait until the gripper is done moving or grasping
-        :return: success
-        """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        if not self.is_busy():
-            return
-        while not self.is_done():
-            self.spin_handler()
-            time.sleep(0.01)
-    def _home(self):
-        self._homing = True
-        # self.node.get_logger().info("Homing gripper")
-        request = Trigger.Request()
-        # self.node.get_logger().info("Calling home service")
-        self.service_future = self.home_service.call_async(request)
-
-    def _move(self, width, speed):
-        self._moving = True
-        # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
-        request = Move.Request()
-        request.width = float(width)
-        request.speed = float(speed)
-        # self.node.get_logger().info("Calling move service")
-        self.service_future = self.move_service.call_async(request)
-
-    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
-        self._grasping = True
-        # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
-        request = Grasp.Request()
-        request.width = float(width)
-        request.force = float(force)
-        request.speed = float(speed)
-        request.epsilon_inner = float(epsilon_inner)
-        request.epsilon_outer = float(epsilon_outer)
-        # self.node.get_logger().info("Calling grasp service")
-        self.service_future = self.grasp_service.call_async(request)
-
-
-    def set_spin_handler(self, spin_handler):
-        self.spin_handler = spin_handler

scripts/example_gripper_control.py

@@ -1,58 +1,48 @@
-import threading
+import rospy
 
-import rclpy
 from sas_robot_driver_franka import FrankaGripperInterface
-import time
 
 
-def main_loop(node):
+def main_loop():
+    rate = rospy.Rate(10)  # 10 Hz
     iteration_ = 0
-    node_name = node.get_name()
+
-    hand1 = FrankaGripperInterface(node, "/franka_hand_1")
+    hand1 = FrankaGripperInterface("/franka_hand_1")
-    logger = rclpy.node.get_logger(node_name)
 
     while not hand1.is_enabled():
-        logger.info("Waiting for gripper to be enabled...")
+        rospy.loginfo("Waiting for gripper to be enabled...")
-        time.sleep(0.1)
+        rate.sleep()
-    rclpy.node.get_logger(node_name).info("Gripper enabled!")
+    rospy.loginfo("Gripper enabled!")
 
-    def spin_all(node_):
+    rate = rospy.Rate(2)  # 0.5 Hz
-        while rclpy.ok():
-            rclpy.spin_once(node_, timeout_sec=0.1)
-            time.sleep(0.1)
-            
-    thread = threading.Thread(target=spin_all, args=(node, ), daemon=True)
-    thread.start()
-    
-    rate = node.create_rate(2)
 
-    while rclpy.ok():
+    while not rospy.is_shutdown():
-        logger.info("Main loop running...")
+        rospy.loginfo("Main loop running...")
 
         # Get the temperature of the gripper
         temperature = hand1.get_temperature()
-        logger.info(f"Temperature: {temperature}")
+        rospy.loginfo(f"Temperature: {temperature}")
         max_width = hand1.get_max_width()
-        logger.info(f"Max width: {max_width}")
+        rospy.loginfo(f"Max width: {max_width}")
         width = hand1.get_width()
-        logger.info(f"Width: {width}")
+        rospy.loginfo(f"Width: {width}")
         is_grasped = hand1.is_grasped()
-        logger.info(f"Is grasped: {is_grasped}")
+        rospy.loginfo(f"Is grasped: {is_grasped}")
         is_moving = hand1.is_moving()
-        logger.info(f"Is moving: {is_moving}")
+        rospy.loginfo(f"Is moving: {is_moving}")
         is_grasping = hand1.is_grasping()
-        logger.info(f"Is grasping: {is_grasping}")
+        rospy.loginfo(f"Is grasping: {is_grasping}")
-        logger.warn("calling move(0.01)")
+        rospy.logwarn("calling move(0.01)")
         if not hand1.is_busy():
             hand1.grasp(0.01)
         else:
-            logger.warn("Gripper is busy. Waiting...")
+            rospy.logwarn("Gripper is busy. Waiting...")
         result_ready = hand1.is_done()
         if not result_ready:
-            logger.info("Waiting for gripper to finish moving...")
+            rospy.loginfo("Waiting for gripper to finish moving...")
         else:
             result = hand1.get_result()
-            logger.info(f"Result: {result}")
+            rospy.loginfo(f"Result: {result}")
 
 
         # Check if there is a response in the queue
@@ -60,12 +50,7 @@ def main_loop(node):
         iteration_ += 1
         rate.sleep()
 
-    rclpy.shutdown()
-    thread.join()
-
 
 if __name__ == "__main__":
-    rclpy.init()
+    rospy.init_node("example_gripper_control_node")
-    node_name_ = "example_gripper_control_node"
+    main_loop()
-    NODE = rclpy.create_node(node_name_)
-    main_loop(NODE)

scripts/publish_dumy_robot_dynmaics.py

@@ -1,46 +1,29 @@
-import time
+import rospy
-
+from sas_robot_driver_franka import RobotDynamicsProvider
-import rclpy
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
-from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
 import dqrobotics as dql
 import numpy as np
 
 
-
-
 if __name__ == "__main__":
-    rclcpp_init()
+    rospy.init_node("dummy_robot_dynamics_provider")
-    rclpy.init()
-    sas_node = rclcpp_Node("dummy_robot_dynamics_server_sas")
-    node = rclpy.create_node("dummy_robot_dynamics_server")
-    dynam_provider = RobotDynamicsServer(sas_node, "/franka1")
 
+    dynam_provider = RobotDynamicsProvider("/franka1")
     t = dql.DQ([0, 0, 1])
     r = dql.DQ([1, 0, 0, 0])
     base_pose = r + 0.5 * dql.E_ * t * r
     dynam_provider.set_world_to_base_tf(base_pose)
     t_ = 0
-    node.get_logger().info("Dummy robot dynamics server started")
+    rospy.loginfo("Publishing dummy robot dynamics")
     r = dql.DQ([0, 0, 0, 1])
-    rate = node.create_rate(100)
+    rate = rospy.Rate(100)
     dummy_force = np.random.rand(3) * 100
     dummy_torque = np.random.rand(3) * 10
-    try:
-        while rclpy.ok():
-            t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
-            ee_pose = r + 0.5 * dql.E_ * t * r
-            dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
-            dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
-            dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
-            rclcpp_spin_some(sas_node)
-            rclpy.spin_once(node)
-            t_ += 0.01
-            time.sleep(0.1)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        rclcpp_shutdown()
-        node.destroy_node()
-        rclpy.shutdown()
 
+    while not rospy.is_shutdown():
+        t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
+        ee_pose = r + 0.5 * dql.E_ * t * r
+        dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
+        dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
+        dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
+        rate.sleep()
+        t_ += 0.01

scripts/subscribe_dummy_dynamic.py

@@ -1,53 +1,30 @@
-import time
+import rospy
-
+from sas_robot_driver_franka import RobotDynamicsInterface
-import rclpy
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
-from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
 import dqrobotics as dql
 import numpy as np
 
 from dqrobotics.interfaces.vrep import DQ_VrepInterface
 
-# vrep = DQ_VrepInterface()
+vrep = DQ_VrepInterface()
-# vrep.connect("192.168.10.103", 19997, 100, 10)
+vrep.connect("192.168.10.103", 19997, 100, 10)
-vrep = None
 
 if __name__ == "__main__":
-    rclcpp_init()
+    rospy.init_node("dummy_robot_dynamics_subscriber")
-    rclpy.init()
-    sas_node = rclcpp_Node("dummy_robot_dynamics_client_sas")
-    node = rclpy.create_node("dummy_robot_dynamics_client")
-    executor = rclpy.executors.MultiThreadedExecutor()
-    executor.add_node(node)
-    try:
-        executor.spin_once(timeout_sec=0.1)
-        dynam_provider = RobotDynamicsClient(sas_node, "/franka1")
-        rclcpp_spin_some(sas_node)
-        while not dynam_provider.is_enabled():
-            node.get_logger().info("Waiting for robot dynamics provider to be enabled")
-            rclcpp_spin_some(sas_node)
-            executor.spin_once(timeout_sec=0.1)
-            node.get_logger().info("retrying...")
-            time.sleep(0.5)
 
-        node.get_logger().info("Dummy robot dynamics client started")
+    dynam_provider = RobotDynamicsInterface("/franka1")
-        while rclpy.ok():
+    while not dynam_provider.is_enabled():
-            force = dynam_provider.get_stiffness_force()
+        rospy.loginfo("Waiting for robot dynamics provider to be enabled")
-            torque = dynam_provider.get_stiffness_torque()
+        rospy.sleep(1)
-            ee_pose = dynam_provider.get_stiffness_frame_pose()
+
-            if vrep is not None:
+    rospy.loginfo("Subscribing to dummy robot dynamics")
-                vrep.set_object_pose("xd1", ee_pose)
+    rate = rospy.Rate(50)
-            node.get_logger().info(f"EE Pose: {ee_pose}")
+
-            node.get_logger().info(f"Force: {force}")
+    while not rospy.is_shutdown():
-            node.get_logger().info(f"Torque: {torque}")
+        force = dynam_provider.get_stiffness_force()
-            rclcpp_spin_some(sas_node)
+        torque = dynam_provider.get_stiffness_torque()
-            executor.spin_once(timeout_sec=0.1)
+        ee_pose = dynam_provider.get_stiffness_frame_pose()
-            time.sleep(0.5)
+        vrep.set_object_pose("xd1", ee_pose)
-    except KeyboardInterrupt:
+        rospy.loginfo("EE Pose: %s", ee_pose)
-        pass
+        rospy.loginfo("Force: %s", force)
-    finally:
+        rospy.loginfo("Torque: %s", torque)
-        rclcpp_shutdown()
+        rate.sleep()
-        node.destroy_node()
-        if vrep is not None:
-            vrep.disconnect()
-        rclpy.shutdown()

setup.py

@@ -0,0 +1,12 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=['sas_robot_driver_franka'],
+    package_dir={'': 'src'},
+)
+
+setup(**setup_args)

src/coppelia/sas_robot_driver_coppelia.cpp

@@ -1,7 +1,7 @@
-#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
+#include "sas_robot_driver_coppelia.h"
 
 
-namespace qros
+namespace sas
 {
 
 RobotDriverCoppelia::~RobotDriverCoppelia() {
@@ -9,10 +9,9 @@ RobotDriverCoppelia::~RobotDriverCoppelia() {
     disconnect();
 }
 
-RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
+RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
         configuration_(configuration),
-        node_sptr_(node_sptr),
+        clock_(configuration.thread_sampling_time_nsec),
-        clock_(configuration.thread_sampling_time_sec),
         break_loops_(break_loops),
         robot_mode_(ControlMode::Position),
         vi_(std::make_shared<DQ_VrepInterface>())
@@ -22,11 +21,11 @@ RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr,
     joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()};
     if(configuration_.using_real_robot)
     {
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using real robot, Instantiating robot interface to real driver at ["+configuration_.robot_topic_prefix+"].");
+        ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver.");
-        real_robot_interface_ = std::make_shared<sas::RobotDriverClient>(node_sptr_, configuration_.robot_topic_prefix);
+        real_robot_interface_ = std::make_shared<RobotDriverInterface>(nh, configuration_.robot_topic_prefix);
     }else{
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
+        ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
-        robot_provider_ = std::make_shared<sas::RobotDriverServer>(node_sptr_, configuration_.robot_topic_prefix);
+        robot_provider_ = std::make_shared<RobotDriverProvider>(nh, configuration_.robot_topic_prefix);
         std::string _mode_upper = configuration_.robot_mode;
         std::transform(_mode_upper.begin(), _mode_upper.end(), _mode_upper.begin(), ::toupper);
         if(_mode_upper == "POSITIONCONTROL"){
@@ -34,7 +33,7 @@ RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr,
         }else if(_mode_upper == "VELOCITYCONTROL"){
             robot_mode_ = ControlMode::Velocity;
         }else{
-            throw std::invalid_argument("[" + std::string(node_sptr_->get_name()) + "]::Robot mode must be either 'position' or 'velocity'");
+            throw std::invalid_argument("[" + ros::this_node::getName() + "]::Robot mode must be either 'position' or 'velocity'");
         }
     }
 
@@ -60,11 +59,11 @@ void RobotDriverCoppelia::_update_vrep_velocity(const VectorXd & joint_velocity)
 
 void RobotDriverCoppelia::_start_control_loop(){
     clock_.init();
-    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Starting control loop...");
+    ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Starting control loop...");
     while(!_should_shutdown()){
         clock_.update_and_sleep();
-        rclcpp::spin_some(node_sptr_);
+        ros::spinOnce();
-        if(!rclcpp::ok()){break_loops_->store(true);}
+        if(!ros::ok()){break_loops_->store(true);}
         if(configuration_.using_real_robot){
             // real_robot_interface_
             auto joint_position = real_robot_interface_->get_joint_positions();
@@ -83,7 +82,7 @@ void RobotDriverCoppelia::_start_control_loop(){
                         //                    try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
                     }
                     else{
-                        RCLCPP_DEBUG_STREAM(node_sptr_->get_logger(), "::Velocity control is not enabled.");
+                        ROS_DEBUG_STREAM("[" + ros::this_node::getName() + "]::Velocity control is not enabled.");
                     }
                     target_joint_positions = simulated_joint_positions_;
                 }else{
@@ -104,29 +103,29 @@ void RobotDriverCoppelia::_start_control_loop(){
 
 int RobotDriverCoppelia::start_control_loop(){
     try{
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Waiting to connect with coppelia...");
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with coppelia...");
         connect();
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Connected to coppelia.");
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Connected to coppelia.");
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Initializing...");
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Initializing...");
         initialize();
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::initialized.");
+        ROS_INFO_STREAM(ros::this_node::getName() << "::initialized.");
 
         _start_control_loop();
     }
     catch(const std::exception& e)
     {
-        RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Error or exception caught::" << e.what());
+        ROS_WARN_STREAM(ros::this_node::getName() + "::Error or exception caught::" << e.what());
     }
     catch(...)
     {
-        RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Unexpected error or exception caught");
+        ROS_WARN_STREAM(ros::this_node::getName() + "::Unexpected error or exception caught");
     }
-    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Deinitializing...");
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Deinitializing...");
     deinitialize();
-    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::deinitialized.");
+    ROS_INFO_STREAM(ros::this_node::getName() << "::deinitialized.");
-    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnecting from coppelia...");
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnecting from coppelia...");
     disconnect();
-    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnected from coppelia.");
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnected from coppelia.");
 
     return 0;
 }
@@ -137,7 +136,7 @@ void RobotDriverCoppelia::connect(){
     if(!ret){
         throw std::runtime_error("[RobotDriverCoppelia]::connect::Could not connect to Vrep");
     }
-    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::connect::Connected to Vrep");
+    ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::connect::Connected to Vrep");
 }
 void RobotDriverCoppelia::disconnect(){
     vi_->disconnect_all();
@@ -146,29 +145,27 @@ void RobotDriverCoppelia::disconnect(){
 void RobotDriverCoppelia::initialize(){
     if(configuration_.using_real_robot)
     {
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
-        rclcpp::spin_some(node_sptr_);
+        ros::spinOnce();
         int count = 0;
-        while (!real_robot_interface_->is_enabled(sas::RobotDriver::Functionality::PositionControl)) {
+        while (!real_robot_interface_->is_enabled()) {
-            rclcpp::spin_some(node_sptr_);
+            ros::spinOnce();
-            // std::cout<<"Waiting for real robot interface to initialize..."<<std::endl;
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            rclcpp::spin_some(node_sptr_);
             count++;
             if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
-                RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
+                ROS_ERROR_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
-                throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
+                throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
             }
-            if(!rclcpp::ok()) {
+            if(!ros::ok()) {
-                RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::ROS shutdown received. Exiting...");
+                ROS_WARN_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved. Exiting...");
-                throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::ROS shutdown received, not OK.");
+                throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved, not OK.");
             }
         }
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface initialized.");
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface initialized.");
         joint_limits_ = real_robot_interface_->get_joint_limits();
         _update_vrep_position(real_robot_interface_->get_joint_positions(), true);
     }else{
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Simulation mode.");
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Simulation mode.");
         // initialization information for robot driver provider
         /**
         * TODO: check for making sure real robot is not actually connected
@@ -209,12 +206,13 @@ void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
      */
     simulation_thread_started_ = true;
     try{
-        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +
-        sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC);
+                        "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
-        double tau = VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC;
+        sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC);
+        double tau = static_cast<double>(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC) * 1e-9;
         auto current_joint_positions = simulated_joint_positions_;
         clock.init();
-        while (!(*break_loops_) && rclcpp::ok()) {
+        while (!(*break_loops_) && ros::ok()) {
 
             current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model
             // cap joint limit
@@ -232,13 +230,12 @@ void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
             clock.update_and_sleep();
         }
     }catch(std::exception &e){
-        RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" << e.what());
+        ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" + e.what());
         simulation_thread_started_ = false;
     }catch(...){
-        RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
+        ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
         simulation_thread_started_ = false;
     }
-    break_loops_->store(true);
 
 }
 

src/coppelia/sas_robot_driver_coppelia.h

@@ -38,22 +38,23 @@
 #include <vector>
 #include <memory>
 #include <dqrobotics/DQ.h>
-#include <sas_core/sas_clock.hpp>
+#include <sas_robot_driver/sas_robot_driver.h>
+#include <sas_clock/sas_clock.h>
 // #include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
 #include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
 #include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
 // #include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
 #include <thread>
 #include <atomic>
-#include <sas_robot_driver/sas_robot_driver_server.hpp>
+#include <sas_robot_driver/sas_robot_driver_interface.h>
-#include <sas_robot_driver/sas_robot_driver_client.hpp>
+#include <sas_robot_driver/sas_robot_driver_provider.h>
 
-#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC 0.002  // 2ms, 500Hz
+#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC 2000000  // 2ms, 500Hz
 #define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500
 using namespace DQ_robotics;
 using namespace Eigen;
 
-namespace qros
+namespace sas
 {
 
 
@@ -61,7 +62,7 @@ namespace qros
 struct RobotDriverCoppeliaConfiguration
 {
 
-    double thread_sampling_time_sec;  // frontend vrep update rate
+    int thread_sampling_time_nsec;  // frontend vrep update rate
     int vrep_port;
     std::string vrep_ip;
     std::vector<std::string> vrep_joint_names;
@@ -83,16 +84,15 @@ private:
     };
 
     RobotDriverCoppeliaConfiguration configuration_;
-    std::shared_ptr<Node> node_sptr_;
 
-    sas::Clock clock_;
+    Clock clock_;
     std::atomic_bool* break_loops_;
     bool _should_shutdown() const {return (*break_loops_);}
     ControlMode robot_mode_;
 
     std::shared_ptr<DQ_VrepInterface> vi_;
-    std::shared_ptr<sas::RobotDriverClient> real_robot_interface_ = nullptr;
+    std::shared_ptr<sas::RobotDriverInterface> real_robot_interface_ = nullptr;
-    std::shared_ptr<sas::RobotDriverServer> robot_provider_ = nullptr;
+    std::shared_ptr<sas::RobotDriverProvider> robot_provider_ = nullptr;
 
     // backend thread for simulaton
     /**
@@ -123,7 +123,7 @@ public:
     RobotDriverCoppelia()=delete;
     ~RobotDriverCoppelia();
 
-    RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
+    RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
 
     int start_control_loop();  // public entry point
 

src/deprecated/robot_coppelia_ros_interface_node.cpp

@@ -39,7 +39,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_franka.h"
+#include "../../include/sas_robot_driver_franka/sas_robot_driver_franka.h"
 #include "robot_coppelia_ros_interface.h"
 
 /*********************************************

src/hand/qros_effector_driver_franka_hand.cpp

@@ -27,12 +27,8 @@
 #
 # ################################################################
 */
-#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
+#include "qros_effector_driver_franka_hand.h"
 
-#include <franka/exception.h>
-
-using namespace std::placeholders;
-using namespace sas_robot_driver_franka_interfaces;
 
 
 namespace qros
@@ -51,21 +47,19 @@ namespace qros
     EffectorDriverFrankaHand::EffectorDriverFrankaHand(
         const std::string& driver_node_prefix,
         const EffectorDriverFrankaHandConfiguration& configuration,
-        std::shared_ptr<Node> node,
+        ros::NodeHandle& node_handel,
         std::atomic_bool* break_loops):
         driver_node_prefix_(driver_node_prefix),
         configuration_(configuration),
-        node_(node),
+        node_handel_(node_handel),
         break_loops_(break_loops)
     {
         gripper_sptr_ = nullptr;
-        grasp_srv_ = node->create_service<srv::Grasp>(driver_node_prefix_ + "/grasp",
+        grasp_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/grasp",
-            std::bind(&EffectorDriverFrankaHand::_grasp_srv_callback, this, _1, _2));
+                                                      &EffectorDriverFrankaHand::_grasp_srv_callback, this);
-        move_srv_ = node->create_service<srv::Move>(driver_node_prefix_ + "/move",
+        move_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/move",
-            std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2));
+                                                     &EffectorDriverFrankaHand::_move_srv_callback, this);
-        homing_srv_ = node->create_service<std_srvs::srv::Trigger>(driver_node_prefix_ + "/homing",
+        gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(
-            std::bind(&EffectorDriverFrankaHand::_home_srv_callback, this, _1, _2));
-        gripper_status_publisher_ = node->create_publisher<msg::GripperState>(
             driver_node_prefix_ + "/gripper_status", 1);
     }
 
@@ -80,48 +74,56 @@ namespace qros
     }
 
 
-    void EffectorDriverFrankaHand::start_control_loop() {
+    void EffectorDriverFrankaHand::start_control_loop()
-        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
+    {
+        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
         clock.init();
-        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
+        ROS_INFO_STREAM(
-        RCLCPP_WARN_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::Gripper READY.");
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
         while (!(*break_loops_))
         {
-            if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
+            if (!_is_connected()) throw std::runtime_error(
+                "[" + ros::this_node::getName() +
+                "]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
 
             if (!status_loop_running_)
             {
-                RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
+                ROS_WARN_STREAM(
+                    "["+ ros::this_node::getName()+
+                    "]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
                 break_loops_->store(true);
                 break;
             }
 
             clock.update_and_sleep();
-            spin_some(node_);
+            ros::spinOnce();
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
+        ROS_INFO_STREAM(
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
     }
 
 
     void EffectorDriverFrankaHand::connect()
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
+        ROS_WARN_STREAM(
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
         return;
 #endif
         gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
         if (!_is_connected()) throw std::runtime_error(
-            "[" + std::string(node_->get_name())+
+            "[" + ros::this_node::getName() +
             "]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
     }
 
     void EffectorDriverFrankaHand::disconnect() noexcept
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
+        ROS_WARN_STREAM(
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
         return;
 #endif
-        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnecting...");
+        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnecting...");
         gripper_sptr_->~Gripper();
         gripper_sptr_ = nullptr;
     }
@@ -132,42 +134,35 @@ namespace qros
     void EffectorDriverFrankaHand::gripper_homing()
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
+        ROS_WARN_STREAM(
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
         return;
 #endif
         if (!_is_connected()) throw std::runtime_error(
-            "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
+            "[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
         auto ret = gripper_sptr_->homing();
         if (!ret)
         {
-            throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
+            throw std::runtime_error(
+                "[" + ros::this_node::getName() +
+                "]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
+        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
     }
 
     void EffectorDriverFrankaHand::initialize()
     {
         if (!_is_connected()) throw std::runtime_error(
-            "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
+            "[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
-        if (configuration_.initialize_with_homing) {
+        gripper_homing();
-            gripper_homing();
-        }
         // start gripper status loop
         status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
-        // check status loop with timeout
-        auto time_now = std::chrono::system_clock::now();
-        auto time_out = time_now + std::chrono::seconds(5);
-        while (!status_loop_running_)
-        {
-            if (std::chrono::system_clock::now() > time_out){throw std::runtime_error("[" + std::string(node_->get_name()) + "]::[EffectorDriverFrankaHand]::initialize::Could not start status loop.");}
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        }
     }
 
     void EffectorDriverFrankaHand::deinitialize()
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
+        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
         return;
 #endif
         if (_is_connected())
@@ -181,144 +176,130 @@ namespace qros
     }
 
 
-    bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> &req, std::shared_ptr<srv::Grasp::Response> res)
+    bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req,
+                                                       sas_robot_driver_franka::Grasp::Response& res)
     {
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
+        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
-        auto force = req->force;
+        auto force = req.force;
-        auto speed = req->speed;
+        auto speed = req.speed;
-        auto epsilon_inner = req->epsilon_inner;
+        auto epsilon_inner = req.epsilon_inner;
-        auto epsilon_outer = req->epsilon_outer;
+        auto epsilon_outer = req.epsilon_outer;
         if (force <= 0.0) force = configuration_.default_force;
         if (speed <= 0.0) speed = configuration_.default_speed;
         if (epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
         if (epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req->width));
+        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width));
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
+        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
         bool ret = false;
         bool function_ret = true;
+        gripper_in_use_.lock();
 #ifdef IN_TESTING
         ret = true;
         std::this_thread::sleep_for(std::chrono::milliseconds(2000));
 #else
         try
         {
-            std::lock_guard<std::mutex> lock(gripper_in_use_);
+            ret = gripper_sptr_->grasp(req.width, speed, force, epsilon_inner, epsilon_outer);
-            ret = gripper_sptr_->grasp(req->width, speed, force, epsilon_inner, epsilon_outer);
         }catch(franka::CommandException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + std::string(e.what()));
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
             function_ret = false;
         }catch(franka::NetworkException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + std::string(e.what()));
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
             function_ret = false;
         }
 #endif
-        res->set__success(ret);
+        gripper_in_use_.unlock();
+        res.success = ret;
         return function_ret;
     }
 
 
-    bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> &req, std::shared_ptr<srv::Move::Response> res)
+    bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req,
+                                                      sas_robot_driver_franka::Move::Response& res)
     {
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
+        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
-        auto speed = req->speed;
+        auto speed = req.speed;
         if (speed <= 0.0) speed = configuration_.default_speed;
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width));
+        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req.width));
         bool ret = false;
         bool function_ret = true;
+        gripper_in_use_.lock();
 #ifdef IN_TESTING
         ret = true;
         std::this_thread::sleep_for(std::chrono::milliseconds(2000));
 #else
         try
         {
-            std::lock_guard<std::mutex> lock(gripper_in_use_);
+            ret = gripper_sptr_->move(req.width, speed);
-            ret = gripper_sptr_->move(req->width, speed);
         }catch(franka::CommandException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + std::string(e.what()));
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
             function_ret = false;
         }catch(franka::NetworkException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + std::string(e.what()));
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
             function_ret = false;
         }
 #endif
-        res->set__success(ret);
+        gripper_in_use_.unlock();
-        return function_ret;
+        res.success = ret;
-    }
-
-
-    bool EffectorDriverFrankaHand::_home_srv_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request> &req, std::shared_ptr<std_srvs::srv::Trigger::Response> res)
-    {
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::Homing...");
-        bool ret = false;
-        bool function_ret = true;
-#ifdef IN_TESTING
-        ret = true;
-        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-#else
-        try
-        {
-            std::lock_guard<std::mutex> lock(gripper_in_use_);
-            ret = gripper_sptr_->homing();
-        }catch(franka::CommandException& e)
-        {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::CommandException::" + std::string(e.what()));
-            res->set__message("_home_srv_callback::CommandException::" + std::string(e.what()));
-            function_ret = false;
-            ret = false;
-        }catch(franka::NetworkException& e)
-        {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::NetworkException::" + std::string(e.what()));
-            res->set__message("_home_srv_callback::NetworkException::" + std::string(e.what()));
-            function_ret = false;
-            ret = false;
-        }
-#endif
-        res->set__success(ret);
         return function_ret;
     }
 
 
     void EffectorDriverFrankaHand::_gripper_status_loop()
     {
-        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
+        status_loop_running_ = true;
-        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
+        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
+        ROS_INFO_STREAM(
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
+        ;
         clock.init();
         try
         {
-            status_loop_running_ = true;
             while (status_loop_running_)
             {
-                if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
+                bool should_unlock = false;
+                if (!_is_connected()) throw std::runtime_error(
+                    "[" + ros::this_node::getName() +
+                    "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
                 try
                 {
 #ifdef IN_TESTING
-                    RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+                    ROS_WARN_STREAM(
-                    throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+                        "["+ ros::this_node::getName()+
+                        "]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+                    throw std::runtime_error(
+                        "[" + ros::this_node::getName() +
+                        "]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
 #endif
-                    msg::GripperState msg;
-                    {
 #ifdef BLOCK_READ_IN_USED
-                        std::lock_guard<std::mutex> lock(gripper_in_use_);
+                    gripper_in_use_.lock();
+                    should_unlock = true;
 #endif
-                        franka::GripperState gripper_state = gripper_sptr_->readOnce();
+                    franka::GripperState gripper_state = gripper_sptr_->readOnce();
-                        msg.set__width(static_cast<float>(gripper_state.width));
+#ifdef BLOCK_READ_IN_USED
-                        msg.set__max_width(static_cast<float>(gripper_state.max_width));
+                    gripper_in_use_.unlock();
-                        msg.set__is_grasped(gripper_state.is_grasped);
+#endif
-                        msg.set__temperature(gripper_state.temperature);
+                    sas_robot_driver_franka::GripperState msg;
-                        msg.set__duration_ms(gripper_state.time.toMSec());
+                    msg.width = static_cast<float>(gripper_state.width);
-                    }
+                    msg.max_width = static_cast<float>(gripper_state.max_width);
-                    gripper_status_publisher_->publish(msg);
+                    msg.is_grasped = gripper_state.is_grasped;
+                    msg.temperature = gripper_state.temperature;
+                    msg.duration_ms = gripper_state.time.toMSec();
+                    gripper_status_publisher_.publish(msg);
                 }
                 catch (...)
                 {
-                    RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
+#ifdef BLOCK_READ_IN_USED
-                    msg::GripperState msg;
+                    if (should_unlock) gripper_in_use_.unlock();
+#endif
+                    ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
+                    sas_robot_driver_franka::GripperState msg;
                     msg.width = 0.0;
-                    gripper_status_publisher_->publish(msg);
+                    gripper_status_publisher_.publish(msg);
                 }
 
                 clock.update_and_sleep();
@@ -327,14 +308,19 @@ namespace qros
         }
         catch (std::exception& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + std::string(e.what()));
+            ROS_ERROR_STREAM(
+                "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.
+                what());
             status_loop_running_ = false;
         }
         catch (...)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
+            ROS_ERROR_STREAM(
+                "["+ ros::this_node::getName()+
+                "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
             status_loop_running_ = false;
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
+        ROS_INFO_STREAM(
+            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
     }
 } // qros

src/hand/qros_effector_driver_franka_hand.h

@@ -38,30 +38,22 @@
 #include <thread>
 #include <mutex>
 
+#include <franka/exception.h>
 // #define BLOCK_READ_IN_USED
 // #define IN_TESTING
 
 // #include <sas_robot_driver/sas_robot_driver.h>
-#include <sas_core/sas_clock.hpp>
+#include <sas_clock/sas_clock.h>
+#include <ros/ros.h>
+#include <ros/service.h>
+#include <sas_robot_driver_franka/Grasp.h>
+#include <sas_robot_driver_franka/Move.h>
+#include <sas_robot_driver_franka/GripperState.h>
 
-#include <rclcpp/service.hpp>
-#include <rclcpp/rclcpp.hpp>
 
-#ifdef IN_TESTING
-#include "local/srv/grasp.hpp"
-#include "local/srv/move.hpp"
-#include "local/msg/gripper_state.hpp"
-#else
-#include <sas_robot_driver_franka_interfaces/srv/grasp.hpp>
-#include <sas_robot_driver_franka_interfaces/srv/move.hpp>
-#include <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
-#endif
-#include <std_srvs/srv/trigger.hpp>
 
 // using namespace DQ_robotics;
 // using namespace Eigen;
-using namespace rclcpp;
-using namespace sas_robot_driver_franka_interfaces;
 
 
 namespace qros {
@@ -69,8 +61,7 @@ namespace qros {
 struct EffectorDriverFrankaHandConfiguration
 {
     std::string robot_ip;
-    bool initialize_with_homing = true;
+    int thread_sampeling_time_ns = 1e8;  // 10Hz
-    double thread_sampeling_time_s = 1e8;  // 10Hz
     double default_force = 3.0;
     double default_speed = 0.1;
     double default_epsilon_inner = 0.005;
@@ -81,7 +72,7 @@ class EffectorDriverFrankaHand{
 private:
     std::string driver_node_prefix_;
     EffectorDriverFrankaHandConfiguration configuration_;
-    std::shared_ptr<Node> node_;
+    ros::NodeHandle& node_handel_;
 
     std::shared_ptr<franka::Gripper> gripper_sptr_;
 
@@ -94,30 +85,17 @@ private:
     void _gripper_status_loop();
     std::thread status_loop_thread_;
     std::atomic_bool status_loop_running_{false};
-
+    ros::Publisher gripper_status_publisher_;
-    Publisher<msg::GripperState>::SharedPtr gripper_status_publisher_;
 
     std::mutex gripper_in_use_;
-    Service<srv::Grasp>::SharedPtr grasp_srv_;
+    ros::ServiceServer grasp_server_;
-    Service<srv::Move>::SharedPtr move_srv_;
+    ros::ServiceServer move_server_;
-    Service<std_srvs::srv::Trigger>::SharedPtr homing_srv_;
 
 public:
 
-    bool _grasp_srv_callback(
+    bool _grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res);
-        const std::shared_ptr<srv::Grasp::Request> &req,
-        std::shared_ptr<srv::Grasp::Response> res
-        );
 
-    bool _move_srv_callback(
+    bool _move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res);
-        const std::shared_ptr<srv::Move::Request> &req,
-        std::shared_ptr<srv::Move::Response> res
-        );
-
-    bool _home_srv_callback(
-        const std::shared_ptr<std_srvs::srv::Trigger::Request> &req,
-        std::shared_ptr<std_srvs::srv::Trigger::Response> res
-        );
 
     EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
     EffectorDriverFrankaHand()=delete;
@@ -126,7 +104,7 @@ public:
     EffectorDriverFrankaHand(
         const std::string &driver_node_prefix,
         const EffectorDriverFrankaHandConfiguration& configuration,
-        std::shared_ptr<Node> node,
+        ros::NodeHandle& node_handel,
         std::atomic_bool* break_loops);
 
     void start_control_loop();

src/hand/robot_interface_hand.cpp

@@ -1,33 +1,4 @@
-/*
+#include <sas_robot_driver_franka/robot_interface_hand.h>
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include <sas_robot_driver_franka/robot_interface_hand.hpp>
 
 
 

src/joint/robot_interface_franka.cpp

@@ -30,7 +30,10 @@
 */
 
 
-#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
+#include <sas_robot_driver_franka/robot_interface_franka.h>
+
+#include <ros/this_node.h>
+#include <rosconsole/macros_generated.h>
 
 
 /**
@@ -310,7 +313,6 @@ void RobotInterfaceFranka::initialize()
     initialize_flag_ = true;
 
     // initialize and set the robot to default behavior (collision behavior, impedance, etc)
-    // robot_sptr_->setDefaultBehavior();
     setDefaultBehavior(*robot_sptr_);
     robot_sptr_->setCollisionBehavior(
         franka_configuration_.lower_torque_threshold,

src/joint/sas_robot_driver_franka.cpp

@@ -30,35 +30,29 @@
 # ################################################################
 */
 
-
+#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
-#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
-#include <sas_core/sas_clock.hpp>
-#include <dqrobotics/utils/DQ_Math.h>
 
 
 namespace sas
 {
-    RobotDriverFranka::RobotDriverFranka(
+    RobotDriverFranka::RobotDriverFranka(qros::RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
-        const std::shared_ptr<Node> &node,
-        const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
-    ):
     RobotDriver(break_loops),
-    node_(node),
     configuration_(configuration),
-    robot_dynamic_provider_sptr_(robot_dynamic_provider),
+    robot_dynamic_provider_(robot_dynamic_provider),
     break_loops_(break_loops)
    {
         joint_positions_.resize(7);
         joint_velocities_.resize(7);
-        joint_torques_.resize(7);
+        joint_forces_.resize(7);
         //end_effector_pose_.resize(7);
         //joint_positions_buffer_.resize(8,0);
         //end_effector_pose_euler_buffer_.resize(7,0);
         //end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
-        // std::cout<<configuration.ip_address<<std::endl;
+        std::cout<<configuration.ip_address<<std::endl;
 
         RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
-        RCLCPP_INFO_STREAM(node_->get_logger(), "Mode: " << configuration_.mode);
+
+        std::cout<<configuration.mode<<std::endl;
 
         if (configuration_.mode == std::string("None"))
         {
@@ -92,7 +86,7 @@ namespace sas
         Vector3d force, torque;
         std::tie(force, torque) = robot_driver_interface_sptr_->get_stiffness_force_torque();
         const auto pose = robot_driver_interface_sptr_->get_stiffness_pose();
-        robot_dynamic_provider_sptr_->publish_stiffness(pose, force, torque);
+        robot_dynamic_provider_->publish_stiffness(pose, force, torque);
     }
 
 
@@ -145,15 +139,12 @@ namespace sas
     VectorXd  RobotDriverFranka::get_joint_positions()
     {
         if(robot_driver_interface_sptr_->get_err_state()) {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),
+            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
-                "["+std::string(node_->get_name())+"]::driver interface "
-                "error on:"+robot_driver_interface_sptr_->get_status_message());
             break_loops_->store(true);
         }
-        if(!ok()) {
+        if(!ros::ok()) {
-            RCLCPP_WARN_STREAM(node_->get_logger(),
+            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::ROS shutdown requested.");
-                "["+std::string(node_->get_name())+"]::driver interface exit on shotdown signal recieved."
+            break_loops_->store(true);
-                );
         }
         _update_stiffness_contact_and_pose();
         return robot_driver_interface_sptr_->get_joint_positions();
@@ -169,9 +160,7 @@ namespace sas
     {
         robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad);
         if(robot_driver_interface_sptr_->get_err_state()) {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),
+            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
-                "["+std::string(node_->get_name())+"]::driver interface "
-                "error on:"+robot_driver_interface_sptr_->get_status_message());
             break_loops_->store(true);
         }
     }
@@ -193,12 +182,10 @@ namespace sas
      */
     void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
     {
-        // desired_joint_velocities_ = desired_joint_velocities;
+        desired_joint_velocities_ = desired_joint_velocities;
         robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
         if(robot_driver_interface_sptr_->get_err_state()) {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),
+            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
-                                "["+std::string(node_->get_name())+"]::driver interface "
-                                "error on:"+robot_driver_interface_sptr_->get_status_message());
             break_loops_->store(true);
         }
     }
@@ -207,10 +194,10 @@ namespace sas
      * @brief RobotDriverFranka::get_joint_forces
      * @return
      */
-    VectorXd RobotDriverFranka::get_joint_torques()
+    VectorXd RobotDriverFranka::get_joint_forces()
     {
-        joint_torques_ = robot_driver_interface_sptr_->get_joint_forces();
+        joint_forces_ = robot_driver_interface_sptr_->get_joint_forces();
-        return joint_torques_;
+        return joint_forces_;
     }
 
 }

src/robot_dynamics/qros_robot_dynamic_py.cpp

@@ -32,29 +32,29 @@
 #include <pybind11/stl.h>
 #include <pybind11/eigen.h>
 
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>
 
 namespace py = pybind11;
-using RDC = qros::RobotDynamicsClient;
+using RDI = qros::RobotDynamicInterface;
-using RDS = qros::RobotDynamicsServer;
+using RDP = qros::RobotDynamicProvider;
 
 
-PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m)
+PYBIND11_MODULE(_qros_robot_dynamic, m)
 {
-    py::class_<RDC>(m, "RobotDynamicsClient")
+    py::class_<RDI>(m, "RobotDynamicsInterface")
-        .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())
+        .def(py::init<const std::string&>())
-        .def("get_stiffness_force",&RDC::get_stiffness_force)
+        .def("get_stiffness_force",&RDI::get_stiffness_force)
-        .def("get_stiffness_torque",&RDC::get_stiffness_torque)
+        .def("get_stiffness_torque",&RDI::get_stiffness_torque)
-        .def("get_stiffness_frame_pose",&RDC::get_stiffness_frame_pose)
+        .def("get_stiffness_frame_pose",&RDI::get_stiffness_frame_pose)
-        .def("is_enabled",&RDC::is_enabled,"Returns true if the RobotDynamicInterface is enabled.")
+        .def("is_enabled",&RDI::is_enabled,"Returns true if the RobotDynamicInterface is enabled.")
-        .def("get_topic_prefix",&RDC::get_topic_prefix);
+        .def("get_topic_prefix",&RDI::get_topic_prefix);
 
-    py::class_<RDS>(m, "RobotDynamicsServer")
+    py::class_<RDP>(m, "RobotDynamicsProvider")
-        .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())
+        .def(py::init<const std::string&>())
-        .def("publish_stiffness",&RDS::publish_stiffness)
+        .def("publish_stiffness",&RDP::publish_stiffness)
-        .def("set_world_to_base_tf", &RDS::set_world_to_base_tf)
+        .def("set_world_to_base_tf", &RDP::set_world_to_base_tf)
-        .def("is_enabled",&RDS::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
+        .def("is_enabled",&RDP::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
-        .def("get_topic_prefix",&RDS::get_topic_prefix);
+        .def("get_topic_prefix",&RDP::get_topic_prefix);
 
 }

src/robot_dynamics/qros_robot_dynamics_client.cpp

@@ -1,102 +0,0 @@
-/*
-# Copyright (c) 2024 Quenitin Lin
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Quenitin Lin
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include <memory>
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp>
-
-using namespace qros;
-
-
-RobotDynamicsClient::RobotDynamicsClient(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
-    node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
-    child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
-    last_stiffness_force_(Vector3d::Zero()),
-    last_stiffness_torque_(Vector3d::Zero()),
-    last_stiffness_frame_pose_(0)
-{
-    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
-    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-
-    // Strip potential leading slash
-    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
-    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
-    RCLCPP_INFO_STREAM(node_->get_logger(),
-        "["+ std::string(node_->get_name()) + "]::Initializing RobotDynamicsClient with prefix " + topic_prefix);
-    subscriber_cartesian_stiffness_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(topic_prefix + "/get/cartesian_stiffness", 1,
-        std::bind(&RobotDynamicsClient::_callback_cartesian_stiffness, this, std::placeholders::_1)
-        );
-
-}
-
-void RobotDynamicsClient::_callback_cartesian_stiffness(const geometry_msgs::msg::WrenchStamped &msg)
-{
-    last_stiffness_force_(0) = msg.wrench.force.x;
-    last_stiffness_force_(1) = msg.wrench.force.y;
-    last_stiffness_force_(2) = msg.wrench.force.z;
-
-    last_stiffness_torque_(0) = msg.wrench.torque.x;
-    last_stiffness_torque_(1) = msg.wrench.torque.y;
-    last_stiffness_torque_(2) = msg.wrench.torque.z;
-
-    try {
-        const auto transform_stamped = tf_buffer_->lookupTransform( parent_frame_id_, child_frame_id_, tf2::TimePointZero);
-        last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
-    } catch (tf2::TransformException &ex) {
-        RCLCPP_WARN_STREAM(node_->get_logger(), "["+ std::string(node_->get_name()) + "]::" + ex.what());
-    }
-}
-
-DQ RobotDynamicsClient::_geometry_msgs_transform_to_dq(const geometry_msgs::msg::Transform& transform)
-{
-    const auto t = DQ(0,transform.translation.x,transform.translation.y,transform.translation.z);
-    const auto r = DQ(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
-    return r + 0.5 * E_ * t * r;
-}
-
-VectorXd RobotDynamicsClient::get_stiffness_force()
-{
-    if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_force on uninitialized topic");
-    return last_stiffness_force_;
-}
-VectorXd RobotDynamicsClient::get_stiffness_torque()
-{
-    if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_torque on uninitialized topic");
-    return last_stiffness_torque_;
-}
-DQ RobotDynamicsClient::get_stiffness_frame_pose()
-{
-    if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_frame_pose on uninitialized Interface");
-    return last_stiffness_frame_pose_;
-}
-
-bool RobotDynamicsClient::is_enabled() const
-{
-    if(last_stiffness_frame_pose_==0) return false;
-    return true;
-}

src/robot_dynamics/qros_robot_dynamics_interface.cpp

@@ -0,0 +1,103 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>
+
+using namespace qros;
+
+RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &nodehandle, const std::string &node_prefix):
+    RobotDynamicInterface(nodehandle, nodehandle, node_prefix)
+{
+    //Delegated
+}
+
+RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
+    node_prefix_(node_prefix),
+    child_frame_id_(node_prefix + "_stiffness_frame"),
+    parent_frame_id_(node_prefix + "_base"),
+    tf_buffer_(ros::Duration(BUFFER_DURATION_DEFAULT_S)),
+    tf_listener_(tf_buffer_, subscriber_nodehandle),
+    last_stiffness_force_(Vector3d::Zero()),
+    last_stiffness_torque_(Vector3d::Zero()),
+    last_stiffness_frame_pose_(0)
+{
+    // Strip potential leading slash
+    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
+    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
+    ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicInterface with prefix " + node_prefix);
+    subscriber_cartesian_stiffness_ = subscriber_nodehandle.subscribe(node_prefix_ + "/get/cartesian_stiffness", 1, &RobotDynamicInterface::_callback_cartesian_stiffness, this);
+
+}
+
+void RobotDynamicInterface::_callback_cartesian_stiffness(const geometry_msgs::WrenchStampedConstPtr &msg)
+{
+    last_stiffness_force_(0) = msg->wrench.force.x;
+    last_stiffness_force_(1) = msg->wrench.force.y;
+    last_stiffness_force_(2) = msg->wrench.force.z;
+
+    last_stiffness_torque_(0) = msg->wrench.torque.x;
+    last_stiffness_torque_(1) = msg->wrench.torque.y;
+    last_stiffness_torque_(2) = msg->wrench.torque.z;
+
+    try {
+        const auto transform_stamped = tf_buffer_.lookupTransform( parent_frame_id_, child_frame_id_, ros::Time(0));
+        last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
+    } catch (tf2::TransformException &ex) {
+        ROS_WARN_STREAM(ros::this_node::getName() + "::" + ex.what());
+    }
+}
+
+DQ RobotDynamicInterface::_geometry_msgs_transform_to_dq(const geometry_msgs::Transform& transform)
+{
+    const auto t = DQ(0,transform.translation.x,transform.translation.y,transform.translation.z);
+    const auto r = DQ(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
+    return r + 0.5 * E_ * t * r;
+}
+
+VectorXd RobotDynamicInterface::get_stiffness_force()
+{
+    if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_force on uninitialized topic");
+    return last_stiffness_force_;
+}
+VectorXd RobotDynamicInterface::get_stiffness_torque()
+{
+    if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_torque on uninitialized topic");
+    return last_stiffness_torque_;
+}
+DQ RobotDynamicInterface::get_stiffness_frame_pose()
+{
+    if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_frame_pose on uninitialized Interface");
+    return last_stiffness_frame_pose_;
+}
+
+bool RobotDynamicInterface::is_enabled() const
+{
+    if(last_stiffness_frame_pose_==0) return false;
+    return true;
+}

src/robot_dynamics/qros_robot_dynamics_provider.cpp

@@ -0,0 +1,126 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+
+using namespace qros;
+
+RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const std::string &node_prefix):
+    RobotDynamicProvider(nodehandle, nodehandle, node_prefix)
+{
+    //Delegated
+}
+
+RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
+    node_prefix_(node_prefix),
+    child_frame_id_(node_prefix + "_stiffness_frame"),
+    parent_frame_id_(node_prefix + "_base"),
+    world_to_base_tf_(0)
+{
+    // Strip potential leading slash
+    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
+    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
+
+    ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix);
+    publisher_cartesian_stiffness_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_stiffness", 1);
+
+}
+
+
+geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(const DQ& pose)
+{
+    geometry_msgs::Transform tf_msg;
+    const auto t = translation(pose);
+    const auto r = rotation(pose);
+    tf_msg.translation.x = t.q(1);
+    tf_msg.translation.y = t.q(2);
+    tf_msg.translation.z = t.q(3);
+    tf_msg.rotation.w = r.q(0);
+    tf_msg.rotation.x = r.q(1);
+    tf_msg.rotation.y = r.q(2);
+    tf_msg.rotation.z = r.q(3);
+    return tf_msg;
+}
+
+void RobotDynamicProvider::set_world_to_base_tf(const DQ& world_to_base_tf)
+{
+    if(world_to_base_tf_==0)
+    {
+        world_to_base_tf_ = world_to_base_tf;
+        _publish_base_static_tf();
+    }else
+    {
+        throw std::runtime_error("The world to base transform has already been set");
+    }
+}
+
+void RobotDynamicProvider::_publish_base_static_tf()
+{
+    geometry_msgs::TransformStamped base_tf;
+    base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
+    base_tf.header.stamp = ros::Time::now();
+    base_tf.header.frame_id = WORLD_FRAME_ID;
+    base_tf.child_frame_id = parent_frame_id_;
+    static_base_tf_broadcaster_.sendTransform(base_tf);
+
+}
+
+
+void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
+{
+    std_msgs::Header header;
+    header.stamp = ros::Time::now();
+    header.seq = seq_++;
+    geometry_msgs::WrenchStamped msg;
+    msg.header = header;
+    msg.header.frame_id = child_frame_id_;
+    msg.wrench.force.x = force(0);
+    msg.wrench.force.y = force(1);
+    msg.wrench.force.z = force(2);
+    msg.wrench.torque.x = torque(0);
+    msg.wrench.torque.y = torque(1);
+    msg.wrench.torque.z = torque(2);
+    publisher_cartesian_stiffness_.publish(msg);
+    if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
+    {
+        geometry_msgs::TransformStamped tf_msg;
+        tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
+        tf_msg.header = header;
+        tf_msg.header.frame_id = parent_frame_id_;
+        tf_msg.child_frame_id = child_frame_id_;
+        tf_broadcaster_.sendTransform(tf_msg);
+    }
+}
+
+
+bool RobotDynamicProvider::is_enabled() const
+{
+    return true; //Always enabled
+}
+

src/robot_dynamics/qros_robot_dynamics_server.cpp

@@ -1,120 +0,0 @@
-/*
-# Copyright (c) 2024 Quenitin Lin
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Quenitin Lin
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
-
-using namespace qros;
-
-RobotDynamicsServer::RobotDynamicsServer(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
-    node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
-    child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
-    tf_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
-    static_base_tf_broadcaster_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_)),
-    world_to_base_tf_(0)
-{
-    // Strip potential leading slash
-    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
-    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
-    RCLCPP_INFO_STREAM(node_->get_logger(),
-        "["+ std::string(node_->get_name()) + "]::Initializing RobotDynamicsServer with prefix " + topic_prefix);
-    publisher_cartesian_stiffness_ = node_->create_publisher<geometry_msgs::msg::WrenchStamped>(topic_prefix + "/get/cartesian_stiffness", 1);
-}
-
-geometry_msgs::msg::Transform RobotDynamicsServer::_dq_to_geometry_msgs_transform(const DQ& pose)
-{
-    geometry_msgs::msg::Transform tf_msg;
-    const auto t = translation(pose);
-    const auto r = rotation(pose);
-    tf_msg.translation.x = t.q(1);
-    tf_msg.translation.y = t.q(2);
-    tf_msg.translation.z = t.q(3);
-    tf_msg.rotation.w = r.q(0);
-    tf_msg.rotation.x = r.q(1);
-    tf_msg.rotation.y = r.q(2);
-    tf_msg.rotation.z = r.q(3);
-    return tf_msg;
-}
-
-void RobotDynamicsServer::set_world_to_base_tf(const DQ& world_to_base_tf)
-{
-    if(world_to_base_tf_==0)
-    {
-        world_to_base_tf_ = world_to_base_tf;
-        _publish_base_static_tf();
-    }else
-    {
-        throw std::runtime_error("["+ std::string(node_->get_name()) + "]::[RobotDynamicsServer]::The world to base transform has already been set");
-    }
-}
-
-void RobotDynamicsServer::_publish_base_static_tf()
-{
-    geometry_msgs::msg::TransformStamped base_tf;
-    base_tf.set__transform(_dq_to_geometry_msgs_transform(world_to_base_tf_));
-    std_msgs::msg::Header header;
-    header.set__stamp(node_->now());
-    header.set__frame_id(WORLD_FRAME_ID);
-    base_tf.set__header(header);
-    base_tf.set__child_frame_id(parent_frame_id_);
-    static_base_tf_broadcaster_->sendTransform(base_tf);
-
-}
-
-
-void RobotDynamicsServer::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
-{
-    std_msgs::msg::Header header;
-    header.set__stamp(node_->now());
-    geometry_msgs::msg::WrenchStamped msg;
-    msg.set__header(header);
-    msg.header.set__frame_id(child_frame_id_);
-    msg.wrench.force.set__x(force(0));
-    msg.wrench.force.set__y(force(1));
-    msg.wrench.force.set__z(force(2));
-    msg.wrench.torque.set__x(torque(0));
-    msg.wrench.torque.set__y(torque(1));
-    msg.wrench.torque.set__z(torque(2));
-    publisher_cartesian_stiffness_->publish(msg);
-    if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
-    {
-        header.set__frame_id(parent_frame_id_);
-        geometry_msgs::msg::TransformStamped tf_msg;
-        tf_msg.set__transform(_dq_to_geometry_msgs_transform(base_to_stiffness));
-        tf_msg.set__header(header);
-        tf_msg.set__child_frame_id(child_frame_id_);
-        tf_broadcaster_->sendTransform(tf_msg);
-    }
-}
-
-
-bool RobotDynamicsServer::is_enabled() const
-{
-    return true; //Always enabled
-}
-

src/sas_robot_driver_coppelia_node.cpp

@@ -27,8 +27,7 @@
 #         - Adapted from sas_robot_driver_denso_node.cpp
 #           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
 #      2. Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp)
-#         - Adapted for simplify operation
+          -Adapted for simplify operation
-#         - porting to ROS2
 #
 # ################################################################
 */
@@ -39,9 +38,10 @@
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
 //#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
-#include <sas_common/sas_common.hpp>
+#include <sas_common/sas_common.h>
-#include <sas_conversions/eigen3_std_conversions.hpp>
+#include <sas_conversions/eigen3_std_conversions.h>
-#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include "../../src/coppelia/sas_robot_driver_coppelia.h"
 
 /*********************************************
  * SIGNAL HANDLER
@@ -59,46 +59,46 @@ int main(int argc, char **argv)
 {
     if(signal(SIGINT, sig_int_handler) == SIG_ERR)
     {
-        throw std::runtime_error("Error setting the signal int handler.");
+        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
     }
 
-    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
+    ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler);
-    auto context = rclcpp::contexts::get_global_default_context();
+    ROS_WARN("=====================================================================");
-
+    ROS_WARN("--------------------- Quentin Lin -----------------------------------");
-    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_coppelia_node");
+    ROS_WARN("=====================================================================");
-
+    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
-    const auto node_name = std::string(node->get_name());
-    RCLCPP_WARN(node->get_logger(),"=====================================================================");
-    RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
-    RCLCPP_WARN(node->get_logger(),"=====================================================================");
-    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),":Loading parameters from parameter server.");
 
 
-    qros::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
+    ros::NodeHandle nh;
-    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_coppelia_configuration.thread_sampling_time_sec);
+    sas::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
-    sas::get_ros_parameter(node,"vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
+    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_coppelia_configuration.thread_sampling_time_nsec);
-    sas::get_ros_parameter(node,"vrep_port",robot_driver_coppelia_configuration.vrep_port);
+    sas::get_ros_param(nh,"/vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
-    sas::get_ros_parameter(node,"vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
+    sas::get_ros_param(nh,"/vrep_port",robot_driver_coppelia_configuration.vrep_port);
-    sas::get_ros_parameter(node,"vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
+    sas::get_ros_param(nh,"/vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
-    sas::get_ros_parameter(node,"robot_mode", robot_driver_coppelia_configuration.robot_mode);
+    sas::get_ros_param(nh,"/vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
-    sas::get_ros_parameter(node,"using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
+    sas::get_ros_param(nh,"/robot_mode", robot_driver_coppelia_configuration.robot_mode);
-    sas::get_ros_parameter(node,"robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
+    sas::get_ros_param(nh,"/using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
-    sas::get_ros_parameter(node,"robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
+    sas::get_ros_param(nh,"/robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
+    sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
+
+    // std::vector<double> q_min_vec, q_max_vec;
+    // sas::get_ros_param(nh,"/q_min", q_min_vec);
+    // robot_driver_coppelia_configuration.q_min = sas::std_vector_double_to_vectorxd(q_min_vec);
+    // sas::get_ros_param(nh,"/q_max", q_max_vec);
+    // robot_driver_coppelia_configuration.q_max = sas::std_vector_double_to_vectorxd(q_max_vec);
+
 
     try
     {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"::Instantiating Coppelia robot mirror.");
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
-        qros::RobotDriverCoppelia robot_driver_coppelia(node, robot_driver_coppelia_configuration,
+        sas::RobotDriverCoppelia robot_driver_coppelia(nh, robot_driver_coppelia_configuration,
                                                        &kill_this_process);
 
         return robot_driver_coppelia.start_control_loop();
     }
     catch (const std::exception& e)
     {
-        kill_this_process = true;
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
-        RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
-        shutdown(context, "Exception in main function: " + std::string(e.what()));
-        return -1;
     }
 
 

src/sas_robot_driver_franka/__init__.py

@@ -0,0 +1,4 @@
+"""
+"""
+from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider
+from .franka_gripper_interface import FrankaGripperInterface

src/sas_robot_driver_franka/franka_gripper_interface.py

@@ -0,0 +1,182 @@
+import rospy
+from sas_robot_driver_franka.srv import Move, MoveRequest, MoveResponse, Grasp, GraspRequest, GraspResponse
+from sas_robot_driver_franka.msg import GripperState
+import os
+import threading
+from queue import Queue
+import struct
+
+MOVE_TOPIC_SUFFIX = "move"
+GRASP_TOPIC_SUFFIX = "grasp"
+STATUS_TOPIC_SUFFIX = "gripper_status"
+
+
+class FrankaGripperInterface:
+    """
+    Non blocking interface to control the Franka gripper
+    """
+
+    def __init__(self, robot_prefix):
+        self.robot_prefix = robot_prefix
+        self.move_service = rospy.ServiceProxy(os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX), Move)
+        self._moving = False
+        self.grasp_service = rospy.ServiceProxy(os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX), Grasp)
+        self._grasping = False
+        self.status_subscriber = rospy.Subscriber(os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), GripperState,
+                                                  self._status_callback)
+
+        self.result_queue = Queue()
+
+        # gripper state
+        self.state_width = None
+        self.state_max_width = None
+        self.state_temperature = None
+        self.state_is_grasped = None
+
+    def _is_service_ready(self, service):
+        try:
+            rospy.wait_for_service(service, timeout=0.1)
+            return True
+        except rospy.ROSException:
+            return False
+
+    def is_enabled(self):
+        if self.state_width is None:
+            return False
+        if not self._is_service_ready(self.move_service.resolved_name):
+            return False
+        if not self._is_service_ready(self.grasp_service.resolved_name):
+            return False
+        return True
+
+    def _status_callback(self, msg: GripperState):
+        self.state_width = msg.width
+        self.state_max_width = msg.max_width
+        self.state_temperature = msg.temperature
+        self.state_is_grasped = msg.is_grasped
+
+    def move(self, width, speed=0):
+        """
+        Move the gripper to a specific width
+        :param width: width in meters
+        :param speed: speed in meters per second
+        :return: None
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        threading.Thread(target=self._move, args=(width, speed)).start()
+
+    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
+        """
+        Grasp an object with the gripper
+        :param width:
+        :param force:
+        :param speed:
+        :param epsilon_inner:
+        :param epsilon_outer:
+        :return:
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
+
+    def get_max_width(self):
+        """ Get the maximum width of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_max_width
+
+    def get_width(self):
+        """ Get the current width of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_width
+
+    def get_temperature(self):
+        """ Get the temperature of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_temperature
+
+    def is_grasped(self):
+        """ Check if an object is grasped """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_is_grasped
+
+    def is_moving(self):
+        """ Check if the gripper is currently moving """
+        return self._moving
+
+    def is_grasping(self):
+        """ Check if the gripper is currently grasping """
+        return self._grasping
+
+    def is_busy(self):
+        """ Check if the gripper is currently moving or grasping """
+        return self._moving or self._grasping
+
+    def is_done(self):
+        """ Check if the gripper is done moving or grasping """
+        if not self.is_busy():
+            rospy.logwarn("Gripper is not moving or grasping")
+            return False
+        else:
+            if self.result_queue.empty():
+                return False
+            else:
+                return True
+
+    def get_result(self):
+        """
+        Get the result of the last action
+        :return:
+        """
+        if not self.result_queue.empty():
+            response = self.result_queue.get()
+            self._moving = False
+            self._grasping = False
+            return response.success
+        else:
+            raise Exception("No result available")
+
+    def wait_until_done(self):
+        """
+        Wait until the gripper is done moving or grasping
+        :return: success
+        """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        if not self._moving and not self._grasping:
+            raise Exception("Gripper is not moving or grasping")
+        while self._moving or self._grasping:
+            rospy.sleep(0.1)
+            if not self.result_queue.empty():
+                response = self.result_queue.get()
+                if isinstance(response, MoveResponse):
+                    self._moving = False
+                elif isinstance(response, GraspResponse):
+                    self._grasping = False
+                else:
+                    raise Exception("Invalid response type")
+                break
+        return response.success
+
+    def _move(self, width, speed):
+        self._moving = True
+        request = MoveRequest()
+        request.width = width
+        request.speed = speed
+        response = self.move_service(request)
+        self.result_queue.put(response)
+
+    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
+        self._grasping = True
+        request = GraspRequest()
+        request.width = width
+        request.force = force
+        request.speed = speed
+        request.epsilon_inner = epsilon_inner
+        request.epsilon_outer = epsilon_outer
+        response = self.grasp_service(request)
+        self.result_queue.put(response)

src/sas_robot_driver_franka_hand_node.cpp

@@ -33,9 +33,10 @@
 
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
-#include <sas_common/sas_common.hpp>
+#include <sas_common/sas_common.h>
-// #include <sas_conversions/eigen3_std_conversions.hpp>
+#include <sas_conversions/eigen3_std_conversions.h>
-#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include "../../src/hand/qros_effector_driver_franka_hand.h"
 
 
 /*********************************************
@@ -51,14 +52,14 @@ void sig_int_handler(int)
 
 
 template<typename T>
-void get_optional_parameter(std::shared_ptr<Node> node, const std::string &param_name, T &param)
+void get_optional_parameter(const std::string &node_prefix, ros::NodeHandle &nh, const std::string &param_name, T &param)
 {
-    try
+    if(nh.hasParam(node_prefix + param_name))
     {
-        sas::get_ros_parameter(node,param_name,param);
+        sas::get_ros_param(nh,param_name,param);
-    }catch (const std::exception& e)
+    }else
     {
-        RCLCPP_INFO_STREAM(node->get_logger(), "["+std::string(node->get_name())+"]:Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
+        ROS_INFO_STREAM(ros::this_node::getName() + "::Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
     }
 
 }
@@ -68,54 +69,55 @@ int main(int argc, char **argv)
 {
     if(signal(SIGINT, sig_int_handler) == SIG_ERR)
     {
-        throw std::runtime_error("Error setting the signal int handler.");
+        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
     }
+    ros::init(argc, argv, "sas_robot_driver_franka_hand_node", ros::init_options::NoSigintHandler);
+    ROS_WARN("=====================================================================");
+    ROS_WARN("---------------------------Quentin Lin-------------------------------");
+    ROS_WARN("=====================================================================");
+    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
+    const std::string& effector_driver_provider_prefix = ros::this_node::getName();
 
-    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
-    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_hand_node");
-
-    const auto node_name = std::string(node->get_name());
-    RCLCPP_WARN(node->get_logger(),"=====================================================================");
-    RCLCPP_WARN(node->get_logger(),"---------------------------Quentin Lin-------------------------------");
-    RCLCPP_WARN(node->get_logger(),"=====================================================================");
-    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
 
+    ros::NodeHandle nh;
     qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
-    get_optional_parameter(node, "initialize_with_homing", robot_driver_franka_hand_configuration.initialize_with_homing);
+
-    sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
+    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
-    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s);
+
-    sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/thread_sampling_time_nsec",robot_driver_franka_hand_configuration.thread_sampeling_time_ns);
-    sas::get_ros_parameter(node,"default_speed",robot_driver_franka_hand_configuration.default_speed);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_force",robot_driver_franka_hand_configuration.default_force);
-    sas::get_ros_parameter(node,"default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_speed",robot_driver_franka_hand_configuration.default_speed);
-    sas::get_ros_parameter(node,"default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
 
     qros::EffectorDriverFrankaHand franka_hand_driver(
-        node_name,
+        effector_driver_provider_prefix,
         robot_driver_franka_hand_configuration,
-        node,
+        nh,
         &kill_this_process
     );
     try
     {
-        RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Instantiating Franka hand.");
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka hand.");
         franka_hand_driver.connect();
         franka_hand_driver.initialize();
-        RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Starting control loop.");
+
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating control loop.");
         franka_hand_driver.start_control_loop();
 
     }
     catch (const std::exception& e)
     {
-        RCLCPP_ERROR_STREAM(node->get_logger(), "["+node_name+"]::Exception::" + e.what());
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
     }catch (...)
     {
-        RCLCPP_ERROR_STREAM(node->get_logger(), "["+node_name+"]::Exception::Unknown exception.");
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::Unknown exception.");
     }
-    RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Exiting...");
+    ROS_INFO_STREAM(ros::this_node::getName()+"::Exiting...");
     franka_hand_driver.deinitialize();
-    RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Deinitialized.");
+    ROS_INFO_STREAM(ros::this_node::getName()+"::deinitialized.");
     franka_hand_driver.disconnect();
-    RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Disconnected.");
+    ROS_INFO_STREAM(ros::this_node::getName()+"::disconnected.");
 
 
   return 0;

src/sas_robot_driver_franka_node.cpp

@@ -26,8 +26,6 @@
 #      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
 #         - Adapted from sas_robot_driver_denso_node.cpp
 #           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
-#      2. Quentin Lin
-#         - Adaption to ROS2
 #
 # ################################################################
 */
@@ -38,11 +36,11 @@
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
 #include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
-#include <sas_common/sas_common.hpp>
+#include <sas_common/sas_common.h>
-#include <sas_conversions/eigen3_std_conversions.hpp>
+#include <sas_conversions/eigen3_std_conversions.h>
-#include <sas_robot_driver/sas_robot_driver_ros.hpp>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
-#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
+#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
 
 
 /*********************************************
@@ -76,106 +74,100 @@ std::array<T, N> std_vec_to_std_array(const std::vector<T>& vector)
     }
     return array;
 }
-VectorXd std_vec_to_eigen_vector(const std::vector<double>& vector)
-{
-    VectorXd eigen_vector(vector.size());
-    for(std::size_t i = 0; i < vector.size(); i++)
-    {
-        eigen_vector(i) = vector[i];
-    }
-    return eigen_vector;
-}
 
 
 int main(int argc, char **argv)
 {
     if(signal(SIGINT, sig_int_handler) == SIG_ERR)
     {
-        throw std::runtime_error("Error setting the signal int handler.");
+        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
     }
 
-    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
+    ros::init(argc, argv, "sas_robot_driver_franka_node", ros::init_options::NoSigintHandler);
-    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_node");
+    ROS_WARN("=====================================================================");
+    ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
+    ROS_WARN("=====================================================================");
+    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
 
-    const auto node_name = std::string(node->get_name());
+
-    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    ros::NodeHandle nh;
-    RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
-    RCLCPP_WARN(node->get_logger(),"=====================================================================");
-    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
-    
     sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
     RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration;
 
-    sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_configuration.ip_address);
+    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
-    sas::get_ros_parameter(node,"robot_mode",      robot_driver_franka_configuration.mode);
+    sas::get_ros_param(nh,"/robot_mode",      robot_driver_franka_configuration.mode);
     double upper_scale_factor = 1.0;
     std::vector<std::string> all_params;
-
+    if(nh.hasParam(ros::this_node::getName()+"/force_upper_limits_scaling_factor"))
-    try {
+    {
-        sas::get_ros_parameter(node,"force_upper_limits_scaling_factor",upper_scale_factor);
+        sas::get_ros_param(nh,"/force_upper_limits_scaling_factor",upper_scale_factor);
-        RCLCPP_WARN_STREAM_ONCE(node->get_logger(),"Set force upper limits scaling factor: " << upper_scale_factor);
+        ROS_WARN_STREAM(ros::this_node::getName()+"::Set force upper limits scaling factor: " << upper_scale_factor);
-    }catch(...) {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
     }
-
+    if(nh.hasParam(ros::this_node::getName()+"/upper_torque_threshold")) {
-    try {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold override and set.");
         std::vector<double> upper_torque_threshold_std_vec;
-        sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
+        sas::get_ros_param(nh,"/upper_torque_threshold",upper_torque_threshold_std_vec);
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
         franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
-    }catch(...) {
+    }else {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
     }
-    try {
+    if(nh.hasParam(ros::this_node::getName()+"/upper_force_threshold")) {
-        std::vector<double> upper_force_threshold_std_vec;
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold override and set.");
-        sas::get_ros_parameter(node,"upper_force_threshold",upper_force_threshold_std_vec);
+        std::vector<double> upper_torque_threshold_std_vec;
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
+        sas::get_ros_param(nh,"/upper_force_threshold",upper_torque_threshold_std_vec);
-        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_force_threshold_std_vec);
+        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
-    }catch(...) {
+    }else {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
     }
-    try {
+    if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path"))
+    {
         std::string robot_parameter_file_path;
-        sas::get_ros_parameter(node,"robot_parameter_file_path",robot_parameter_file_path);
+        sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path);
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading robot parameters from file: " + robot_parameter_file_path);
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Loading robot parameters from file: " + robot_parameter_file_path);
         const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
         robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
-    }catch(...) {
+    }else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");
+
+    if(nh.hasParam(ros::this_node::getName()+"/automatic_error_recovery")) {
+        sas::get_ros_param(nh,"/automatic_error_recovery",robot_driver_franka_configuration.automatic_error_recovery);
+        if(robot_driver_franka_configuration.automatic_error_recovery)
+        {
+            ROS_WARN_STREAM(ros::this_node::getName()+"::Automatic error recovery enabled. STATUS EXPERIMENTAL");
+        }
     }
 
+
+
     robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
 
     sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
 
-    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_ros_configuration.thread_sampling_time_sec);
+    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    sas::get_ros_parameter(node,"q_min",robot_driver_ros_configuration.q_min);
+    sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-    sas::get_ros_parameter(node,"q_max",robot_driver_ros_configuration.q_max);
+    sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
 
     // initialize the robot dynamic provider
-    robot_driver_ros_configuration.robot_driver_provider_prefix = node_name;
+    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-    std::shared_ptr<qros::RobotDynamicsServer> robot_dynamic_provider_ptr = std::make_shared<qros::RobotDynamicsServer>(node, robot_driver_ros_configuration.robot_driver_provider_prefix);
+    qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
     if(robot_driver_franka_configuration.robot_reference_frame!=0)
     {
-        robot_dynamic_provider_ptr->set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
+        robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
     }
 
     try
         {
-            RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating Franka robot.");
+            ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
-            std::shared_ptr<sas::RobotDriverFranka> robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
+            sas::RobotDriverFranka robot_driver_franka(
-                node,
+                &robot_dynamic_provider,
-                robot_dynamic_provider_ptr,
                 robot_driver_franka_configuration,
                 &kill_this_process
             );
-            std::tuple<VectorXd, VectorXd> joint_limits = {std_vec_to_eigen_vector(robot_driver_ros_configuration.q_min), std_vec_to_eigen_vector(robot_driver_ros_configuration.q_max)};
+            //robot_driver_franka.set_joint_limits({qmin, qmax});
-            robot_driver_franka -> set_joint_limits(joint_limits);
+            ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
-            RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
+            sas::RobotDriverROS robot_driver_ros(nh,
-            sas::RobotDriverROS robot_driver_ros(node,
+                                                &robot_driver_franka, 
-                                                robot_driver_franka,
                                                 robot_driver_ros_configuration,
                                                 &kill_this_process);
             robot_driver_ros.control_loop();
@@ -183,7 +175,7 @@ int main(int argc, char **argv)
     catch (const std::exception& e)
     {
         kill_this_process = true;
-        RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
     }     
  
 

srv/Grasp.srv

@@ -0,0 +1,7 @@
+float32 width
+float32 speed
+float32 force
+float32 epsilon_inner
+float32 epsilon_outer
+---
+bool success

srv/Move.srv

@@ -0,0 +1,4 @@
+float32 width
+float32 speed
+---
+bool success