two bufix: mutexes, and strictness management

.github/workflows/industrial_ci_action.yml

@@ -9,12 +9,12 @@ name: 'ROS INDUSTRIAL CI'
 # or more fine-grained
 on:
   push:
-    branches: [ master ]
+    branches: [ marco-devel ]
     paths-ignore:
     - 'docs/**'  # When there is a pull request against master
     - 'README.md'
   pull_request:
-    branches: [ master ]
+    branches: [ marco-devel ]
 
 jobs:
   industrial_ci:
@@ -24,11 +24,11 @@ jobs:
         ROS_REPO: [main]
     env:
       CCACHE_DIR: "/home/runner/target_ws/.ccache"  #/github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
-      UPSTREAM_WORKSPACE: dependencies.rosinstall
+      UPSTREAM_WORKSPACE: ci.rosinstall
       ISOLATION: "shell"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     steps:
-      - uses: actions/checkout@v2   # clone target repository
+      - uses: actions/checkout@v3   # clone target repository
       - uses: actions/cache@v2      # fetch/store the directory used by ccache before/after the ci run
         with:
           path: ${{ env.CCACHE_DIR }}

ci.rosinstall

@@ -0,0 +1,48 @@
+- git:
+    local-name: cnr_logger
+    uri: https://github.com/CNR-STIIMA-IRAS/cnr_logger
+- git:
+    local-name: realtime_utilities
+    uri:  https://github.com/CNR-STIIMA-IRAS/realtime_utilities
+- git:
+    local-name: name_sorting
+    uri:  https://github.com/CNR-STIIMA-IRAS/name_sorting
+- git:
+    local-name: subscription_notifier
+    uri:  https://github.com/CNR-STIIMA-IRAS/subscription_notifier
+- git:
+    local-name: configuration_msgs
+    uri: https://github.com/CNR-STIIMA-IRAS/configuration_msgs
+- git:
+    local-name: cnr_hardware_interface
+    uri:  https://github.com/CNR-STIIMA-IRAS/cnr_hardware_interface.git
+- git:
+    local-name: rosdyn
+    uri:  https://github.com/CNR-STIIMA-IRAS/rosdyn.git
+- git:
+    local-name: rosdyn_utilities
+    uri:  https://github.com/CNR-STIIMA-IRAS/rosdyn_utilities.git
+- git:
+    local-name: binary_logger
+    uri: https://github.com/CNR-STIIMA-IRAS/binary_logger.git
+- git:
+    local-name: cnr_control_toolbox
+    uri: https://github.com/CNR-STIIMA-IRAS/cnr_control_toolbox
+- git:
+    local-name: eigen_utils
+    uri: https://github.com/CNR-STIIMA-IRAS/eigen_utils
+- git:
+    local-name: rosparam_utilities
+    uri: https://github.com/CNR-STIIMA-IRAS/rosparam_utilities.git
+- git:
+    local-name: motion/moveit/moveit_planning_helper
+    uri: https://github.com/CNR-STIIMA-IRAS/moveit_planning_helper
+- git:
+    local-name: test_depend/code_coverage
+    uri: https://github.com/mikeferguson/code_coverage.git
+    version: master
+- git:
+    local-name: test_dependencies/universal_robot
+    uri: https://github.com/fmauch/universal_robot.git
+    version: calibration_devel    
+

cnr_configuration_manager/CMakeLists.txt

@@ -2,13 +2,9 @@ cmake_minimum_required(VERSION 3.1)
 
 project(cnr_configuration_manager)
 
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE Release)
-endif()
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
+include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/cnrConfigMacros.cmake)
 
+cnr_set_flags()
 # Debug only, collects stats on how callbacks are doled out to worker threads
 #add_definitions(-DNODELET_QUEUE_DEBUG)
 
