minor general bug fix

CMakeLists.txt

@@ -244,22 +244,22 @@ install(TARGETS
 ##### PYBIND11 LIBRARY ROBOT_DYNAMICS #####
 ament_python_install_package(${PROJECT_NAME})
 
-pybind11_add_module(_robot_dynamics SHARED
+pybind11_add_module(_qros_franka_robot_dynamic 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(_robot_dynamics
+target_include_directories(_qros_franka_robot_dynamic
         PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot_dynamics>
         $<INSTALL_INTERFACE:include/robot_dynamics>)
 
-target_compile_definitions(_robot_dynamics PRIVATE IS_SAS_PYTHON_BUILD)
+target_compile_definitions(_qros_franka_robot_dynamic PRIVATE IS_SAS_PYTHON_BUILD)
 # https://github.com/pybind/pybind11/issues/387
-target_link_libraries(_robot_dynamics PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
+target_link_libraries(_qros_franka_robot_dynamic PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
 
-install(TARGETS _robot_dynamics
+install(TARGETS _qros_franka_robot_dynamic
         DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
 )
 ##END## PYBIND11 LIBRARY ROBOT_DYNAMICS #####

CMakeLists_catkin.txt

@@ -1,239 +0,0 @@
-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)
-add_compile_options(-Werror=return-type)
-
-
-#Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view.
-file(GLOB_RECURSE EXTRA_FILES */*)
-#add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
-add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-        roscpp
-        rospy
-        std_msgs
-        tf2_ros
-        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(Eigen3 REQUIRED)
-find_package(yaml-cpp REQUIRED)
-include_directories(${EIGEN3_INCLUDE_DIR})
-
-# To correctly find and link with QT
-set(CMAKE_PREFIX_PATH $ENV{QT_PATH})
-set(CMAKE_AUTOMOC ON)
-set(CMAKE_AUTORCC ON)
-set(CMAKE_AUTOUIC ON)
-if (CMAKE_VERSION VERSION_LESS "3.7.0")
-    set(CMAKE_INCLUDE_CURRENT_DIR ON)
-endif ()
-find_package(Qt5 COMPONENTS Widgets REQUIRED)
-
-find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets)
-find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets)
-
-
-add_library(MotionGenerator src/generator/motion_generator.cpp)
-target_link_libraries(MotionGenerator Franka::Franka)
-
-
-add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp)
-
-add_library(QuadraticProgramMotionGenerator src/generator/quadratic_program_motion_generator.cpp)
-target_link_libraries(QuadraticProgramMotionGenerator
-        qpOASES
-        dqrobotics
-        ConstraintsManager)
-
-add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp)
-target_link_libraries(CustomMotionGeneration
-        qpOASES
-        dqrobotics
-        ConstraintsManager)
-
-add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
-target_link_libraries(robot_interface_franka Franka::Franka
-        dqrobotics
-        MotionGenerator
-        ConstraintsManager
-        QuadraticProgramMotionGenerator
-        CustomMotionGeneration)
-
-add_library(robot_interface_hand src/hand/robot_interface_hand.cpp)
-target_link_libraries(robot_interface_hand Franka::Franka
-        dqrobotics)
-
-
-############
-## Build ###
-############
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-
-
-include_directories(
-        include
-        include/generator
-        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_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
-target_link_libraries(sas_robot_driver_franka
-        qros_robot_dynamics_provider
-        dqrobotics
-        dqrobotics-interface-json11
-        robot_interface_franka)
-
-add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
-target_link_libraries(qros_effector_driver_franka_hand
-        dqrobotics
-#        robot_interface_hand
-        Franka::Franka)
-
-
-add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
-target_link_libraries(sas_robot_driver_coppelia
-        dqrobotics
-        dqrobotics-interface-vrep)
-
-add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-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})
-
-
-add_executable(JuankaEmika
-        qt/configuration_window/main.cpp
-        qt/configuration_window/mainwindow.cpp
-        qt/configuration_window/mainwindow.ui
-)
-
-target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
-        dqrobotics
-        ${catkin_LIBRARIES}
-        robot_interface_franka
-)
-
-
-#####################################################################################
-# python binding
-include_directories(
-        include/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
-)
-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 ${PROJECT_NAME}
-        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
-
-install(TARGETS sas_robot_driver_franka_node
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(TARGETS sas_robot_driver_coppelia_node
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(TARGETS sas_robot_driver_franka_hand_node
-        DESTINATION ${CATKIN_PACKAGE_BIN_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}
-)
-

include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp

@@ -62,7 +62,7 @@ private:
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_base_tf_broadcaster_;
 
-    DQ world_to_base_tf_;
+    DQ world_to_base_tf_ = DQ(0);
 
     static geometry_msgs::msg::Transform _dq_to_geometry_msgs_transform(const DQ& pose);
 

sas_robot_driver_franka/__init__.py

@@ -1,6 +1,6 @@
 """
 """
-from sas_robot_driver_franka._robot_dynamics import RobotDynamicsInterface, RobotDynamicsProvider
+from sas_robot_driver_franka._qros_franka_robot_dynamic import RobotDynamicsClient, RobotDynamicsServer
 
 import rclpy
 from rclpy.node import Node

src/robot_dynamics/qros_robot_dynamics_py.cpp

@@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient;
 using RDS = qros::RobotDynamicsServer;
 
 
-PYBIND11_MODULE(_qros_robot_dynamic, m)
+PYBIND11_MODULE(_qros_franka_robot_dynamic, m)
 {
     py::class_<RDC>(m, "RobotDynamicsClient")
         .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())

src/robot_dynamics/qros_robot_dynamics_server.cpp

@@ -34,9 +34,9 @@ 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"),
-    world_to_base_tf_(0),
     tf_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
-    static_base_tf_broadcaster_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(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);}