diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 8b716b7..c6414cd 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.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 }} diff --git a/ci.rosinstall b/ci.rosinstall new file mode 100644 index 0000000..a7f07bd --- /dev/null +++ b/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 + diff --git a/cnr_configuration_manager/CMakeLists.txt b/cnr_configuration_manager/CMakeLists.txt index 67d907a..9e5e76f 100644 --- a/cnr_configuration_manager/CMakeLists.txt +++ b/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 - $<$:-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 - $<$:-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 @@ -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) ############# diff --git a/cnr_configuration_manager/cmake/cnrConfigMacros.cmake b/cnr_configuration_manager/cmake/cnrConfigMacros.cmake new file mode 100644 index 0000000..66f88fd --- /dev/null +++ b/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 $<$:-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 $<$:-Ofast -funroll-loops -ffast-math >) + +elseif(CMAKE_CXX_COMPILER_ID MATCHES "MSVC") + + target_compile_options(${TARGET_NAME} PRIVATE /W3 ) + + endif() +endmacro() diff --git a/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_loader.h b/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_loader.h index 7713059..4c83153 100644 --- a/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_loader.h +++ b/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_loader.h @@ -109,7 +109,7 @@ 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 * @@ -117,7 +117,7 @@ class ConfigurationLoader */ bool loadAndStartControllers(const std::vector& 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 */ diff --git a/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_manager.h b/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_manager.h index cde5236..2570cc7 100644 --- a/cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_manager.h +++ b/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(); }; diff --git a/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_loader.cpp b/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_loader.cpp index daa1f63..c895cf0 100644 --- a/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_loader.cpp +++ b/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_loader.cpp @@ -266,6 +266,7 @@ bool ConfigurationLoader::unloadHw(const std::vector& 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& 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 ? 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) { diff --git a/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_manager.cpp b/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_manager.cpp index 2fbaa88..9ae6966 100644 --- a/cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_manager.cpp +++ b/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 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(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"); diff --git a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt index f5c79cb..143d664 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt +++ b/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 - $<$:-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() + diff --git a/cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake b/cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake new file mode 100644 index 0000000..e9a56da --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/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 $<$:-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 $<$:-Ofast -funroll-loops -ffast-math >) + +elseif(CMAKE_CXX_COMPILER_ID MATCHES "MSVC") + + target_compile_options(${TARGET_NAME} PRIVATE /W3 /bigobj /Gy ) + + endif() +endmacro() diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_controller_interface.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_controller_interface.h index e4f0e94..65c5726 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_controller_interface.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_controller_interface.h @@ -53,7 +53,17 @@ namespace cnr { namespace control { +template +void unused(T const & v) +{ + static_cast(v); +} +template +void unused(T first, Args... args) { + unused(first); + return unused(args...); +} template class Controller: public controller_interface::Controller, @@ -83,22 +93,27 @@ class Controller: public controller_interface::Controller, } virtual bool doStarting(const ros::Time& time) { + unused(time); return true; } virtual bool doUpdate(const ros::Time& time, const ros::Duration& period) { + unused(time,period); return true; } virtual bool doStopping(const ros::Time& time) { + unused(time); return true; } virtual bool doWaiting(const ros::Time& time) { + unused(time); return true; } virtual bool doAborting(const ros::Time& time) { + unused(time); return true; } std::string getRootNamespace() @@ -211,7 +226,7 @@ class Controller: public controller_interface::Controller, protected: T* m_hw; ros::Duration m_dt; - cnr_logger::TraceLoggerPtr m_logger; + mutable cnr_logger::TraceLoggerPtr m_logger; std::string m_hw_name; std::string m_ctrl_name; double m_sampling_period; @@ -229,7 +244,7 @@ class Controller: public controller_interface::Controller, std::vector> m_sub_notifier; std::vector> m_sub; - std::vector m_sub_time; + std::vector m_sub_time; std::vector m_sub_time_track; bool callAvailable( ); diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_command_controller_interface.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_command_controller_interface.h index 01c9f5f..76f0605 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_command_controller_interface.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_command_controller_interface.h @@ -116,11 +116,24 @@ class JointCommandController: public cnr::control::JointController mutable std::mutex m_mtx; private: + + // alias + using jc = cnr::control::JointController; + + // change scope, to avoid that they are used from inherited classes + using jc::stop_update_transformations_; + using jc::update_transformations_runnig_; + using jc::m_rstate_mtx; + using jc::m_rstate_threaded; + using jc::m_chain_threaded; + InputType m_priority; + mutable std::mutex m_target_mtx; rosdyn::ChainState m_target; + rosdyn::ChainState m_target_threaded; + rosdyn::ChainState m_last_target; rosdyn::ChainStatePublisherPtr m_target_pub; - double m_override; void overrideCallback(const std_msgs::Int64ConstPtr& msg); @@ -131,7 +144,7 @@ class JointCommandController: public cnr::control::JointController void safeOverrideCallback_1(const std_msgs::Int64ConstPtr& msg); void safeOverrideCallback_2(const std_msgs::Int64ConstPtr& msg); - virtual void updateTransformationsThread(int ffwd_kin_type, double hz); + virtual void updateTransformationsThread(int ffwd_kin_type, double hz) override; }; } // namespace control diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_controller_interface.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_controller_interface.h index 13e69ff..487f352 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_controller_interface.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_joint_controller_interface.h @@ -89,8 +89,8 @@ class JointController: public cnr::control::Controller protected: // Accessors, to be used by the inherited classes - const unsigned int& nAx( ) const { return m_chain.getActiveJointsNumber(); } - const std::vector& jointNames( ) const { return m_chain.getActiveJointsName(); } + const unsigned int& nAx( ) const; + const std::vector& jointNames( ) const; Handler m_handler; urdf::ModelInterfaceSharedPtr m_urdf_model; @@ -116,22 +116,35 @@ class JointController: public cnr::control::Controller const Eigen::Vector6d& getTwistd( ) const; const rosdyn::Matrix6Xd& getJacobian( ) const; + double getKinUpdatePeriod() const; + void setKinUpdatePeriod(const double& fkin_update_period); + +protected: bool startUpdateTransformationsThread(int ffwd_kin_type, double hz = 10.0); void stopUpdateTransformationsThread(); virtual void updateTransformationsThread(int ffwd_kin_type, double hz); - std::thread update_transformations_; - bool stop_update_transformations_; bool update_transformations_runnig_; - mutable std::mutex mtx_; - - double getKinUpdatePeriod() const { return m_fkin_update_period; } - void setKinUpdatePeriod(const double& fkin_update_period) { m_fkin_update_period = fkin_update_period; } + bool stop_update_transformations_; + + mutable std::mutex m_rstate_mtx; + rosdyn::ChainState m_rstate_threaded; + rosdyn::Chain m_chain_threaded; + private: + + + std::thread update_transformations_; + + rosdyn::LinkPtr m_root_link; //link primitivo da cui parte la catena cinematica(world ad esempio) - rosdyn::Chain m_chain; + + mutable std::mutex m_chain_mtx; + rosdyn::Chain m_chain; + rosdyn::ChainState m_rstate; + Eigen::IOFormat m_cfrmt; double m_fkin_update_period; }; diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_controller_interface.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_controller_interface.h new file mode 100644 index 0000000..8296ce0 --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_controller_interface.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (New BSD License) + * + * Copyright 2020 National Council of Research of Italy (CNR) + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef CNR_CONTROLLER_INTERFACE__MULTI_CHAIN_CONTROLLER_INTERFACE_H +#define CNR_CONTROLLER_INTERFACE__MULTI_CHAIN_CONTROLLER_INTERFACE_H + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +namespace cnr +{ +namespace control +{ + +/** + * @brief The class is designed to get the feedback of a set of joints, + * and the joints must be connected to each other. + * The class is built aroun a 'rosdyn::ChainState' that stores the state + * of the joints, and at each cycle time the internal status is updated. + * The class creates a parallel thread cyclically (as the sampling rate) + * compute the forward kinematics. Furthermore, the effort may be computed + * from the external force measure id available. + * The computation is therefore done in parallel to avoid that the 'update' method + * takes too long, breaking the soft-realtime of the controller + * + */ +template +class MultiChainController: public cnr::control::Controller +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + virtual ~MultiChainController(); + + virtual bool doInit() override; + virtual bool doStarting(const ros::Time& time) override; + virtual bool doUpdate(const ros::Time& time, const ros::Duration& period) override; + virtual bool doStopping(const ros::Time& time) override; + virtual bool doWaiting(const ros::Time& time) override; + virtual bool doAborting(const ros::Time& time) override; + + virtual bool enterInit() override; + virtual bool enterStarting() override; + virtual bool exitStarting() override; + virtual bool enterUpdate() override; + +protected: + // Accessors, to be used by the inherited classes + const unsigned int& nAx(const std::string& id) const; + unsigned int totalNAx() const; + const std::vector& chainNames( ) const; + const std::vector& jointNames(const std::string& id) const; + std::vector allJointNames() const; + + std::map > handlers_; + urdf::ModelInterfaceSharedPtr m_urdf_model; + + const rosdyn::Chain& chain(const std::string& id) const; + rosdyn::Chain& chainNonConst(const std::string& id); + + const rosdyn::ChainState& chainState(const std::string& id) const; + rosdyn::ChainState& chainState(const std::string& id); + + const rosdyn::VectorXd& getPosition (const std::string& id) const; + const rosdyn::VectorXd& getVelocity (const std::string& id) const; + const rosdyn::VectorXd& getAcceleration(const std::string& id) const; + const rosdyn::VectorXd& getEffort (const std::string& id) const; + + double getPosition (const std::string& id,int idx) const; + double getVelocity (const std::string& id,int idx) const; + double getAcceleration(const std::string& id,int idx) const; + double getEffort (const std::string& id,int idx) const; + + const Eigen::Affine3d& getToolPose(const std::string& id) const; + const Eigen::Vector6d& getTwist(const std::string& id) const; + const Eigen::Vector6d& getTwistd(const std::string& id) const; + const rosdyn::Matrix6Xd& getJacobian(const std::string& id) const; + + bool startUpdateTransformationsThread(int ffwd_kin_type, double hz = 10.0); + void stopUpdateTransformationsThread(); + virtual void updateTransformationsThread(int ffwd_kin_type, double hz); + + std::thread update_transformations_; + bool stop_update_transformations_; + bool update_transformations_runnig_; + mutable std::mutex rstate_mtx_; + std::map rstate_threaded_; + + + double getKinUpdatePeriod() const; + void setKinUpdatePeriod(const double& fkin_update_period); + +private: + rosdyn::LinkPtr m_root_link; //link primitivo da cui parte la catena cinematica(world ad esempio) + + mutable std::mutex m_chain_mtx; + std::vector m_chain_ids; + std::map m_chain; + std::map m_chain_threaded; + + std::map m_rstate; + + Eigen::IOFormat m_cfrmt; + double m_fkin_update_period; +}; + +} // control +} // cnr + +#include + +#endif // CNR_CONTROLLER_INTERFACE__MULTI_CHAIN_CONTROLLER_INTERFACE_H + diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_controller_interface_impl.h index db750d4..1f79e3b 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_controller_interface_impl.h @@ -59,7 +59,12 @@ template Controller::~Controller() { CNR_TRACE_START(m_logger); - shutdown("UNLOADED"); + //shutdown("UNLOADED"); + bool ret = shutdown("UNLOADED"); + if(ret) + { + m_controller_nh_callback_queue.disable(); + } CNR_TRACE(m_logger, "[ DONE]"); } @@ -177,15 +182,17 @@ bool Controller::prepareInit(T* hw, l = __LINE__; m_logger.reset(new cnr_logger::TraceLogger()); - if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, ctrl_name, false, false)) + std::string what; + if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, ctrl_name, false, false, &what)) { - if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, hw_name, false, false)) + if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, hw_name, false, false, &what)) { std::cerr << cnr_logger::RED() << __PRETTY_FUNCTION__ << ":" << __LINE__ << ": " ; std::cerr << "The logger cannot be configured:" << std::endl; std::cerr << " root ns (the namespace of the hw): " << hw_name << std::endl; std::cerr << " controller ns (the namespace of the ctrl): " << ctrl_name << cnr_logger::RST() << std::endl; + std::cerr << "what: " << what << std::endl; return false; } } @@ -196,7 +203,6 @@ bool Controller::prepareInit(T* hw, m_controller_nh = controller_nh; // handle to callback and remapping m_hw = hw; - std::string what; if(!rosparam_utilities::get(m_root_nh.getNamespace()+"/sampling_period", m_sampling_period, what)) { CNR_RETURN_FALSE(m_logger, what); @@ -255,32 +261,35 @@ template void Controller::starting(const ros::Time& time) { CNR_TRACE_START(m_logger); + bool started = false; + bool aborted = false; try { - if(enterStarting() && doStarting(time) && exitStarting()) + started = enterStarting() && doStarting(time) && exitStarting(); + } + catch(std::exception& e) + { + CNR_ERROR(m_logger, "The starting of the controller failed. Exception: " << e.what() ); + started = false; + } + + if(!started) + { + try { - //dump_state("RUNNING"); - CNR_RETURN_OK(m_logger, void(), "Starting Ok! Ready to go!"); + CNR_ERROR(m_logger, "The starting of the controller failed. Abort Request to ControllerManager sent."); + aborted = controller_interface::Controller::abortRequest(time); } - else + catch(std::exception& e) { - CNR_ERROR(m_logger, "The starting of the controller failed. Abort Request to ControllerManager sent."); - if(controller_interface::Controller::abortRequest(time)) - { - //dump_state("ABORTED"); - } - else - { - this->state_ = controller_interface::ControllerBase::ControllerState::ABORTED; - //dump_state("ERROR"); - } + CNR_ERROR(m_logger, "The starting of the controller failed. Exception: " << e.what() ); + } + if(!aborted) + { + throw std::runtime_error("The starting of the controller failed, as the abort!?!?!"); } } - catch(std::exception& e) - { - CNR_ERROR(m_logger, "The starting of the controller failed. Exception:" << e.what() ); - } - CNR_RETURN_NOTOK(m_logger, void()); + CNR_RETURN_VOID(m_logger, (started ? true : false)); } template @@ -477,8 +486,8 @@ bool Controller::exitUpdate() CNR_TRACE_START_THROTTLE_DEFAULT(m_logger); if(m_sub.size() > 0) { - ros::WallTime now = ros::WallTime::now(); - ros::WallTime last_message_time = ros::WallTime::now(); + ros::Time now = ros::Time::now(); + ros::Time last_message_time = ros::Time::now(); for(size_t idx=0; idx::exitUpdate() if(m_sub_time_track.at(idx)) { m_sub_time.at(idx)->get(last_message_time); - ros::WallDuration time_span = (now - last_message_time); + ros::Duration time_span = (now - last_message_time); if(time_span.toSec() > m_watchdog) { CNR_ERROR_THROTTLE(m_logger, 5.0, "Watchdog on subscribed topic '" + m_sub.at(idx)->getTopic()+ "' " + @@ -515,12 +524,12 @@ template bool Controller::exitStopping() { CNR_TRACE_START(m_logger); - bool ret = shutdown("STOPPED"); - if(ret) - { - m_controller_nh_callback_queue.disable(); - } - CNR_RETURN_BOOL(m_logger, ret); + // bool ret = shutdown("STOPPED"); + // if(ret) + // { + // m_controller_nh_callback_queue.disable(); + // } + CNR_RETURN_BOOL(m_logger, true); } template @@ -595,6 +604,8 @@ bool Controller::dump_state() template bool Controller::shutdown(const std::string& state_final) { + unused(state_final); + CNR_TRACE_START(m_logger); for(auto & t : m_sub) { diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_handles.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_handles.h index e110fe2..64b2627 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_handles.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_handles.h @@ -2,6 +2,7 @@ #define CNR_CONTROLLER_INTERFACE__CNR_HANDLES__H #include +#include #include #include #include @@ -13,7 +14,7 @@ namespace cnr namespace control { -inline const rosdyn::ChainState* const getPtr(const rosdyn::ChainState& in) +inline const rosdyn::ChainState* getPtr(const rosdyn::ChainState& in) { return ∈ } @@ -29,8 +30,9 @@ typedef const std::shared_ptr HandleIndexesConstPtr; HandleIndexes get_index_map(const std::vector& names, const rosdyn::Chain& ks); -struct HandlerBase +class HandlerBase { +protected: bool initialized_ = false; HandleIndexes indexes_; @@ -42,6 +44,9 @@ struct HandlerBase indexes_ = get_index_map(names,chain); initialized_ = true; } + +public: + virtual void init(const rosdyn::Chain& chain) = 0; }; @@ -50,8 +55,9 @@ struct Handler : public HandlerBase { std::map handles_; - void flush(rosdyn::ChainState& /*ks*/, const rosdyn::Chain& /*chain*/) {} - void update(const rosdyn::ChainState& /*ks*/, const rosdyn::Chain& /*chain*/) {} + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& /*ks*/) {} + void update(const rosdyn::ChainState& /*ks*/) {} }; @@ -64,9 +70,12 @@ struct Handler handles_; - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -76,7 +85,7 @@ struct Handler handles_; - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); - for(auto const ax : indexes_) + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); ks.qd(ax.second) = handles_.at(ax.first).getVelocity(); @@ -102,9 +114,11 @@ struct Handler handles_; - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -134,9 +151,11 @@ struct Handler handles_; - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -167,9 +189,11 @@ struct Handler handles_; - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -197,10 +224,11 @@ struct Handler handles_; - - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -230,9 +260,10 @@ struct Handler handles_; - void flush(rosdyn::ChainState& ks, const rosdyn::Chain& chain) + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } + void flush(rosdyn::ChainState& ks) { - if(!initialized_) init(handles_, chain); + if(!initialized_) + throw std::runtime_error(("Handler must be initialized! [line:" + std::to_string(__LINE__) +"]").c_str() ); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -260,9 +294,11 @@ struct Handler::enterInit() CNR_RETURN_FALSE(this->m_logger); } + m_priority = QD_PRIORITY; - m_target.init(this->chainNonConst()); - m_last_target.init(this->chainNonConst()); + { + std::lock_guard lock(this->m_target_mtx); + m_target.init(this->chainNonConst()); + m_last_target.init(this->chainNonConst()); + m_target_threaded.init(this->chainNonConst()); + } this->template add_subscriber("/speed_ovr" , 1, boost::bind(&JointCommandController::overrideCallback, this, _1), false); @@ -154,7 +159,10 @@ inline bool JointCommandController::enterStarting() m_target.setZero(this->chainNonConst()); m_target.q() = this->getPosition(); - this->m_handler.update(m_target, this->chain()); + { + std::lock_guard lock(this->m_target_mtx); + this->m_handler.update(m_target); + } CNR_DEBUG(this->m_logger, "Target at Start: Position: " << m_target.q().transpose() ); CNR_DEBUG(this->m_logger, "Target at Start: Velocity: " << m_target.qd().transpose() ); @@ -186,9 +194,10 @@ inline bool JointCommandController::exitUpdate() bool print_report = false; double throttle_time = 1.0; CNR_TRACE_START_THROTTLE_DEFAULT(this->m_logger); - + size_t ll = __LINE__; try { + ll = __LINE__; report << "==========\n"; report << "Priority : " << std::to_string(m_priority) << "\n"; report << "upper limit : " << TP(this->chain().getQMax()) << "\n"; @@ -196,6 +205,7 @@ inline bool JointCommandController::exitUpdate() report << "Speed Limit : " << TP(this->chain().getDQMax()) << "\n"; report << "Acceleration Limit : " << TP(this->chain().getDDQMax()) << "\n"; report << "----------\n"; + ll = __LINE__; // ============================== ============================== auto nominal_qd = m_target.q(); if(m_priority == Q_PRIORITY) @@ -220,32 +230,44 @@ inline bool JointCommandController::exitUpdate() report << "Nominal command qd(input) : " << TP(nominal_qd) << "\n"; report << "----------\n"; // ============================== ============================== - + ll = __LINE__; // ============================== ============================== auto saturated_qd = nominal_qd; + ll = __LINE__; if (m_priority != NONE) { + ll = __LINE__; if(rosdyn::saturateSpeed(this->chain(), saturated_qd, m_last_target.qd(), m_last_target.q(), this->m_sampling_period, m_max_velocity_multiplier, true, &report)) { print_report = true; m_target.qd() = saturated_qd; } + ll = __LINE__; m_target.q() = m_last_target.q() + saturated_qd * this->m_dt.toSec() +0.5*m_target.qdd()*std::pow(this->m_dt.toSec(),2.0); } + ll = __LINE__; m_last_target.copy(m_target, m_target.ONLY_JOINT); + ll = __LINE__; if(m_target_pub) { m_target_pub->publish(); } + ll = __LINE__; // ============================== } + catch(std::exception& e) + { + CNR_WARN(this->m_logger,"Exception (last executed line: " << ll << "):\n" << e.what()); + m_target.q() = m_last_target.q(); + eigen_utils::setZero(m_target.qd()); + } catch(...) { - CNR_WARN(this->m_logger,"something wrong in JointTargetFilter::update"); + CNR_WARN(this->m_logger,"Unhadled (last executed line: " << ll << ")"); m_target.q() = m_last_target.q(); eigen_utils::setZero(m_target.qd()); } @@ -254,7 +276,10 @@ inline bool JointCommandController::exitUpdate() report<< "qd trg: " << TP(m_target.qd()) << "\n"; report<< "ef trg: " << TP(m_target.effort()) << "\n"; - this->m_handler.update(m_target, this->chain()); + { + std::lock_guard lock(this->m_target_mtx); + this->m_handler.update(m_target); + } //for(size_t iAx=0; iAxjointNames().size(); iAx++) //{ @@ -282,8 +307,12 @@ inline bool JointCommandController::exitStopping() m_target.q(iAx) = this->getPosition(iAx); } eigen_utils::setZero(m_target.qd()); - this->m_handler.update(m_target, this->chain()); + { + std::lock_guard lock(this->m_target_mtx); + this->m_handler.update(m_target); + } + if(!JointController::exitStopping()) { CNR_RETURN_FALSE(this->m_logger); @@ -344,7 +373,7 @@ inline const rosdyn::ChainState& JointCommandController::chainCommand() con if(this->getKinUpdatePeriod()<=0) throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_target_mtx); return m_target; } @@ -353,7 +382,7 @@ inline rosdyn::ChainState& JointCommandController::chainCommand() { if(this->getKinUpdatePeriod()<=0) throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_target_mtx); return m_target; } @@ -473,47 +502,35 @@ template inline void JointCommandController::updateTransformationsThread(int ffwd_kin_type, double hz) { CNR_TRACE_START(this->logger()); - rosdyn::ChainState rstate; - rosdyn::ChainState target; - - { - std::lock_guard lock(this->mtx_); - if(!rstate.init(this->chainNonConst())) - { - CNR_FATAL(this->m_logger, "Chain failure!"); - CNR_RETURN_NOTOK(this->m_logger, void()); - } - if(!target.init(this->chainNonConst())) - { - CNR_FATAL(this->m_logger, "Chain failure!"); - CNR_RETURN_NOTOK(this->m_logger, void()); - } - } - ros::Rate rt(hz); while(!this->stop_update_transformations_) { + this->m_rstate_threaded.copy(this->chainState(), rosdyn::ChainState::ONLY_JOINT); + { - //std::lock_guard lock(this->mtx_); - rstate.copy(this->chainState(), this->chainState().ONLY_JOINT); - target.copy(this->m_target, this->m_target.ONLY_JOINT); + std::lock_guard lock(this->m_target_mtx); + m_target_threaded.copy(this->m_target, rosdyn::ChainState::ONLY_JOINT); } - rstate.updateTransformations(this->chainNonConst(), ffwd_kin_type); - target.updateTransformations(this->chainNonConst(), ffwd_kin_type); + this->m_rstate_threaded.updateTransformations(this->m_chain_threaded, ffwd_kin_type); + m_target_threaded.updateTransformations(this->m_chain_threaded, ffwd_kin_type); + + this->chainState().copy(this->m_rstate_threaded, rosdyn::ChainState::ONLY_CART); + { - //std::lock_guard lock(this->mtx_); - this->chainState().copy(rstate, rstate.ONLY_CART); - this->m_target.copy(target, target.ONLY_CART); + std::lock_guard lock(this->m_target_mtx); + this->m_target.copy(m_target_threaded, rosdyn::ChainState::ONLY_CART); } + if(!this->update_transformations_runnig_) { - CNR_INFO(this->logger(), "First state & target update ;)" + CNR_DEBUG(this->logger(), "First state & target update ;)" << "\nstate:\n" << std::to_string(this->chainState()) << "\ntarget:\n" << std::to_string(this->m_target)); this->update_transformations_runnig_ = true; } rt.sleep(); } + CNR_RETURN_OK(this->m_logger, void()); } diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_controller_interface_impl.h index a307703..5b302dd 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_controller_interface_impl.h @@ -107,8 +107,14 @@ bool JointController::enterInit() { std::string what; size_t l = __LINE__; + + try { + //==================================================== + std::lock_guard lock(this->m_chain_mtx); + //==================================================== + m_cfrmt = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); CNR_TRACE_START(this->m_logger); if(!Controller::enterInit()) @@ -194,11 +200,13 @@ bool JointController::enterInit() NEW_HEAP(m_root_link, rosdyn::Link() ); m_root_link->fromUrdf(GET(m_urdf_model->root_link_)); std::string error; - if(!m_chain.init(error,m_root_link,base_link, tool_link)) + if(!m_chain.init(error,m_root_link,base_link, tool_link) + || !m_chain_threaded.init(error,m_root_link,base_link, tool_link)) { CNR_ERROR(this->m_logger, "Failing in creating the Chain from the URDF model:\n\t" + error + ""); CNR_RETURN_FALSE(this->m_logger); } + //======================================= @@ -214,6 +222,8 @@ bool JointController::enterInit() CNR_ERROR(this->m_logger, "Mismatch between the chain names and the controlled joint names. Abort."); CNR_RETURN_FALSE(this->m_logger); } + m_chain_threaded.setInputJointsName(joint_names); + if(m_chain.getActiveJointsNumber()!=joint_names.size()) { CNR_ERROR(this->m_logger, "Mismatch of the dimension of the chain names and the controlled joint names. Abort."); @@ -253,9 +263,14 @@ bool JointController::enterInit() { CNR_WARN(this->m_logger, "Warning in setting the kin limits.: '" + error + "'"); } + m_chain_threaded.enforceLimitsFromRobotDescriptionParam(robot_description_planning_param, error); l = __LINE__; - m_rstate.init(m_chain); + { + std::lock_guard lock(this->m_rstate_mtx); + m_rstate.init(m_chain); + m_rstate_threaded.init(m_chain); + } l = __LINE__; for(unsigned int iAx=0; iAx::getControllerNamespace() + std::string("'") + "The controlled joint named '" + m_chain.getActiveJointName(iAx) + "' is managed by hardware_interface"); } + m_handler.init(m_chain); + CNR_DEBUG(this->m_logger, "Q sup : " << eigen_utils::to_string(m_chain.getQMax() )); CNR_DEBUG(this->m_logger, "Q inf : " << eigen_utils::to_string(m_chain.getQMin() )); CNR_DEBUG(this->m_logger, "Qd max : " << eigen_utils::to_string(m_chain.getDQMax() )); @@ -310,16 +327,19 @@ bool JointController::enterStarting() CNR_RETURN_FALSE(this->m_logger); } - CNR_DEBUG(this->m_logger, "HW Status\n" << std::to_string(m_handler) ); - CNR_DEBUG(this->m_logger, "Chain: " << std::to_string(m_chain.getActiveJointsNumber()) ); - CNR_DEBUG(this->m_logger, "First joint name: " << m_chain.getActiveJointsName().front() ); - CNR_DEBUG(this->m_logger, "Last joint name: " << m_chain.getActiveJointsName().back() ); - - m_handler.flush(m_rstate, m_chain); - - CNR_DEBUG(this->m_logger, "Position: " << eigen_utils::to_string(m_rstate.q()) ); - CNR_DEBUG(this->m_logger, "Velocity: " << eigen_utils::to_string(m_rstate.qd()) ); - CNR_DEBUG(this->m_logger, "Effort : " << eigen_utils::to_string(m_rstate.effort()) ); + { + CNR_DEBUG(this->m_logger, "HW Status\n" << std::to_string(m_handler) ); + CNR_DEBUG(this->m_logger, "Chain: " << std::to_string(m_chain.getActiveJointsNumber()) ); + CNR_DEBUG(this->m_logger, "First joint name: " << m_chain.getActiveJointsName().front() ); + CNR_DEBUG(this->m_logger, "Last joint name: " << m_chain.getActiveJointsName().back() ); + { + std::lock_guard lock(this->m_rstate_mtx); + m_handler.flush(m_rstate); + CNR_DEBUG(this->m_logger, "Position: " << eigen_utils::to_string(m_rstate.q()) ); + CNR_DEBUG(this->m_logger, "Velocity: " << eigen_utils::to_string(m_rstate.qd()) ); + CNR_DEBUG(this->m_logger, "Effort : " << eigen_utils::to_string(m_rstate.effort()) ); + } + } CNR_RETURN_TRUE(this->m_logger); } @@ -332,7 +352,29 @@ bool JointController::exitStarting() int ffwd = rosdyn::ChainState::SECOND_ORDER | rosdyn::ChainState::FFWD_STATIC; if(m_fkin_update_period>0) - startUpdateTransformationsThread(ffwd, 1.0/this->m_fkin_update_period); + { + stop_update_transformations_ = false; + update_transformations_runnig_ = false; + CNR_DEBUG(this->logger(), "Creating the fkin update thread"); + + update_transformations_ = std::thread( + &JointController::updateTransformationsThread, this, ffwd, 1.0/this->m_fkin_update_period); + + CNR_DEBUG(this->logger(), "Fkin Thread created"); + + double timeout = 1.0; + ros::Time st = ros::Time::now(); + while(!update_transformations_runnig_) + { + ros::Duration(0.005).sleep(); + if((ros::Time::now()-st).toSec()>timeout) + { + CNR_ERROR(this->m_logger,"The thread that updates the fkin didn't start within the timeout of "<< timeout << ". Abort"); + CNR_RETURN_TRUE(this->m_logger); + } + } + } + //startUpdateTransformationsThread(ffwd, 1.0/this->m_fkin_update_period); if(!Controller::exitStarting()) { @@ -351,7 +393,11 @@ bool JointController::enterUpdate() CNR_RETURN_FALSE(this->m_logger); } - m_handler.flush(m_rstate, m_chain); + { + std::lock_guard lock(this->m_rstate_mtx); + m_handler.flush(m_rstate); + } + // NOTE: the transformations may take time, especially due the pseudo inversion of the Jacobian, to estimate the external wrench. // Therefore, they are executed in parallel //m_rstate.updateTransformations(); @@ -366,10 +412,12 @@ inline bool JointController::startUpdateTransformationsThread(int ffwd_kin_ stop_update_transformations_ = false; update_transformations_runnig_ = false; CNR_DEBUG(this->logger(), "Creating the fkin update thread"); + update_transformations_ = std::thread( &JointController::updateTransformationsThread, this, ffwd_kin_type, hz); + CNR_DEBUG(this->logger(), "Fkin Thread created"); - + double timeout = 1.0; ros::Time st = ros::Time::now(); while(!update_transformations_runnig_) @@ -400,24 +448,21 @@ inline void JointController::stopUpdateTransformationsThread() template inline void JointController::updateTransformationsThread(int ffwd_kin_type, double hz) { - CNR_TRACE_START(this->m_logger); - rosdyn::ChainState rstate; - + CNR_TRACE_START(this->logger()); + ros::Rate rt(hz); + while(!this->stop_update_transformations_) { - std::lock_guard lock(this->mtx_); - if(!rstate.init(m_chain)) { - CNR_FATAL(this->m_logger, "Chain failure!"); - CNR_RETURN_NOTOK(this->m_logger, void()); + std::lock_guard lock(this->m_rstate_mtx); + m_rstate_threaded.copy(m_rstate, rosdyn::ChainState::ONLY_JOINT); + } + + m_rstate_threaded.updateTransformations(m_chain_threaded, ffwd_kin_type); + + { + std::lock_guard lock(this->m_rstate_mtx); + m_rstate.copy(m_rstate_threaded, rosdyn::ChainState::ONLY_CART); } - } - - ros::Rate rt(hz); - while(!stop_update_transformations_) - { - rstate.copy(m_rstate, m_rstate.ONLY_JOINT); - rstate.updateTransformations(m_chain, ffwd_kin_type); - m_rstate.copy(rstate, m_rstate.ONLY_CART); if(!this->update_transformations_runnig_) { @@ -426,88 +471,107 @@ inline void JointController::updateTransformationsThread(int ffwd_kin_type, } rt.sleep(); } - CNR_RETURN_OK(this->m_logger, void()); + CNR_RETURN_OK(this->logger(), void()); + } template inline const rosdyn::Chain& JointController::chain() const { + std::lock_guard lock(this->m_chain_mtx); return m_chain; } +template +inline const unsigned int& JointController::nAx( ) const +{ + std::lock_guard lock(this->m_chain_mtx); + return m_chain.getActiveJointsNumber(); +} + +template +inline const std::vector& JointController::jointNames( ) const +{ + std::lock_guard lock(this->m_chain_mtx); + return m_chain.getActiveJointsName(); +} + + template inline rosdyn::Chain& JointController::chainNonConst() { + std::lock_guard lock(this->m_chain_mtx); return m_chain; } + template inline const rosdyn::ChainState& JointController::chainState() const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate; } template inline rosdyn::ChainState& JointController::chainState() { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate; } template inline const rosdyn::VectorXd& JointController::getPosition( ) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.q(); } template inline const rosdyn::VectorXd& JointController::getVelocity( ) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.qd(); } template inline const rosdyn::VectorXd& JointController::getAcceleration( ) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.qdd(); } template inline const rosdyn::VectorXd& JointController::getEffort( ) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.effort(); } template inline double JointController::getPosition(int idx) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.q(idx); } template inline double JointController::getVelocity(int idx) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.qd(idx); } template inline double JointController::getAcceleration(int idx) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.qdd(idx); } template inline double JointController::getEffort(int idx) const { - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.effort(idx); } @@ -516,7 +580,7 @@ inline const Eigen::Affine3d& JointController::getToolPose( ) const { if(m_fkin_update_period<=0) throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.toolPose(); } @@ -525,7 +589,7 @@ inline const Eigen::Vector6d& JointController::getTwist( ) const { if(m_fkin_update_period<=0) throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.toolTwist(); } @@ -534,7 +598,7 @@ inline const Eigen::Vector6d& JointController::getTwistd( ) const { if(m_fkin_update_period<=0) throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.toolTwistd(); } @@ -543,11 +607,20 @@ inline const rosdyn::Matrix6Xd& JointController::getJacobian( ) const { if(m_fkin_update_period<=0) throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); - std::lock_guard lock(this->mtx_); + std::lock_guard lock(this->m_rstate_mtx); return m_rstate.toolJacobian(); } - +template +inline double JointController::getKinUpdatePeriod() const +{ + return m_fkin_update_period; +} +template +inline void JointController::setKinUpdatePeriod(const double& fkin_update_period) +{ + m_fkin_update_period = fkin_update_period; +} } // cnr_controller_interface } // namespace cnr diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_multi_chain_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_multi_chain_controller_interface_impl.h new file mode 100644 index 0000000..7e5e199 --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_multi_chain_controller_interface_impl.h @@ -0,0 +1,718 @@ +/* + * Software License Agreement(New BSD License) + * + * Copyright 2020 National Council of Research of Italy(CNR) + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES(INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#pragma once // workaround clang-tidy qtcreator + +#ifndef CNR_CONTOLLER_INTERFACE__CNR_MULTI_CHAIN_CONTROLLER_INTERFACE_IMPL_H +#define CNR_CONTOLLER_INTERFACE__CNR_MULTI_CHAIN_CONTROLLER_INTERFACE_IMPL_H + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ru = rosparam_utilities; + +namespace cnr +{ +namespace control +{ + +template +MultiChainController::~MultiChainController() +{ + CNR_TRACE_START(this->m_logger); + stopUpdateTransformationsThread(); + CNR_TRACE(this->m_logger, "OK"); +} + +template +bool MultiChainController::doInit() +{ + m_fkin_update_period = -1.0; + return cnr::control::Controller::doInit(); +} +template +bool MultiChainController::doStarting(const ros::Time& time) +{ + return cnr::control::Controller::doStarting(time); +} + +template +bool MultiChainController::doUpdate(const ros::Time& time, const ros::Duration& period) +{ + return cnr::control::Controller::doUpdate(time,period); +} + +template +bool MultiChainController::doStopping(const ros::Time& time) +{ + stopUpdateTransformationsThread(); + return cnr::control::Controller::doStopping(time); +} + +template +bool MultiChainController::doWaiting(const ros::Time& time) +{ + return cnr::control::Controller::doWaiting(time); +} + +template +bool MultiChainController::doAborting(const ros::Time& time) +{ + return cnr::control::Controller::doAborting(time); +} + +template +bool MultiChainController::enterInit() +{ + std::string what; + size_t l = __LINE__; + try + { + + //==================================================== + std::lock_guard lock(this->m_chain_mtx); + //==================================================== + + m_cfrmt = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); + CNR_TRACE_START(this->m_logger); + if(!Controller::enterInit()) + { + CNR_RETURN_FALSE(this->m_logger); + } + + m_fkin_update_period = -1; + if(!ru::get(this->getControllerNamespace() + "/kin_update_period", m_fkin_update_period, what, &m_fkin_update_period)) + { + CNR_WARN(this->m_logger, what); + CNR_WARN(this->m_logger, "The chain status will not be updated.");; + } + + //======================================= + // JOINT NAMES(RESOURCES) + //======================================= + std::vector joint_names; + rosdyn::get_joint_names(this->getControllerNh(),joint_names); + if(joint_names.size()==1 && joint_names.front() == "all") + { + rosdyn::get_joint_names(this->getRootNh(), joint_names); + } + + if(joint_names.size()==0) + { + CNR_RETURN_FALSE(this->m_logger, "Neither '" + this->getControllerNamespace() + "/controlled_joint(s)' nor '" + + this->getControllerNamespace() + "/controlled_resources(s)' are specified. Abort" ); + } + //======================================= + + + + //======================================= + // CHAINS + //======================================= + if(!ru::get(this->getControllerNamespace() + "/chain_ids", m_chain_ids, what ) ) + { + if(!ru::get(this->getRootNamespace() + "/chain_ids", m_chain_ids, what ) ) + { + CNR_RETURN_FALSE(this->m_logger, "'Neither '" + this->getControllerNamespace() + "/chain_ids' " + + "nor '" + this->getRootNamespace() + "/chain_ids' are not in rosparam server."); + } + } + //TODO FINd duplacete in the chain + + + + std::vector base_links; + if(!ru::get(this->getControllerNamespace() + "/base_links", base_links, what ) ) + { + if(!ru::get(this->getRootNamespace() + "/base_links", base_links, what ) ) + { + CNR_RETURN_FALSE(this->m_logger, "'Neither '" + this->getControllerNamespace() + "/base_links' " + + "nor '" + this->getRootNamespace() + "/base_links' are not in rosparam server."); + } + } + if(base_links.size()!=m_chain_ids.size()) + { + CNR_RETURN_FALSE(this->m_logger, "'Mismatch in dimensionf of 'base_links' and 'chain_ids'."); + } + + std::vector tool_links; + if(!ru::get(this->getControllerNamespace() + "/tool_links", tool_links, what ) ) + { + if(!ru::get(this->getRootNamespace() + "/tool_links", tool_links, what ) ) + { + CNR_RETURN_FALSE(this->m_logger, "'Neither '" + this->getControllerNamespace() + "/tool_links' " + + "nor '" + this->getRootNamespace() + "/tool_links' are not in rosparam server."); + } + } + if(tool_links.size()!=m_chain_ids.size()) + { + CNR_RETURN_FALSE(this->m_logger, "'Mismatch in dimensionf of 'tool_links' and 'chain_ids'."); + } + + std::string robot_description_param; + if(!ru::get(this->getControllerNamespace() + "/robot_description_param", robot_description_param, what ) ) + { + if(!ru::get(this->getRootNamespace() + "/robot_description_param", robot_description_param, what ) ) + { + CNR_RETURN_FALSE(this->m_logger, "'Neither '" + this->getControllerNamespace() + + "/robot_description_param' " + "nor '" + this->getRootNamespace() + + "/robot_description_param' are not in rosparam server."); + } + } + + std::string urdf_string; + if(!ros::param::get(robot_description_param, urdf_string)) + { + CNR_ERROR(this->m_logger, "\nWeird error in getting the parameter '" << robot_description_param + << "'. It was already checked the existence.\n"); + CNR_RETURN_FALSE(this->m_logger); + } + + m_urdf_model = urdf::parseURDF(urdf_string); + if(m_urdf_model == nullptr ) + { + CNR_ERROR(this->m_logger, "Failing in parsing the URDF model from parameter '" + urdf_string + "'"); + CNR_RETURN_FALSE(this->m_logger); + } + + NEW_HEAP(m_root_link, rosdyn::Link() ); + m_root_link->fromUrdf(GET(m_urdf_model->root_link_)); + std::string error; + for(size_t i=0;im_logger, "Multiple Chain with same ID!"); + CNR_RETURN_FALSE(this->m_logger); + } + if(!m_chain[m_chain_ids.at(i)].init(error,m_root_link,base_links.at(i), tool_links.at(i)) + || !m_chain_threaded[m_chain_ids.at(i)].init(error,m_root_link,base_links.at(i), tool_links.at(i))) + { + CNR_ERROR(this->m_logger, "Failing in creating the Chain from the URDF model:\n\t" + error + ""); + CNR_RETURN_FALSE(this->m_logger); + } + } + //======================================= + + + //======================================= + // SELECT ACTIVE JOINT FOR THE CHAIN + //======================================= + CNR_DEBUG(this->m_logger, "Controlled joint names: " << cnr::control::to_string(joint_names)); + std::map > chain_joint_names; + std::vector::iterator> joint_names_used; + for(const auto & id : this->chainNames()) + { + auto ajns = m_chain.at(id).getActiveJointsName(); + CNR_DEBUG(this->m_logger, "Chain '" << id << "' - Default Active Joint Names: " << cnr::control::to_string(ajns)); + + chain_joint_names[id] = std::vector{}; + for(auto const & ajn : ajns) + { + auto jt = std::find(joint_names.begin(), joint_names.end(), ajn); + if( jt == joint_names.end()) + { + continue; + } + + if( std::find(joint_names_used.begin(), joint_names_used.end(), jt ) != joint_names_used.end() ) + { + CNR_ERROR(this->m_logger, "Chain '" << id << "' - The joint '"+ *jt + "' is an active joint in multiple chain"); + CNR_RETURN_FALSE(this->m_logger); + } + + joint_names_used.push_back(jt); + chain_joint_names[id].push_back(ajn); + } + + int ok_coherence_jnames_and_chain = m_chain.at(id).setInputJointsName(chain_joint_names.at(id)); + if(ok_coherence_jnames_and_chain!=1) + { + CNR_ERROR(this->m_logger, "Chain '"<m_logger); + } + m_chain_threaded.at(id).setInputJointsName(chain_joint_names.at(id)); + } + + CNR_DEBUG(this->m_logger, "Check number of axes [" << totalNAx() <<"/" << joint_names_used.size() << "]"); + if(this->totalNAx()!=joint_names_used.size()) + { + CNR_ERROR(this->m_logger, "Mismatch of the dimension of the chain names and the controlled joint names"); + std::string jnu = "["; + for(const auto & jt : joint_names_used) + { + jnu += *jt + ","; + } + jnu +="]"; + CNR_ERROR(this->m_logger, "-All Joint Names used by Chains\n\t" << to_string(this->allJointNames())); + CNR_ERROR(this->m_logger, "- Joint Names Used\n\t" << jnu); + CNR_RETURN_FALSE(this->m_logger); + } + //======================================= + + + //======================================= + // KINEMATICS LIMITS + //======================================= + CNR_DEBUG(this->m_logger, "Get the 'robot_description_planning_param'"); + std::string robot_description_planning_param; + if(!ru::get(this->getControllerNamespace() + "/robot_description_planning_param", robot_description_planning_param, what ) ) + { + if(!ru::get(this->getRootNamespace() + "/robot_description_planning_param", robot_description_planning_param, what ) ) + { + CNR_ERROR(this->m_logger, "'Neither '" + this->getControllerNamespace() + + "/robot_description_planning_param' " + "nor '" + this->getRootNamespace() + + "/robot_description_param' are not in rosparam server."); + CNR_RETURN_FALSE(this->m_logger); + } + } + if(!ros::param::has(robot_description_planning_param)) + { + CNR_ERROR(this->m_logger, "The parameter '" << robot_description_planning_param << + " does not exist(check the value of parameter '" << robot_description_planning_param <<"'"); + CNR_RETURN_FALSE(this->m_logger); + } + +l = __LINE__; + for(auto const & id : m_chain_ids ) + { + CNR_WARN(this->m_logger, "ENFORCING LIMITS OF CHAIN '" + id + "'"); + auto & chain = m_chain.at(id); + + int res = chain.enforceLimitsFromRobotDescriptionParam(robot_description_planning_param, error); + if(res==-1) + { + CNR_ERROR(this->m_logger, "Failing in setting the kin limits. Error: '" + error + "'"); + CNR_RETURN_FALSE(this->m_logger); + } + else if(res==0) + { + CNR_WARN(this->m_logger, "Warning in setting the kin limits.: '" + error + "'"); + } + m_chain_threaded.at(id).enforceLimitsFromRobotDescriptionParam(robot_description_planning_param, error); + + { + std::lock_guard lock(this->rstate_mtx_); + m_rstate[id].init(chain); + rstate_threaded_[id].init(chain); + } + + CNR_DEBUG(this->m_logger, "Chain '"<::m_hw) + { + throw std::runtime_error("The HW is malformed!"); + } + if(handlers_.find(id)!=handlers_.end() + && handlers_.at(id).handles_.find(jn)!=handlers_.at(id).handles_.end()) + { + CNR_ERROR(this->m_logger, " Error: multiple handlers with same id"); + CNR_RETURN_FALSE(this->m_logger); + } + handlers_[id].handles_[jn] = Controller::m_hw->getHandle(jn); + handlers_[id].init(m_chain.at(id)); + } + catch(std::exception& e) + { + CNR_RETURN_FALSE(this->m_logger, + "Controller '" + Controller::getControllerNamespace() + "' failed in init. " + std::string("") + + "The controlled joint named '"+jn+"' is not managed by hardware_interface." + "Error: " + std::string(e.what())); + } + catch(...) + { + CNR_RETURN_FALSE(this->m_logger, + "Controller '" + Controller::getControllerNamespace() + "' failed in init. " + std::string("") + + "The controlled joint named '"+jn+"' is not managed by hardware_interface"); + } + + CNR_DEBUG(this->m_logger, + "Controller '" + Controller::getControllerNamespace() + std::string("'") + + "The controlled joint named '"+jn+"' is managed by hardware_interface"); + } + CNR_DEBUG(this->m_logger, "Q sup : " << eigen_utils::to_string(chain.getQMax() )); + CNR_DEBUG(this->m_logger, "Q inf : " << eigen_utils::to_string(chain.getQMin() )); + CNR_DEBUG(this->m_logger, "Qd max : " << eigen_utils::to_string(chain.getDQMax() )); + CNR_DEBUG(this->m_logger, "Qdd max: " << eigen_utils::to_string(chain.getDDQMax())); + } + } + catch(std::exception& e) + { + std::cerr << cnr_logger::RED() << __PRETTY_FUNCTION__ << ":" << __LINE__ << ": " ; + std::cerr << "Exception at line: " + << std::to_string(l) << " error: " + std::string(e.what()) + << std::endl; + } + + CNR_RETURN_TRUE(this->m_logger); +} + +template +bool MultiChainController::enterStarting() +{ + CNR_TRACE_START(this->m_logger); + if(!Controller::enterStarting()) + { + CNR_RETURN_FALSE(this->m_logger); + } + + { + std::lock_guard lock(this->rstate_mtx_); + for(auto const & id : this->chainNames()) + { + auto & chain = m_chain.at(id); + CNR_DEBUG(this->m_logger, "HW Status\n" << std::to_string(handlers_.at(id)) ); + CNR_DEBUG(this->m_logger, "Chain: " << std::to_string(chain.getActiveJointsNumber()) ); + CNR_DEBUG(this->m_logger, "First joint name: " << chain.getActiveJointsName().front() ); + CNR_DEBUG(this->m_logger, "Last joint name: " << chain.getActiveJointsName().back() ); + handlers_.at(id).flush(m_rstate.at(id)); + + CNR_DEBUG(this->m_logger, "Position: " << eigen_utils::to_string(m_rstate.at(id).q()) ); + CNR_DEBUG(this->m_logger, "Velocity: " << eigen_utils::to_string(m_rstate.at(id).qd()) ); + CNR_DEBUG(this->m_logger, "Effort : " << eigen_utils::to_string(m_rstate.at(id).effort()) ); + } + } + CNR_RETURN_TRUE(this->m_logger); +} + +template +bool MultiChainController::exitStarting() +{ + CNR_TRACE_START(this->m_logger); + + int ffwd = rosdyn::ChainState::SECOND_ORDER | rosdyn::ChainState::FFWD_STATIC; + + if(m_fkin_update_period>0) + startUpdateTransformationsThread(ffwd, 1.0/this->m_fkin_update_period); + + if(!Controller::exitStarting()) + { + CNR_RETURN_FALSE(this->m_logger); + } + + CNR_RETURN_TRUE(this->m_logger); +} + +template +bool MultiChainController::enterUpdate() +{ + CNR_TRACE_START_THROTTLE_DEFAULT(this->m_logger); + if(!Controller::enterUpdate()) + { + CNR_RETURN_FALSE(this->m_logger); + } + + { + std::lock_guard lock(this->rstate_mtx_); + for(auto const & id : this->chainNames()) + { + handlers_.at(id).flush(m_rstate.at(id)); + // NOTE: the transformations may take time, especially due the pseudo inversion of the Jacobian, to estimate the external wrench. + // Therefore, they are executed in parallel + } + } + + CNR_RETURN_TRUE_THROTTLE_DEFAULT(this->m_logger); +} + +template +inline bool MultiChainController::startUpdateTransformationsThread(int ffwd_kin_type, double hz) +{ + CNR_TRACE_START(this->m_logger); + stop_update_transformations_ = false; + update_transformations_runnig_ = false; + CNR_DEBUG(this->logger(), "Creating the fkin update thread"); + update_transformations_ = std::thread( + &MultiChainController::updateTransformationsThread, this, ffwd_kin_type, hz); + CNR_DEBUG(this->logger(), "Fkin Thread created"); + + double timeout = 1.0; + ros::Time st = ros::Time::now(); + while(!update_transformations_runnig_) + { + ros::Duration(0.005).sleep(); + if((ros::Time::now()-st).toSec()>timeout) + { + CNR_ERROR(this->m_logger,"The thread that updates the fkin didn't start within the timeout of "<< timeout << ". Abort"); + CNR_RETURN_TRUE(this->m_logger); + } + } + CNR_RETURN_TRUE(this->m_logger); +} + +template +inline void MultiChainController::stopUpdateTransformationsThread() +{ + CNR_TRACE_START(this->m_logger); + if(m_fkin_update_period>0) + { + stop_update_transformations_ = true; + if(update_transformations_.joinable()) + update_transformations_.join(); + } + CNR_RETURN_OK(this->m_logger, void()); +} + +template +inline void MultiChainController::updateTransformationsThread(int ffwd_kin_type, double hz) +{ + CNR_TRACE_START(this->logger()); + ros::Rate rt(hz); + while(!this->stop_update_transformations_) + { + for(auto const & id : this->chainNames() ) + { + auto & rstate = m_rstate.at(id); + auto & rstate_threaded = rstate_threaded_.at(id); + auto & chain_threaded = m_chain_threaded.at(id); + + { + std::lock_guard lock(this->rstate_mtx_); + rstate_threaded.copy(rstate, rosdyn::ChainState::ONLY_JOINT); + } + + rstate_threaded.updateTransformations(chain_threaded, ffwd_kin_type); + + { + std::lock_guard lock(this->rstate_mtx_); + rstate.copy(rstate_threaded, rosdyn::ChainState::ONLY_CART); + } + + if(!this->update_transformations_runnig_) + { + CNR_INFO(this->logger(), "First state update ;)"); + this->update_transformations_runnig_ = true; + } + } + rt.sleep(); + } + CNR_RETURN_OK(this->m_logger, void()); +} + +template +inline const rosdyn::Chain& MultiChainController::chain(const std::string& id) const +{ + std::lock_guard lock(this->m_chain_mtx); + return m_chain.at(id); +} + +template +inline const unsigned int& MultiChainController::nAx(const std::string& id) const +{ + return m_chain.at(id).getActiveJointsNumber(); +} + +template +inline unsigned int MultiChainController::totalNAx() const +{ + unsigned int ret = 0; + for(auto const & id : m_chain_ids ) + { + ret += this->nAx(id); + } + return ret; +} + + +template +inline const std::vector& MultiChainController::chainNames() const +{ + return m_chain_ids; +} + +template +inline const std::vector& MultiChainController::jointNames(const std::string& id) const +{ + std::lock_guard lock(this->m_chain_mtx); + return m_chain.at(id).getActiveJointsName(); +} + +template +inline std::vector MultiChainController::allJointNames() const +{ + std::vector ret; + for(auto const & id : m_chain_ids ) + { + ret.insert(ret.end(), this->jointNames(id).begin(), this->jointNames(id).end()); + } + return ret; +} + +template +inline rosdyn::Chain& MultiChainController::chainNonConst(const std::string& id) +{ + std::lock_guard lock(this->m_chain_mtx); + return m_chain.at(id); +} + +template +inline const rosdyn::ChainState& MultiChainController::chainState(const std::string& id) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id); +} + +template +inline rosdyn::ChainState& MultiChainController::chainState(const std::string& id) +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getPosition(const std::string& id) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).q(); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getVelocity(const std::string& id) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).qd(); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getAcceleration(const std::string& id) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).qdd(); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getEffort(const std::string& id) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).effort(); +} + +template +inline double MultiChainController::getPosition(const std::string& id,int idx) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).q(idx); +} + +template +inline double MultiChainController::getVelocity(const std::string& id,int idx) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).qd(idx); +} + +template +inline double MultiChainController::getAcceleration(const std::string& id,int idx) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).qdd(idx); +} + +template +inline double MultiChainController::getEffort(const std::string& id,int idx) const +{ + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).effort(idx); +} + +template +inline const Eigen::Affine3d& MultiChainController::getToolPose(const std::string& id) const +{ + if(m_fkin_update_period<=0) + throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); + + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).toolPose(); +} + +template +inline const Eigen::Vector6d& MultiChainController::getTwist(const std::string& id) const +{ + if(m_fkin_update_period<=0) + throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); + + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).toolTwist(); +} + +template +inline const Eigen::Vector6d& MultiChainController::getTwistd(const std::string& id) const +{ + if(m_fkin_update_period<=0) + throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); + + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).toolTwistd(); +} + +template +inline const rosdyn::Matrix6Xd& MultiChainController::getJacobian(const std::string& id) const +{ + if(m_fkin_update_period<=0) + throw std::runtime_error("The 'kin_update_period' has not been set, and therefore the fkin is not computed!"); + + std::lock_guard lock(this->rstate_mtx_); + return m_rstate.at(id).toolJacobian(); +} + +template +inline double MultiChainController::getKinUpdatePeriod() const +{ + return m_fkin_update_period; +} + +template +void MultiChainController::setKinUpdatePeriod(const double& fkin_update_period) +{ + m_fkin_update_period = fkin_update_period; +} + + +} // cnr_controller_interface +} // namespace cnr +#endif // CNR_CONTOLLER_INTERFACE__CNR_MULTI_CHAIN_CONTROLLER_INTERFACE_IMPL_H diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp index 2919977..8b2cbc7 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -42,6 +42,7 @@ #include #include #include +#include std::shared_ptr root_nh; std::shared_ptr robot_nh; @@ -52,7 +53,9 @@ std::shared_ptr; using JointCommandController = cnr::control::JointCommandController; +using MultiChainController = cnr::control::MultiChainController; +std::shared_ptr jc_ctrl_cc; std::shared_ptr jc_ctrl_x; std::shared_ptr jc_ctrl_6; std::shared_ptr jc_ctrl_cmd_x; @@ -91,6 +94,12 @@ TEST(TestSuite, GenericControllerConstructor) EXPECT_TRUE(ctrl->init(robot_hw->get(), *robot_nh, *ctrl_nh)); } +TEST(TestSuite, MultiChainControllerXConstructor) +{ + EXPECT_NO_FATAL_FAILURE(jc_ctrl_cc.reset(new MultiChainController())); + EXPECT_TRUE(jc_ctrl_cc->init(robot_hw->get(), *robot_nh, *ctrl_nh)); +} + TEST(TestSuite, JointControllerXConstructor) { EXPECT_NO_FATAL_FAILURE(jc_ctrl_x.reset(new JointController())); @@ -128,6 +137,7 @@ TEST(TestSuite, Desctructor) // Run all the tests that were declared with TEST() int main(int argc, char **argv) { + std::cerr << "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^" << std::endl; testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "cnr_logger_tester"); root_nh .reset(new ros::NodeHandle("/")); @@ -135,5 +145,6 @@ int main(int argc, char **argv) ctrl_nh .reset(new ros::NodeHandle("/ur10_hw/fake_controller")); robot_hw.reset(new cnr_hardware_interface::FakeRobotHW()); robot_hw->init(*root_nh, *robot_nh); + std::cerr << "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^" << std::endl; return RUN_ALL_TESTS(); } diff --git a/cnr_controller_interfaces/cnr_controller_manager_interface/CMakeLists.txt b/cnr_controller_interfaces/cnr_controller_manager_interface/CMakeLists.txt index a06ea4e..32be1e3 100644 --- a/cnr_controller_interfaces/cnr_controller_manager_interface/CMakeLists.txt +++ b/cnr_controller_interfaces/cnr_controller_manager_interface/CMakeLists.txt @@ -9,7 +9,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS controller_interface controller_manager controller_manager_msgs cnr_logger cnr_controller_interface_params - realtime_utilities) + realtime_utilities configuration_msgs) if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) find_package(code_coverage REQUIRED) diff --git a/cnr_controller_interfaces/cnr_controller_manager_interface/package.xml b/cnr_controller_interfaces/cnr_controller_manager_interface/package.xml index 90ee401..28ac66e 100644 --- a/cnr_controller_interfaces/cnr_controller_manager_interface/package.xml +++ b/cnr_controller_interfaces/cnr_controller_manager_interface/package.xml @@ -17,6 +17,7 @@ cnr_logger cnr_controller_interface_params realtime_utilities + configuration_msgs controller_manager_msgs diff --git a/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/cnr_controller_manager_interface.cpp b/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/cnr_controller_manager_interface.cpp index 37af48b..ee41369 100644 --- a/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/cnr_controller_manager_interface.cpp +++ b/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/cnr_controller_manager_interface.cpp @@ -195,7 +195,7 @@ bool ControllerManagerInterface::switchController(const std::vector while(ros::ok()) { error_ = ""; - for (const std::string ctrl_name : start_controllers) + for (const std::string& ctrl_name : start_controllers) { if(!cm_->getControllerByName(ctrl_name)) { @@ -209,7 +209,7 @@ bool ControllerManagerInterface::switchController(const std::vector +"' while 'RUNNING' was expected"; } } - for (const std::string ctrl_name : stop_controllers) + for (const std::string& ctrl_name : stop_controllers) { if(!cm_->getControllerByName(ctrl_name)) { diff --git a/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/internal/cnr_controller_manager_interface_base.cpp b/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/internal/cnr_controller_manager_interface_base.cpp index 8d567ff..8dd871d 100644 --- a/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/internal/cnr_controller_manager_interface_base.cpp +++ b/cnr_controller_interfaces/cnr_controller_manager_interface/src/cnr_controller_manager_interface/internal/cnr_controller_manager_interface_base.cpp @@ -43,6 +43,8 @@ #include #include + +#include #include #include @@ -180,12 +182,13 @@ bool ControllerManagerInterfaceBase::switchControllers(const std::vector& next_ctrl, + const std::vector& next, const ros::Duration& watchdog) { - CNR_TRACE_START(logger_, "HW: " + getHwName()); + CNR_TRACE_START(logger_, "============================== SWITCH OF CONTROLLERS (" + getHwName() + ") =================================="); try { + //============================================================================================ std::vector< controller_manager_msgs::ControllerState > ctrl_running; std::vector< controller_manager_msgs::ControllerState > ctrl_stopped; @@ -194,64 +197,113 @@ bool ControllerManagerInterfaceBase::switchControllers(const int strictness, CNR_RETURN_FALSE(logger_, "HW: " + getHwName() + " Getting the controller info failed: " + error_); } - std::vector ctrl_running_names = cnr::control::ctrl_get_names(ctrl_running); - std::vector ctrl_stopped_names = cnr::control::ctrl_get_names(ctrl_stopped); - std::vector ctrl_loaded_names = ctrl_running_names; - ctrl_loaded_names.insert(ctrl_loaded_names.end(), ctrl_stopped_names.begin(), ctrl_stopped_names.end()); - std::vector to_load_and_start_names; - - std::vector ctrl_to_stop_and_restart_names; - std::vector to_stop_unload_names; - std::vector to_restart_names; - std::vector ctrl_to_unload_names; - - std::vector ctrl_next_names = next_ctrl; - - extract(ctrl_next_names, ctrl_running_names, &to_load_and_start_names, &to_stop_unload_names, &ctrl_to_stop_and_restart_names); - //-- - CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(ctrl_next_names, ", Next Controllers: ")); - CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(ctrl_running_names, ", Running Controllers: ")); - CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(ctrl_stopped_names, ", Stopped Controllers: ")); - CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(ctrl_loaded_names, ", Loaded Controllers: ")); - CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(to_stop_unload_names, ", To Stop and Unload: ")); - CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(ctrl_to_stop_and_restart_names, ", To Stop and Restart: ")); - - std::vector to_load_and_start_names_; - extract(to_load_and_start_names, ctrl_stopped_names, &to_load_and_start_names_, &ctrl_to_unload_names, &to_restart_names); - to_load_and_start_names = to_load_and_start_names_; - - to_restart_names.insert(to_restart_names.begin(), ctrl_to_stop_and_restart_names.begin(), ctrl_to_stop_and_restart_names.end()); - ctrl_to_unload_names .insert(ctrl_to_unload_names.begin(), to_stop_unload_names.begin(), to_stop_unload_names.end()); - - //-- - CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(to_load_and_start_names, ", To load and Start: ")); - CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(ctrl_to_unload_names, ", To Unload Controllers: ")); - CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(to_restart_names, ", To Restart Controllers ")); - - if(to_load_and_start_names.size()>0) + std::vector running = cnr::control::ctrl_get_names(ctrl_running); + std::vector stopped = cnr::control::ctrl_get_names(ctrl_stopped); + std::vector loaded = running; + loaded.insert(loaded.end(), stopped.begin(), stopped.end()); + + CNR_DEBUG(logger_, "HW: " + getHwName() + "== Status and GOAL ============================ "); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(next, ", Next Controllers: ")); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(running, ", Running Controllers: ")); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(stopped, ", Stopped Controllers: ")); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(loaded, ", Loaded Controllers: ")); + //============================================================================================ + + + //============================================================================================ + std::vector to_load_and_start; + std::vector stopped_to_unload; + std::vector to_stop_and_unload; + std::vector already_running; + + extract(next, loaded, &to_load_and_start, nullptr, nullptr); + extract(next, stopped, nullptr, &stopped_to_unload, nullptr); + extract(running, next, &to_stop_and_unload, nullptr, nullptr); + extract(next, running, nullptr, nullptr, &already_running); + + CNR_DEBUG(logger_, "HW: " + getHwName() + "== Grouped ============================ "); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(to_load_and_start, ", To Load and Start: ")); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(stopped_to_unload, ", Stopped To Unload: ")); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(to_stop_and_unload, ", To Stop and Unload: ")); + CNR_DEBUG(logger_, "HW: " + getHwName() + to_string(already_running, ", Already Running: ")); + //============================================================================================ + + std::vector to_stop_and_restart; + if(strictness==configuration_msgs::StartConfigurationRequest::FORCE_RESTART) + { + extract(already_running, next, nullptr, nullptr, &to_stop_and_restart); + } + else + { + to_stop_and_restart.clear(); + } + + std::vector to_load; // onlty the ones that must be loaded and then started + to_load.insert(to_load.end(), to_load_and_start.begin(),to_load_and_start.end()); + + std::vector to_start; // concat the ones to load and start, and the ones to stop and restart + to_start.insert(to_start.end(), to_load_and_start.begin(),to_load_and_start.end()); + to_start.insert(to_start.end(), to_stop_and_restart.begin(),to_stop_and_restart.end()); + + std::vector to_stop; // concat the ones to load and start, and the ones to stop and restart + to_stop.insert(to_stop.end(), to_stop_and_unload.begin(),to_stop_and_unload.end()); + to_stop.insert(to_stop.end(), to_stop_and_restart.begin(),to_stop_and_restart.end()); + + std::vector to_unload; + to_unload.insert(to_unload.end(), to_stop_and_unload.begin(),to_stop_and_unload.end()); + to_unload.insert(to_unload.end(), stopped_to_unload.begin(),stopped_to_unload.end()); + + // std::vector to_load_and_start_; + // extract(to_load_and_start, stopped, &to_load_and_start_, &to_unload, &to_restart); + // to_load_and_start = to_load_and_start_; + + // to_restart.insert(to_restart.begin(), to_stop_and_restart.begin(),to_stop_and_restart.end()); + // to_unload .insert(to_unload.begin(), to_stop_unload.begin(), to_stop_unload.end()); + + CNR_DEBUG(logger_, "HW: " + getHwName() + "== ACTIONS ============================ "); + CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(to_load, ", To Load : ")); + CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(to_start, ", To Start : ")); + CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(to_stop, ", To Stop : ")); + CNR_DEBUG(logger_, "HW: "+getHwName() +to_string(to_unload, ", To Unload: ")); + + if(to_load.size()>0) { CNR_DEBUG(logger_, "LOAD CONTROLLERS...."); - if (!loadControllers(to_load_and_start_names, watchdog)) + if (!loadControllers(to_load, watchdog)) { CNR_RETURN_FALSE(logger_, "Loading the controllers for HW '" + getHwName() + "' failed. Error: " + error()); } } + // Stop controller that must be restarted .. a click is lost! - if(to_load_and_start_names.size()>0 || to_restart_names.size()>0 || to_stop_unload_names.size()>0 ) + int ros_control_strictness = + strictness!=configuration_msgs::StartConfigurationRequest::FORCE_RESTART ? controller_manager_msgs::SwitchControllerRequest::STRICT : + strictness!=configuration_msgs::StartConfigurationRequest::STRICT ? controller_manager_msgs::SwitchControllerRequest::STRICT : + controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT; + + if(to_stop_and_restart.size()>0) + { + CNR_DEBUG(logger_, "STOP THE CONTROLLERS THAT MUST BE RESTARTED (a tick will be lost)...."); + if (!switchController({}, to_stop_and_restart, ros_control_strictness, watchdog)) + { + CNR_RETURN_FALSE(logger_, "Switching the controllers '" + getHwName() + "'failed. Error: " + error()); + } + } + + // start controller and restart if needed. Stop controller not needed anymore + if(to_start.size()>0 || to_stop_and_unload.size()>0 ) { CNR_DEBUG(logger_, "SWITCH CONTROLLERS...."); - to_load_and_start_names.insert(to_load_and_start_names.end(), std::make_move_iterator(to_restart_names.begin()), - std::make_move_iterator(to_restart_names.end())); - if (!switchController(to_load_and_start_names, to_stop_unload_names, strictness, watchdog)) + if (!switchController(to_start, to_stop_and_unload, ros_control_strictness, watchdog)) { CNR_RETURN_FALSE(logger_, "Switching the controllers '" + getHwName() + "'failed. Error: " + error()); } } - if(ctrl_to_unload_names.size()>0) + if(to_unload.size()>0) { CNR_DEBUG(logger_, "UNLOAD CONTROLLERS...."); - if (!unloadControllers(ctrl_to_unload_names, watchdog)) + if (!unloadControllers(to_unload, watchdog)) { CNR_RETURN_FALSE(logger_, "Unload the controllers '" + getHwName() + "'failed. Error: " + error()); } @@ -265,10 +317,10 @@ bool ControllerManagerInterfaceBase::switchControllers(const int strictness, { CNR_RETURN_FALSE(logger_, "Unhandled Exception in switch controllers. "); } - CNR_RETURN_TRUE(logger_, "HW: " + getHwName()); + CNR_RETURN_TRUE(logger_, "============================== SWITCH OF CONTROLLERS (" + getHwName() + ") END =============================="); } -bool ControllerManagerInterfaceBase::unloadControllers(const std::vector& ctrl_to_unload_names, +bool ControllerManagerInterfaceBase::unloadControllers(const std::vector& to_unload, const ros::Duration& watchdog) { CNR_TRACE_START(logger_, "HW: " + getHwName()); @@ -286,7 +338,7 @@ bool ControllerManagerInterfaceBase::unloadControllers(const std::vector to_be_stopped; for (const auto r : rr) { - if (std::find(ctrl_to_unload_names.begin(), ctrl_to_unload_names.end(), r) != ctrl_to_unload_names.end()) + if (std::find(to_unload.begin(), to_unload.end(), r) != to_unload.end()) { to_be_stopped.push_back(r); } @@ -301,7 +353,7 @@ bool ControllerManagerInterfaceBase::unloadControllers(const std::vector& ctrl_to_unload_names, +bool ControllerManagerInterfaceBase::unloadControllers(const std::vector& to_unload, const ros::Duration& watchdog) { CNR_TRACE_START(logger_, "HW: " + getHwName()); - std::vector _ctrl_to_unload_names(ctrl_to_unload_names.size()) ; - std::transform(ctrl_to_unload_names.begin(), ctrl_to_unload_names.end(), _ctrl_to_unload_names.begin(), [](auto v) + std::vector _to_unload(to_unload.size()) ; + std::transform(to_unload.begin(), to_unload.end(), _to_unload.begin(), [](auto v) { return v.name; }); - bool ret = unloadControllers(_ctrl_to_unload_names, watchdog); + bool ret = unloadControllers(_to_unload, watchdog); CNR_RETURN_BOOL(logger_, ret, "HW: " + getHwName()); } -bool ControllerManagerInterfaceBase::stopUnloadControllers(const std::vector& to_stop_unload_names, +bool ControllerManagerInterfaceBase::stopUnloadControllers(const std::vector& to_stop_unload, const ros::Duration& watchdog) { static const std::vector vs_empty; CNR_TRACE_START(logger_, "HW: " + getHwName()); - if (!switchController(vs_empty, to_stop_unload_names, 1, watchdog)) + if (!switchController(vs_empty, to_stop_unload, 1, watchdog)) { CNR_RETURN_FALSE(logger_, "HW: " + getHwName()); } - bool ret = unloadControllers(to_stop_unload_names, watchdog); + bool ret = unloadControllers(to_stop_unload, watchdog); CNR_RETURN_BOOL(logger_, ret, "HW: " + getHwName()); } diff --git a/cnr_hardware_driver_interface/CMakeLists.txt b/cnr_hardware_driver_interface/CMakeLists.txt index 49df106..fe4977e 100644 --- a/cnr_hardware_driver_interface/CMakeLists.txt +++ b/cnr_hardware_driver_interface/CMakeLists.txt @@ -1,16 +1,13 @@ cmake_minimum_required(VERSION 3.1) project(cnr_hardware_driver_interface) -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Debug) -endif() +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/cnrConfigMacros.cmake) + +cnr_set_flags() -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) execute_process(COMMAND uname -r OUTPUT_VARIABLE UNAME_RESULT OUTPUT_STRIP_TRAILING_WHITESPACE) -message(-- " Kernel version: " ${UNAME_RESULT}) +message(STATUS " Kernel version: " ${UNAME_RESULT}) if(UNAME_RESULT MATCHES "^([^rt]*)rt") set(PREEMPTIVE_RT ON CACHE BOOL "Compile library for preemptive machines") else() @@ -47,8 +44,8 @@ include_directories(include ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library add_library(${PROJECT_NAME} src/${PROJECT_NAME}/cnr_hardware_driver_interface.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ) -target_compile_options(${PROJECT_NAME} PUBLIC -Wall -faligned-new - $<$:-Ofast -funroll-loops -ffast-math >) +cnr_target_compile_options(${PROJECT_NAME}) + if(PREEMPTIVE_RT) target_compile_definitions(${PROJECT_NAME} PUBLIC -DPREEMPTIVE_RT=1) else() @@ -100,6 +97,7 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(${PROJECT_NAME}_test test/test.launch test/test.cpp) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} ${catkin_LIBRARIES} ${roscpp_LIBRARIES} ) + cnr_target_compile_options(${PROJECT_NAME}_test) if(ENABLE_COVERAGE_TESTING) set(COVERAGE_EXCLUDES "*/test*") diff --git a/cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake b/cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake new file mode 100644 index 0000000..66f88fd --- /dev/null +++ b/cnr_hardware_driver_interface/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 $<$:-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 $<$:-Ofast -funroll-loops -ffast-math >) + +elseif(CMAKE_CXX_COMPILER_ID MATCHES "MSVC") + + target_compile_options(${TARGET_NAME} PRIVATE /W3 ) + + endif() +endmacro() diff --git a/cnr_hardware_driver_interface/include/cnr_hardware_driver_interface/cnr_hardware_driver_interface.h b/cnr_hardware_driver_interface/include/cnr_hardware_driver_interface/cnr_hardware_driver_interface.h index 304b6c5..9943c53 100644 --- a/cnr_hardware_driver_interface/include/cnr_hardware_driver_interface/cnr_hardware_driver_interface.h +++ b/cnr_hardware_driver_interface/include/cnr_hardware_driver_interface/cnr_hardware_driver_interface.h @@ -77,7 +77,7 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface RobotHwDriverInterface() = default; virtual ~RobotHwDriverInterface(); - bool init(const std::string& hw_name, + bool init(const std::string& hw_name, const std::map& remappings); /** @brief Control Loop implementing a infinite loop [HW read()-> CM update() -> HW write()] @@ -93,17 +93,17 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface * @param[out] true/false if ok or some errors arisen. */ bool start(const ros::Duration& watchdog = ros::Duration(1.0) ); - + /** @brief Stop the control loop implemented in 'run()' if running in a separate thread - * + * * @param[in] watchdog * @param[out] true/false if ok or some errors arisen. */ bool stop(const ros::Duration& watchdog = ros::Duration(1.0) ); /** @brief get the state of the the Driver - * - * The state of the driver is the same state of the RobotHW if the + * + * The state of the driver is the same state of the RobotHW if the * loaded class is inherited from cnr_hardware_interface::RobotHW */ const cnr_hardware_interface::StatusHw& getState() const @@ -112,19 +112,19 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface } /** @brief get the state of the the RobotHW - * - * The state of the RobotHW is the state of the driver if the + * + * The state of the RobotHW is the state of the driver if the * loaded class is inherited from cnr_hardware_interface::RobotHW */ const cnr_hardware_interface::StatusHw& retriveState() { - m_state = m_cnr_hw ? m_cnr_hw->getState() : m_state; + m_state = m_cnr_hw ? m_cnr_hw->getState() : m_state; return m_state; } - + RobotHWConstPtr getRobotHw() const { return m_hw; } CnrRobotHWConstPtr getCnrRobotHw() const { return m_cnr_hw; } - + ControllerManagerPtr getControllerManager() { return m_cm; @@ -142,11 +142,12 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface { return "/" + hw_name + "/status/status"; } - + bool stopUnloadAllControllers(const ros::Duration& watchdog); bool loadAndStartControllers(const std::vector& next_controllers, - const size_t& strictness, const ros::Duration& watchdog); + const size_t& strictness, const ros::Duration& watchdog, + const std::string& configuration_name); protected: bool dumpState(const cnr_hardware_interface::StatusHw& status); @@ -164,7 +165,7 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface RobotLoaderPtr m_robot_hw_loader; ControllerManagerPtr m_cm; cnr_controller_manager_interface::ControllerManagerInterfacePtr m_cmi; - + mutable std::mutex m_mtx; cnr_hardware_interface::StatusHw m_state; std::vector m_state_history; diff --git a/cnr_hardware_driver_interface/src/cnr_hardware_driver_interface/cnr_hardware_driver_interface.cpp b/cnr_hardware_driver_interface/src/cnr_hardware_driver_interface/cnr_hardware_driver_interface.cpp index b0086fb..02f84dd 100644 --- a/cnr_hardware_driver_interface/src/cnr_hardware_driver_interface/cnr_hardware_driver_interface.cpp +++ b/cnr_hardware_driver_interface/src/cnr_hardware_driver_interface/cnr_hardware_driver_interface.cpp @@ -193,6 +193,9 @@ RobotHwDriverInterface::~RobotHwDriverInterface() CNR_FATAL(m_logger, "Error in shuttind down the RobotHw: "); } CNR_TRACE(m_logger, "[ DONE] Shutted Down"); + + CNR_DEBUG(m_logger, "\n\n"+ cnr_logger::BOLDMAGENTA()+ "============= " + m_hw_name + + " : driver desctructor ==========" + cnr_logger::RESET() + "\n\n"); } bool RobotHwDriverInterface::init(const std::string& hw_name, const std::map& remappings) @@ -207,15 +210,17 @@ bool RobotHwDriverInterface::init(const std::string& hw_name, const std::mapinit("NL_" + m_hw_name, m_hw_namespace)) + std::string what; + if( !m_logger->init("NL_" + m_hw_name, m_hw_namespace, false, true, &what)) { std::cerr << __PRETTY_FUNCTION__ << ":" << __LINE__ <<": error in creating the logger" << std::endl; + std::cerr << what << std::endl; return false; } - + CNR_DEBUG(m_logger, "\n\n"+ cnr_logger::BOLDMAGENTA()+ "============= " + m_hw_name + + " : driver initialization ==========" + cnr_logger::RESET() + "\n\n"); CNR_TRACE_START(m_logger); double sampling_period = 0.001; - std::string what; if (!rosparam_utilities::get(m_hw_nh.getNamespace() +"/sampling_period", sampling_period,what,&sampling_period)) { CNR_WARN(m_logger, m_hw_namespace + "/sampling_period' does not exist, set equal to 0.001"); @@ -460,7 +465,7 @@ void RobotHwDriverInterface::run() struct periodic_info info; make_periodic( (m_period.toNSec() / 1000 ), &info); #elif defined(USE_WALLRATE) - ros::WallRate wr( m_period ); + ros::Rate wr( m_period ); #endif m_stop_run = false; @@ -672,11 +677,13 @@ bool RobotHwDriverInterface::stopUnloadAllControllers(const ros::Duration& watch bool RobotHwDriverInterface::loadAndStartControllers(const std::vector& next_controllers, - const size_t& strictness, const ros::Duration& watchdog) + const size_t& strictness, const ros::Duration& watchdog,const std::string& configuration_name) { CNR_TRACE_START(m_logger); try { + CNR_DEBUG(m_logger, "\n\n"+ cnr_logger::BOLDMAGENTA()+ "============= CONFIGURATION: " + configuration_name + ", HW: " + m_hw_name + + ", load a new set of controllers (strictness: "+ std::to_string(strictness) +") ==========" + cnr_logger::RESET() + "\n\n"); if(!m_cmi->switchControllers(strictness, next_controllers, watchdog)) { CNR_ERROR(m_logger, m_hw_name << " Error in loading and starting the controllers:" diff --git a/cnr_hardware_interfaces/cnr_fake_hardware_interface/include/cnr_fake_hardware_interface/cnr_fake_robot_hw.h b/cnr_hardware_interfaces/cnr_fake_hardware_interface/include/cnr_fake_hardware_interface/cnr_fake_robot_hw.h index a0b16a6..9fdb45d 100644 --- a/cnr_hardware_interfaces/cnr_fake_hardware_interface/include/cnr_fake_hardware_interface/cnr_fake_robot_hw.h +++ b/cnr_hardware_interfaces/cnr_fake_hardware_interface/include/cnr_fake_hardware_interface/cnr_fake_robot_hw.h @@ -57,12 +57,12 @@ class FakeRobotHW: public cnr_hardware_interface::RobotHW FakeRobotHW(); ~FakeRobotHW(); - virtual bool doInit(); - virtual bool doWrite(const ros::Time& time, const ros::Duration& period); + virtual bool doInit() override; + virtual bool doWrite(const ros::Time& time, const ros::Duration& period) override; virtual bool doPrepareSwitch(const std::list< hardware_interface::ControllerInfo >& start, - const std::list< hardware_interface::ControllerInfo >& stop); - virtual bool doCheckForConflict(const std::list& info); + const std::list< hardware_interface::ControllerInfo >& stop) override; + virtual bool doCheckForConflict(const std::list& info) const override; protected: sensor_msgs::JointState* m_msg; diff --git a/cnr_hardware_interfaces/cnr_fake_hardware_interface/src/cnr_fake_hardware_interface/cnr_fake_robot_hw.cpp b/cnr_hardware_interfaces/cnr_fake_hardware_interface/src/cnr_fake_hardware_interface/cnr_fake_robot_hw.cpp index 0ac096a..78c9732 100644 --- a/cnr_hardware_interfaces/cnr_fake_hardware_interface/src/cnr_fake_hardware_interface/cnr_fake_robot_hw.cpp +++ b/cnr_hardware_interfaces/cnr_fake_hardware_interface/src/cnr_fake_hardware_interface/cnr_fake_robot_hw.cpp @@ -314,7 +314,7 @@ bool FakeRobotHW::doPrepareSwitch(const std::list< hardware_interface::Controlle } -bool FakeRobotHW::doCheckForConflict(const std::list< hardware_interface::ControllerInfo >& info) +bool FakeRobotHW::doCheckForConflict(const std::list< hardware_interface::ControllerInfo >& info) const { std::stringstream report; CNR_TRACE_START(m_logger); @@ -340,7 +340,6 @@ bool FakeRobotHW::doCheckForConflict(const std::list< hardware_interface::Contro { if (global_joint_used.at(iJ)) // if already used by another { - addDiagnosticsMessage("ERROR", "Joint " + name + " is already used by another controller", {{"Transition", "switching"}}, &report); CNR_ERROR(m_logger, report.str()); CNR_RETURN_TRUE(m_logger, "Joint " + name + " is already used by another controller"); } diff --git a/cnr_hardware_interfaces/cnr_topic_hardware_interface/src/cnr_topic_hardware_interface/cnr_topic_robot_hw.cpp b/cnr_hardware_interfaces/cnr_topic_hardware_interface/src/cnr_topic_hardware_interface/cnr_topic_robot_hw.cpp index 919ae9a..7a172b6 100644 --- a/cnr_hardware_interfaces/cnr_topic_hardware_interface/src/cnr_topic_hardware_interface/cnr_topic_robot_hw.cpp +++ b/cnr_hardware_interfaces/cnr_topic_hardware_interface/src/cnr_topic_hardware_interface/cnr_topic_robot_hw.cpp @@ -113,6 +113,7 @@ bool TopicRobotHW::doInit() m_topic_received = false; m_first_topic_received = false; + m_missing_messages = 0; m_js_sub = m_robothw_nh.subscribe(read_js_topic, 1, diff --git a/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/claimed_resources.h b/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/claimed_resources.h index 99b3c19..edbd5db 100644 --- a/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/claimed_resources.h +++ b/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/claimed_resources.h @@ -99,14 +99,14 @@ struct ClaimedResource , ros::NodeHandle& robothw_nh , std::map< std::string, bool> & topics_received); - ~ClaimedResource(); + virtual ~ClaimedResource(); virtual void init(); virtual void setParam(const std::string& ns); virtual void write(const ros::Time& time, const ros::Duration& period); virtual void callback(const typename MSG::ConstPtr& msg, const std::string& topic); virtual void shutdown(); - bool checkForConflict(const std::list< hardware_interface::ControllerInfo >& info); + bool checkForConflict(const std::list< hardware_interface::ControllerInfo >& info) const; const std::string m_namespace; std::map< std::string, bool>& m_topics_received; diff --git a/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/cnr_topics_robot_hw.h b/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/cnr_topics_robot_hw.h index 205e6c1..e550117 100644 --- a/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/cnr_topics_robot_hw.h +++ b/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/cnr_topics_robot_hw.h @@ -54,14 +54,14 @@ class TopicsRobotHW: public cnr_hardware_interface::RobotHW if (!m_shutted_down) shutdown(); } - virtual bool doShutdown(); - virtual bool doRead(const ros::Time& time, const ros::Duration& period); - virtual bool doWrite(const ros::Time& time, const ros::Duration& period); + virtual bool doShutdown() override; + virtual bool doRead(const ros::Time& time, const ros::Duration& period) override; + virtual bool doWrite(const ros::Time& time, const ros::Duration& period) override; - virtual bool doInit() ; - virtual bool doCheckForConflict(const std::list& info); + virtual bool doInit() override; + virtual bool doCheckForConflict(const std::list& info) const override; virtual bool doPrepareSwitch(const std::list& start_list, - const std::list& stop_list); + const std::list& stop_list) override; protected: @@ -94,14 +94,14 @@ class TopicsRobotHW: public cnr_hardware_interface::RobotHW bool allSubscriberConnected() const { bool all_topics_received = true; - for (const std::pair& topic_received : m_topics_subscribed) all_topics_received &= topic_received.second; + for (auto const & topic_received : m_topics_subscribed) all_topics_received &= topic_received.second; return all_topics_received; } bool topicsReceived() const { bool all_topics_received = true; - for (const std::pair& topic_received : m_topics_subscribed) all_topics_received &= topic_received.second; + for (auto const & topic_received : m_topics_subscribed) all_topics_received &= topic_received.second; return all_topics_received; } void resetTopicsReceived() diff --git a/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/internal/claimed_resources_impl.h b/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/internal/claimed_resources_impl.h index feaa5c0..bceb9a1 100644 --- a/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/internal/claimed_resources_impl.h +++ b/cnr_hardware_interfaces/cnr_topics_hardware_interface/include/cnr_topics_hardware_interface/internal/claimed_resources_impl.h @@ -132,7 +132,7 @@ void ClaimedResource::shutdown() } template < typename MSG > -bool ClaimedResource::checkForConflict(const std::list< hardware_interface::ControllerInfo >& info) +bool ClaimedResource::checkForConflict(const std::list< hardware_interface::ControllerInfo >& info) const { std::vector global_joint_used(m_resource_names.size()); std::fill(global_joint_used.begin(), global_joint_used.end(), false); diff --git a/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/claimed_resources.cpp b/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/claimed_resources.cpp index 6197b58..6c0d1a5 100644 --- a/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/claimed_resources.cpp +++ b/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/claimed_resources.cpp @@ -262,9 +262,11 @@ void AnalogClaimedResource::callback(const std_msgs::Float64MultiArray::ConstPtr return; } - if (!name_sorting::permutationName(m_resource_names_map.at(topic), names, values, "ANALOG CLAIMED RESOURCE - TOPICS HW INTERFACE")) + std::stringstream report; + if (!name_sorting::permutationName(m_resource_names_map.at(topic), names, values, &report)) { m_topics_received[topic] = false; + ROS_WARN_THROTTLE(0.1, "[%s] Error", report.str().c_str()); ROS_WARN_THROTTLE(0.1, "[%s] feedback joint states names are wrong!", m_namespace.c_str()); return; } diff --git a/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/cnr_topics_robot_hw.cpp b/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/cnr_topics_robot_hw.cpp index d88b538..dec3eae 100644 --- a/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/cnr_topics_robot_hw.cpp +++ b/cnr_hardware_interfaces/cnr_topics_hardware_interface/src/cnr_topics_hardware_interface/cnr_topics_robot_hw.cpp @@ -51,7 +51,7 @@ PLUGINLIB_EXPORT_CLASS(cnr_hardware_interface::TopicsRobotHW, hardware_interface #define WARNING( MSG )\ - CNR_DEBUG(m_logger, "[ "+ m_robothw_nh.getNamespace() + " ] RobotHW Nodelet: " + std::string( MSG ) ); + CNR_DEBUG(m_logger, "[ "+ m_robothw_nh.getNamespace() + " ] RobotHW: " + std::string( MSG ) ); static size_t line = __LINE__; #define __LL__ line = __LINE__; @@ -360,7 +360,7 @@ bool TopicsRobotHW::doRead(const ros::Time& time, const ros::Duration& period) if (getState() == cnr_hardware_interface::RUNNING) { std::string all_topics = "["; - for (const std::pair& topic_received : m_topics_subscribed) + for (auto const & topic_received : m_topics_subscribed) { if(!topic_received.second) all_topics += topic_received.first + " "; @@ -439,8 +439,11 @@ bool TopicsRobotHW::doPrepareSwitch(const std::list< hardware_interface::Control } - -bool TopicsRobotHW::doCheckForConflict(const std::list< hardware_interface::ControllerInfo >& info) +/** + * + * + */ +bool TopicsRobotHW::doCheckForConflict(const std::list< hardware_interface::ControllerInfo >& info) const #define CHECK_RESOURCE( RES, res_var )\ if( m_resources.count( RES ) )\ {\ @@ -448,10 +451,7 @@ if( m_resources.count( RES ) )\ if( res_var->checkForConflict(info) )\ {\ std::stringstream report;\ - addDiagnosticsMessage("ERROR",\ - "The resource '" + std::string( #RES ) + "' is in conflict with another controller",\ - {{"Transition","switching"}},\ - &report );\ + report << "[" << this->hardwareId() << "] The resource '" + std::string( #RES ) + "' is in conflict with another controller";\ CNR_ERROR_COND(m_logger, report.str().size(), report.str() );\ }\ } diff --git a/dependencies.rosinstall b/dependencies.rosinstall index 657a2d5..e9294ff 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -37,6 +37,3 @@ - git: local-name: motion/moveit/moveit_planning_helper uri: https://github.com/CNR-STIIMA-IRAS/moveit_planning_helper -- git: - local-name: code_coverage - url: https://github.com/mikeferguson/code_coverage.git \ No newline at end of file diff --git a/test_dependencies.rosinstall b/test_dependencies.rosinstall index 840af57..3984437 100644 --- a/test_dependencies.rosinstall +++ b/test_dependencies.rosinstall @@ -1,4 +1,8 @@ - git: - local-name: code_coverage + local-name: test_depend/code_coverage uri: https://github.com/mikeferguson/code_coverage.git version: master +- git: + local-name: test_depend/universal_robot + uri: https://github.com/fmauch/universal_robot.git + version: calibration_devel