@@ -64,14 +60,13 @@ include_directories(
 add_library(${PROJECT_NAME} src/${PROJECT_NAME}/cnr_configuration_loader.cpp
                             src/${PROJECT_NAME}/cnr_configuration_manager.cpp
                             src/${PROJECT_NAME}/signal_handler.cpp )
-target_compile_options(${PROJECT_NAME} PUBLIC -Wall -faligned-new
-        $<$<CONFIG:Release>:-Ofast -funroll-loops -ffast-math >)
+target_link_libraries (${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+cnr_target_compile_options(${PROJECT_NAME})
 
 add_executable        (${PROJECT_NAME}_node src/configuration_manager_node.cpp )
 add_dependencies      (${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries (${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES} )
-target_compile_options(${PROJECT_NAME}_node PUBLIC -Wall -faligned-new
-        $<$<CONFIG:Release>:-Ofast -funroll-loops -ffast-math >)
+target_link_libraries (${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+cnr_target_compile_options(${PROJECT_NAME}_node)
 
 if(${CMAKE_VERSION} VERSION_GREATER "3.16.0")
 	target_precompile_headers (${PROJECT_NAME} PUBLIC <ros/ros.h>
@@ -99,14 +94,17 @@ endif()
 add_executable(configuration_user_interface src/ui_configuration_manager.cpp)
 add_dependencies(configuration_user_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(configuration_user_interface ${catkin_LIBRARIES})
+cnr_target_compile_options(configuration_user_interface)
 
 add_executable(joy_teach_pendant src/joy_teach_pendant.cpp)
 add_dependencies(joy_teach_pendant ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(joy_teach_pendant ${catkin_LIBRARIES})
+cnr_target_compile_options(joy_teach_pendant)
 
 add_executable(dispatcher src/dispatcher.cpp)
 add_dependencies(dispatcher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(dispatcher ${catkin_LIBRARIES})
+cnr_target_compile_options(dispatcher)
 
 
 #############

cnr_configuration_manager/cmake/cnrConfigMacros.cmake

@@ -0,0 +1,55 @@
+####
+#
+#
+####
+macro(cnr_set_flags)
+  if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]")
+    string(REGEX REPLACE "/W[0-4]" "/W3" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+  endif()
+
+  if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+  endif()
+
+  if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+  endif()
+
+  IF("${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}" LESS 3.0)
+    set(CMAKE_CXX_FLAGS "-std=c++11")
+  ELSE()
+    # Default to C++14
+    if(NOT CMAKE_CXX_STANDARD)
+      set(CMAKE_CXX_STANDARD 14)
+    endif()
+
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+    set(CMAKE_CXX_EXTENSIONS OFF)
+  endif()
+
+endmacro()
+
+####
+#
+#
+####
+macro(cnr_target_compile_options TARGET_NAME)
+
+if (CMAKE_CXX_COMPILER_ID MATCHES "Clang|AppleClang|GNU")
+
+  target_compile_options(${TARGET_NAME} 
+    PRIVATE -Wall -Wextra -Wunreachable-code -Wpedantic -Wno-gnu-zero-variadic-macro-arguments
+    PUBLIC $<$<CONFIG:Release>:-Ofast -funroll-loops -ffast-math >)
+
+elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+
+  target_compile_options(${TARGET_NAME} 
+    PRIVATE -Wweak-vtables -Wexit-time-destructors -Wglobal-constructors -Wmissing-noreturn
+    PUBLIC $<$<CONFIG:Release>:-Ofast -funroll-loops -ffast-math >)
+
+elseif(CMAKE_CXX_COMPILER_ID MATCHES "MSVC")
+
+  target_compile_options(${TARGET_NAME} PRIVATE /W3  )
+
+  endif()
+endmacro()

cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_loader.h

@@ -109,15 +109,15 @@ class ConfigurationLoader
   /** @brief Load of the RobotHW (if needed) and load and Start of the controllers for such RobotHW
    */ 
   bool loadAndStartControllers(const std::string& hw_name, const ConfigurationStruct& next_configuration,
-                                const size_t& strictness, std::string& error);
+                                const size_t& strictness, const std::string& configuration_name, std::string& error);
 
   /** @brief Parallel Load of a set of RobotHW (if needed) and load and Start of the controllers for such RobotHW
    * 
    * NOTE: First the HW are loaded, if needed, and then the controllers are launched.
    */ 
   bool loadAndStartControllers(const std::vector<std::string>& hw_next_names,
                                 const ConfigurationStruct& next_configuration, const size_t& strictness,
-                                  std::string& error);
+                                  const std::string& configuration_name, std::string& error);
 
   /** @brief stop and unloads the controller, and unload the RobotHW
    */

cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_manager.h

@@ -65,16 +65,16 @@ class ConfigurationManager
   ~ConfigurationManager() noexcept(false);
 
   bool startCallback(configuration_msgs::StartConfiguration::Request& req,
-                     configuration_msgs::StartConfiguration::Response& res);
+                      configuration_msgs::StartConfiguration::Response& res);
 
   bool stopCallback(configuration_msgs::StopConfiguration::Request& req,
-                    configuration_msgs::StopConfiguration::Response& res);
+                      configuration_msgs::StopConfiguration::Response& res);
 
   bool listConfigurations(configuration_msgs::ListConfigurations::Request& req,
-                          configuration_msgs::ListConfigurations::Response& res);
+                            configuration_msgs::ListConfigurations::Response& res);
 
   bool updateConfigurations(configuration_msgs::UpdateConfigurations::Request& req,
-                            configuration_msgs::UpdateConfigurations::Response& res);
+                              configuration_msgs::UpdateConfigurations::Response& res);
 
   bool init();
   //!
@@ -98,7 +98,8 @@ class ConfigurationManager
   SignalHandler                               m_signal_handler;
 
   bool checkRobotHwState(const std::string& hw, const cnr_hardware_interface::StatusHw& expected);
-  bool callback(const ConfigurationStruct& next_configuration, const int &strictness, const ros::Duration& watchdog);
+  bool callback(const ConfigurationStruct& next_configuration, const int& strictness,
+                  const std::string& configuration_name, const ros::Duration& watchdog);
   bool getAvailableConfigurationsFromParam();
 };
 

cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_loader.cpp

@@ -266,6 +266,7 @@ bool ConfigurationLoader::unloadHw(const std::vector<std::string>& hw_to_unload_
 bool ConfigurationLoader::loadAndStartControllers(const std::string& hw_name,
                                                   const ConfigurationStruct& next_conf,
                                                   const size_t& strictness,
+                                                  const std::string& configuration_name,
                                                   std::string& error)
 {
   //================================================
@@ -309,7 +310,7 @@ bool ConfigurationLoader::loadAndStartControllers(const std::string& hw_name,
 
 
   //================================================
-  if (!drivers_[hw_name]->loadAndStartControllers(next_controllers, strictness, watchdog))
+  if (!drivers_[hw_name]->loadAndStartControllers(next_controllers, strictness, watchdog, configuration_name))
   {
     error = "Error in switching the controller: " 
             + drivers_[hw_name]->getControllerManagerInterface()->error();
@@ -325,6 +326,7 @@ bool ConfigurationLoader::loadAndStartControllers(const std::string& hw_name,
 bool ConfigurationLoader::loadAndStartControllers(const std::vector<std::string>& hw_next_names,
                                                   const ConfigurationStruct& next_conf,
                                                   const size_t& strictness, 
+                                                  const std::string& configuration_name,
                                                   std::string& error)
 {
   // PARALLEL START OF THE CONTROLLERS
@@ -358,7 +360,7 @@ bool ConfigurationLoader::loadAndStartControllers(const std::vector<std::string>
                               ? ros::Duration(0.0) : ros::Duration(2.0);
       }
 
-      ok = drivers_[hw_name]->loadAndStartControllers(next_controllers,strictness, watchdog);
+      ok = drivers_[hw_name]->loadAndStartControllers(next_controllers,strictness, watchdog, configuration_name);
     }
     catch(std::exception& e)
     {

cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_manager.cpp

@@ -147,7 +147,7 @@ bool ConfigurationManager::startCallback(configuration_msgs::StartConfiguration:
     }
     else
     {
-      res.ok = callback(m_configurations.at(req.start_configuration), req.strictness,  ros::Duration(10.0));
+      res.ok = callback(m_configurations.at(req.start_configuration), req.strictness, req.start_configuration, ros::Duration(10.0));
       if (res.ok)
       {
         m_active_configuration_name = req.start_configuration;
@@ -190,7 +190,7 @@ bool ConfigurationManager::stopCallback(configuration_msgs::StopConfiguration::R
   {
     const std::lock_guard<std::mutex> lock(m_callback_mutex);
     ConfigurationStruct empty;
-    res.ok = callback(empty, req.strictness, ros::Duration(10.0));
+    res.ok = callback(empty, req.strictness, m_active_configuration_name, ros::Duration(10.0));
     if (res.ok)
     {
       m_active_configuration_name = "";
@@ -309,7 +309,7 @@ bool ConfigurationManager::run()
     CNR_WARN(m_logger, "********************* RUN ****************************");
     while (ros::ok())
     {
-      bool full_check = (((cnt++) % decimator) == 0);
+      //bool full_check = (((cnt++) % decimator) == 0);
       if (!isOk())
       {
         CNR_WARN_THROTTLE(m_logger, 2, "\n\nRaised an Error by one of the Hw! Stop Configuration start!\n\n");
@@ -423,6 +423,7 @@ bool ConfigurationManager::checkRobotHwState(const std::string& hw, const cnr_ha
  */
 bool ConfigurationManager::callback(const ConfigurationStruct& next_configuration,
                                     const int&                 strictness,
+                                    const std::string&         configuration_name,
                                     const ros::Duration&       watchdog)
 {
   CNR_TRACE_START(m_logger);
@@ -443,6 +444,7 @@ bool ConfigurationManager::callback(const ConfigurationStruct& next_configuratio
   }
 
   extract<std::string>(hw_next_names, hw_active_names, &hw_to_load_names, &hw_to_unload_names, nullptr);
+  CNR_DEBUG(m_logger, "STRICTNESS                 : " << std::to_string(strictness));
   CNR_DEBUG(m_logger, "HW NAMES - ACTIVE (CLASS)  : " << to_string(hw_active_names));
   CNR_DEBUG(m_logger, "HW NAMES - ACTIVE (NODELET): " << to_string(hw_names_from_nodelet));
   CNR_DEBUG(m_logger, "HW NAMES - NEXT            : " << to_string(hw_next_names));
@@ -547,7 +549,7 @@ bool ConfigurationManager::callback(const ConfigurationStruct& next_configuratio
   CNR_INFO(m_logger, cnr_logger::BM() << ">>>>>>>>>>>> Load and Start Controllers (hw: "
                     << cnr::control::to_string(hw_next_names) << ")"  << cnr_logger::RST());
 
-  if(!m_conf_loader.loadAndStartControllers(hw_next_names, next_configuration, strictness, error))
+  if(!m_conf_loader.loadAndStartControllers(hw_next_names, next_configuration, strictness, configuration_name, error))
   {
     CNR_ERROR(m_logger, "Failed while oading the controllers: " << error);
     CNR_RETURN_FALSE(m_logger, "Configuring HW Failed");

cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt

@@ -2,13 +2,9 @@ cmake_minimum_required(VERSION 3.1)
 
 project(cnr_controller_interface)
 
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE Release)
-endif()
+include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/cnrConfigMacros.cmake)
 
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
+cnr_set_flags()
 
 find_package(catkin REQUIRED COMPONENTS
   realtime_utilities cnr_controller_interface_params cnr_hardware_interface controller_interface diagnostic_msgs
@@ -41,9 +37,7 @@ include_directories(
 add_library(${PROJECT_NAME} src/cnr_controller_interface/cnr_controller_interface.cpp
                             src/cnr_controller_interface/internal/cnr_handles.cpp )
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_SYSTEM_LIBRARY} Eigen3::Eigen)
-target_compile_options(${PROJECT_NAME} PUBLIC -Wall -faligned-new
-        $<$<CONFIG:Release>:-Ofast -funroll-loops -ffast-math >)
-
+cnr_target_compile_options(${PROJECT_NAME})
 
 if(${CMAKE_VERSION} VERSION_GREATER  "3.16.0")
 	target_precompile_headers(${PROJECT_NAME} PUBLIC
@@ -100,6 +94,7 @@ if(CATKIN_ENABLE_TESTING)
     ${EIGEN3_INCLUDE_DIRS}
     ${cnr_fake_hardware_interface_INCLUDE_DIRS})
   target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} ${catkin_LIBRARIES} ${roscpp_LIBRARIES} ${cnr_fake_hardware_interface_LIBRARIES})
+  cnr_target_compile_options(${PROJECT_NAME}_test)
 
   if(ENABLE_COVERAGE_TESTING)
     set(COVERAGE_EXCLUDES "*/test*")
@@ -108,5 +103,16 @@ if(CATKIN_ENABLE_TESTING)
       DEPENDENCIES ${PROJECT_NAME}_test
     )
   endif()
+
+  install(DIRECTORY test/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+    PATTERN ".svn" EXCLUDE)
+
+  install(TARGETS ${PROJECT_NAME}_test
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
 endif()
 
+