From a244434c3c5e8bc26ec03ce2388aa9d1f593395f Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 9 Mar 2022 11:25:21 +0100 Subject: [PATCH 01/32] two bufix: mutexes, and strictness management --- .../cnr_configuration_loader.h | 4 +- .../cnr_configuration_manager.h | 11 +- .../cnr_configuration_loader.cpp | 6 +- .../cnr_configuration_manager.cpp | 8 +- .../cnr_joint_command_controller_interface.h | 3 + .../cnr_joint_controller_interface.h | 14 +- .../internal/cnr_controller_interface_impl.h | 26 +-- .../internal/cnr_handles.h | 89 +++++++--- ..._joint_command_controller_interface_impl.h | 65 +++---- .../cnr_joint_controller_interface_impl.h | 153 ++++++++++++----- .../CMakeLists.txt | 2 +- .../package.xml | 1 + .../cnr_controller_manager_interface_base.cpp | 162 ++++++++++++------ .../cnr_hardware_driver_interface.h | 3 +- .../cnr_hardware_driver_interface.cpp | 15 +- 15 files changed, 372 insertions(+), 190 deletions(-) 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..588fbbc 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 = ""; @@ -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/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..ad9f7c5 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 @@ -117,7 +117,10 @@ class JointCommandController: public cnr::control::JointController private: 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; 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..37c0458 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; @@ -123,15 +123,21 @@ class JointController: public cnr::control::Controller std::thread update_transformations_; bool stop_update_transformations_; bool update_transformations_runnig_; - mutable std::mutex mtx_; + mutable std::mutex m_rstate_mtx; + rosdyn::ChainState m_rstate_threaded; double getKinUpdatePeriod() const { return m_fkin_update_period; } void setKinUpdatePeriod(const double& fkin_update_period) { m_fkin_update_period = fkin_update_period; } private: 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::Chain m_chain_threaded; + 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/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..a798503 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_logger(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_logger(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); @@ -515,12 +521,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 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..ad6871c 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 @@ -42,6 +42,8 @@ struct HandlerBase indexes_ = get_index_map(names,chain); initialized_ = true; } + + virtual void init(const rosdyn::Chain& chain) = 0; }; @@ -50,6 +52,7 @@ struct Handler : public HandlerBase { std::map handles_; + void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } void flush(rosdyn::ChainState& /*ks*/, const rosdyn::Chain& /*chain*/) {} void update(const rosdyn::ChainState& /*ks*/, const rosdyn::Chain& /*chain*/) {} }; @@ -64,9 +67,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!"); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -90,9 +96,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!"); + for(auto const ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -102,9 +111,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!"); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -134,9 +148,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!"); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -167,9 +186,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!"); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -197,10 +221,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!"); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -230,9 +257,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!"); + for(auto const & ax : indexes_) { ks.q(ax.second) = handles_.at(ax.first).getPosition(); @@ -260,9 +291,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() ); @@ -254,7 +262,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 +293,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 +359,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 +368,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,37 +488,26 @@ template inline void JointCommandController::updateTransformationsThread(int ffwd_kin_type, double hz) { CNR_TRACE_START(this->logger()); - rosdyn::ChainState rstate; - rosdyn::ChainState target; - + ros::Rate rt(hz); + while(!this->stop_update_transformations_) { - 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()); + std::lock_guard lock(this->m_rstate_mtx); + this->m_rstate_threaded.copy(this->chainState(), rosdyn::ChainState::ONLY_JOINT); } - if(!target.init(this->chainNonConst())) { - CNR_FATAL(this->m_logger, "Chain failure!"); - CNR_RETURN_NOTOK(this->m_logger, void()); + std::lock_guard lock(this->m_target_mtx); + m_target_threaded.copy(this->m_target, rosdyn::ChainState::ONLY_JOINT); } - } - - ros::Rate rt(hz); - while(!this->stop_update_transformations_) - { + this->m_rstate_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); + m_target_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); { - //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_rstate_mtx); + this->chainState().copy(this->m_rstate_threaded, rosdyn::ChainState::ONLY_CART); } - rstate.updateTransformations(this->chainNonConst(), ffwd_kin_type); - target.updateTransformations(this->chainNonConst(), ffwd_kin_type); { - //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_) { @@ -514,6 +518,7 @@ inline void JointCommandController::updateTransformationsThread(int ffwd_ki } 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..b0494c3 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,106 @@ 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 +579,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 +588,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 +597,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,7 +606,7 @@ 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(); } 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/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..39d116d 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/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..182f1ce 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 @@ -146,7 +146,8 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface 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); 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..18c02a9 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_logger("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"); @@ -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:" From 6a689d6176ca03087355eb95b27e88400a29b842 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 9 Mar 2022 23:01:15 +0100 Subject: [PATCH 02/32] cleanup of the logger mess --- .../internal/cnr_controller_interface_impl.h | 4 ++-- ..._joint_command_controller_interface_impl.h | 20 ++++++++++++++++--- .../cnr_hardware_driver_interface.cpp | 2 +- 3 files changed, 20 insertions(+), 6 deletions(-) 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 a798503..85b2790 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 @@ -183,9 +183,9 @@ bool Controller::prepareInit(T* hw, l = __LINE__; m_logger.reset(new cnr_logger::TraceLogger()); std::string what; - if(!m_logger->init_logger(m_hw_name + "-" + m_ctrl_name, ctrl_name, false, false, &what)) + if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, ctrl_name, false, false, &what)) { - if(!m_logger->init_logger(m_hw_name + "-" + m_ctrl_name, hw_name, false, false, &what)) + 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; diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index 18f26f2..8455e4a 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -194,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"; @@ -204,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) @@ -228,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()); } 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 18c02a9..f019a0f 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 @@ -211,7 +211,7 @@ bool RobotHwDriverInterface::init(const std::string& hw_name, const std::mapinit_logger("NL_" + m_hw_name, m_hw_namespace, false, true, &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; From 2dbe58afe8530b5fdefbf07c461b102a247ac738 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Tue, 15 Mar 2022 10:53:38 +0100 Subject: [PATCH 03/32] gazebo integration --- cnr_configuration_manager/src/dispatcher.cpp | 12 ++++++------ .../internal/cnr_controller_interface_impl.h | 6 +++--- .../cnr_hardware_driver_interface.cpp | 4 ++-- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/cnr_configuration_manager/src/dispatcher.cpp b/cnr_configuration_manager/src/dispatcher.cpp index 86596c3..3d072fe 100644 --- a/cnr_configuration_manager/src/dispatcher.cpp +++ b/cnr_configuration_manager/src/dispatcher.cpp @@ -44,7 +44,7 @@ #include -void gazebo_cb(const ros::WallTimerEvent& ev) +void gazebo_cb(const ros::TimerEvent& ev) { ros::NodeHandle nh; std_srvs::Empty srv; @@ -68,7 +68,7 @@ class Dispatch CNR_INFO(m_logger, "Constructor: default configuration " << conf_srv.request.start_configuration); } - void dispatch(const ros::WallTimerEvent& e) + void dispatch(const ros::TimerEvent& e) { CNR_INFO(m_logger, "Timer Callback: starting configuration " << conf_srv.request.start_configuration); m_start_config.call(conf_srv); @@ -98,13 +98,13 @@ int main(int argc, char **argv) if (!nh.getParam("start_gazebo", start_gazebo)) start_gazebo = false; - ros::WallTimer gazebo_timer; + ros::Timer gazebo_timer; if (start_gazebo) { double gazebo_wait_time = 4; if (!nh.getParam("gazebo_bringup_time", gazebo_wait_time)) gazebo_wait_time = 4; - gazebo_timer = nh.createWallTimer(ros::WallDuration(gazebo_wait_time), gazebo_cb, true); + //gazebo_timer = nh.createWallTimer(ros::Duration(gazebo_wait_time), gazebo_cb, true); CNR_INFO(logger, "Waiting " << gazebo_wait_time << "seconds before unpausing gazebo (timer started: " << gazebo_timer.hasStarted() << ") "); @@ -139,11 +139,11 @@ int main(int argc, char **argv) std::vector> dispatches; - std::vector dispatch_timers; + std::vector dispatch_timers; for (const std::pair&p : configurations) { dispatches.push_back(std::make_shared(p.first, start_config, logger)); - dispatch_timers.push_back(nh.createWallTimer(ros::WallDuration(p.second), + dispatch_timers.push_back(nh.createTimer(ros::Duration(p.second), &Dispatch::dispatch, (dispatches.back()).get(), true)) ; 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 85b2790..2236d5b 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 @@ -483,8 +483,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()+ "' " + 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 f019a0f..915d4af 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 @@ -331,7 +331,7 @@ void RobotHwDriverInterface::diagnosticsThread() updater.add(id + "Error" , m_cmi.get(), &cnr_controller_manager_interface::ControllerManagerInterface::diagnosticsError); updater.add(id + "Timers" , m_cmi.get(), &cnr_controller_manager_interface::ControllerManagerInterface::diagnosticsPerformance); - ros::WallDuration wd(updater.getPeriod()); + ros::Duration wd(updater.getPeriod()); m_diagnostics_thread_running = true; while (ros::ok() && !m_stop_diagnostic_thread) { @@ -465,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; From a1f3b31122d85e351c66cf4dd0794b4662b406fd Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Tue, 15 Mar 2022 15:44:20 +0100 Subject: [PATCH 04/32] gazebo support --- .../include/cnr_controller_interface/cnr_controller_interface.h | 2 +- .../src/cnr_topic_hardware_interface/cnr_topic_robot_hw.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) 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..5226187 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 @@ -229,7 +229,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_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, From 99157986a47ee28f7a85540cb7c5bf84a5c6d571 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Mon, 4 Apr 2022 21:54:08 +0200 Subject: [PATCH 05/32] test dep --- dependencies.rosinstall | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dependencies.rosinstall b/dependencies.rosinstall index 657a2d5..bef458f 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -19,6 +19,7 @@ - git: local-name: rosdyn uri: https://github.com/CNR-STIIMA-IRAS/rosdyn.git + version: nicola-devel - git: local-name: rosdyn_utilities uri: https://github.com/CNR-STIIMA-IRAS/rosdyn_utilities.git @@ -39,4 +40,4 @@ 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 + url: https://github.com/mikeferguson/code_coverage.git From 653ec28a243dc175d0d206b2424bfc6ada35035e Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Mon, 4 Apr 2022 21:54:33 +0200 Subject: [PATCH 06/32] test dep --- dependencies.rosinstall | 1 + 1 file changed, 1 insertion(+) diff --git a/dependencies.rosinstall b/dependencies.rosinstall index bef458f..e6ef191 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -41,3 +41,4 @@ - git: local-name: code_coverage url: https://github.com/mikeferguson/code_coverage.git + From 9485878014717e913949274a3aac0a090a4b32cb Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Mon, 4 Apr 2022 22:09:22 +0200 Subject: [PATCH 07/32] test dep --- .github/workflows/industrial_ci_action.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 8b716b7..401eb96 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: [ master marco-devel ] paths-ignore: - 'docs/**' # When there is a pull request against master - 'README.md' pull_request: - branches: [ master ] + branches: [ master marco-devel ] jobs: industrial_ci: From dee180d0e544c4869b7858a310bcb68eb37fcdd4 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Mon, 4 Apr 2022 22:10:33 +0200 Subject: [PATCH 08/32] test dep --- .github/workflows/industrial_ci_action.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 401eb96..13e3633 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 marco-devel ] + branches: [ marco-devel ] paths-ignore: - 'docs/**' # When there is a pull request against master - 'README.md' pull_request: - branches: [ master marco-devel ] + branches: [ marco-devel ] jobs: industrial_ci: From 19050d6cf2c972fbf16ee55e78742b915b448a36 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Tue, 5 Apr 2022 11:22:35 +0200 Subject: [PATCH 09/32] ci: test dep. --- .github/workflows/industrial_ci_action.yml | 2 +- ci.rosinstall | 49 ++++++++++++++++++++++ dependencies.rosinstall | 4 -- test_dependencies.rosinstall | 6 ++- 4 files changed, 55 insertions(+), 6 deletions(-) create mode 100644 ci.rosinstall diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 13e3633..06282e7 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -24,7 +24,7 @@ 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 steps: diff --git a/ci.rosinstall b/ci.rosinstall new file mode 100644 index 0000000..fe531df --- /dev/null +++ b/ci.rosinstall @@ -0,0 +1,49 @@ +- 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 + version: nicola-devel +- 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/dependencies.rosinstall b/dependencies.rosinstall index e6ef191..655f5c0 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -38,7 +38,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 - diff --git a/test_dependencies.rosinstall b/test_dependencies.rosinstall index 840af57..5c0b43b 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_dependencies/universal_robot + uri: https://github.com/fmauch/universal_robot.git + version: calibration_devel From 3e5ea59619786108cceb18e35780332dc46211e8 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Fri, 6 May 2022 16:55:43 +0200 Subject: [PATCH 10/32] clang errors --- .../cnr_configuration_manager.cpp | 2 +- .../cnr_joint_command_controller_interface.h | 2 +- .../internal/cnr_handles.h | 2 +- .../cnr_controller_manager_interface.cpp | 4 ++-- .../cnr_controller_manager_interface_base.cpp | 2 +- .../cnr_fake_robot_hw.h | 8 ++++---- .../cnr_fake_robot_hw.cpp | 3 +-- .../claimed_resources.h | 4 ++-- .../cnr_topics_robot_hw.h | 16 ++++++++-------- .../internal/claimed_resources_impl.h | 2 +- .../claimed_resources.cpp | 4 +++- .../cnr_topics_robot_hw.cpp | 16 ++++++++-------- 12 files changed, 33 insertions(+), 32 deletions(-) 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 588fbbc..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 @@ -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"); 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 ad9f7c5..bca4f3c 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 @@ -134,7 +134,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/internal/cnr_handles.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_handles.h index ad6871c..0481067 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 @@ -102,7 +102,7 @@ struct Handler 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 39d116d..e669945 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 @@ -44,7 +44,7 @@ #include -#include > +#include #include #include 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_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() );\ }\ } From 03fb7c6566429d892f56b8080010473ec93077e1 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 1 Jun 2022 17:48:13 +0200 Subject: [PATCH 11/32] test --- .../cnr_controller_interface/CMakeLists.txt | 40 ++++++++++--------- .../cnr_controller_interface/test/test.cpp | 10 ++++- 2 files changed, 30 insertions(+), 20 deletions(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt index f5c79cb..43881f6 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt +++ b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS find_package (Eigen3 3.3 REQUIRED NO_MODULE) find_package (Boost COMPONENTS system REQUIRED) +find_package (pinocchio REQUIRED) if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) find_package(code_coverage REQUIRED) @@ -36,33 +37,34 @@ include_directories( ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} ) 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_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_SYSTEM_LIBRARY} ${pinocchio_LIBRARIES} Eigen3::Eigen) target_compile_options(${PROJECT_NAME} PUBLIC -Wall -faligned-new $<$:-Ofast -funroll-loops -ffast-math >) -if(${CMAKE_VERSION} VERSION_GREATER "3.16.0") - target_precompile_headers(${PROJECT_NAME} PUBLIC - - - - - - - - - - - - - - - ) -endif() +# if(${CMAKE_VERSION} VERSION_GREATER "3.16.0") +# target_precompile_headers(${PROJECT_NAME} PUBLIC +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# ) +# endif() ############# diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp index 2919977..e0c8d5b 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -33,9 +33,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ + +#include + + #include -#include + +#include #include + #include #include #include @@ -43,6 +49,8 @@ #include #include +#include + std::shared_ptr root_nh; std::shared_ptr robot_nh; std::shared_ptr ctrl_nh; From fb909c7932157b22ec64ea485193411a2aa4db09 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 1 Jun 2022 17:54:15 +0200 Subject: [PATCH 12/32] test --- .../cnr_controller_interface/CMakeLists.txt | 25 +------------------ .../cnr_controller_interface/test/test.cpp | 2 -- 2 files changed, 1 insertion(+), 26 deletions(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt index 43881f6..23f5475 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt +++ b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS find_package (Eigen3 3.3 REQUIRED NO_MODULE) find_package (Boost COMPONENTS system REQUIRED) -find_package (pinocchio REQUIRED) if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) find_package(code_coverage REQUIRED) @@ -37,36 +36,14 @@ include_directories( ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} ) 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} ${pinocchio_LIBRARIES} Eigen3::Eigen) +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 >) - -# if(${CMAKE_VERSION} VERSION_GREATER "3.16.0") -# target_precompile_headers(${PROJECT_NAME} PUBLIC -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# ) -# endif() - - ############# ## Install ## ############# diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp index e0c8d5b..f9bc832 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -49,8 +49,6 @@ #include #include -#include - std::shared_ptr root_nh; std::shared_ptr robot_nh; std::shared_ptr ctrl_nh; From e2936a0de66979386133c7027e68de8adcabd913 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 1 Jun 2022 17:54:39 +0200 Subject: [PATCH 13/32] test --- .../cnr_controller_interface/test/test.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp index f9bc832..8b76dd0 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -34,9 +34,6 @@ */ -#include - - #include #include From 337bf6338f466cb32b4a39adab7c42008bf1c7e3 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 1 Jun 2022 17:54:58 +0200 Subject: [PATCH 14/32] test --- .../cnr_controller_interface/test/test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp index 8b76dd0..ce3bdeb 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -36,7 +36,7 @@ #include -#include +#include #include #include From a19cb2f264694852fc46c57d13753bf38206945e Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 1 Jun 2022 18:14:45 +0200 Subject: [PATCH 15/32] cross compile windows --- cnr_configuration_manager/CMakeLists.txt | 23 +++----- .../cmake/cnrConfigMacros.cmake | 55 +++++++++++++++++++ cnr_hardware_driver_interface/CMakeLists.txt | 16 +++--- .../cmake/cnrConfigMacros.cmake | 55 +++++++++++++++++++ .../cnr_hardware_driver_interface.h | 3 +- dependencies.rosinstall | 1 - 6 files changed, 127 insertions(+), 26 deletions(-) create mode 100644 cnr_configuration_manager/cmake/cnrConfigMacros.cmake create mode 100644 cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake diff --git a/cnr_configuration_manager/CMakeLists.txt b/cnr_configuration_manager/CMakeLists.txt index 67d907a..7f08920 100644 --- a/cnr_configuration_manager/CMakeLists.txt +++ b/cnr_configuration_manager/CMakeLists.txt @@ -2,16 +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) - -# Debug only, collects stats on how callbacks are doled out to worker threads -#add_definitions(-DNODELET_QUEUE_DEBUG) +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/cnrConfigMacros.cmake) +cnr_set_flags() find_package(catkin REQUIRED COMPONENTS cmake_modules @@ -64,14 +57,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 +91,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..8f6a171 --- /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 + 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/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..8f6a171 --- /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 + 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 182f1ce..304b6c5 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 @@ -146,8 +146,7 @@ class RobotHwDriverInterface : public realtime_utilities::DiagnosticsInterface bool stopUnloadAllControllers(const ros::Duration& watchdog); bool loadAndStartControllers(const std::vector& next_controllers, - const size_t& strictness, const ros::Duration& watchdog, - const std::string& configuration_name); + const size_t& strictness, const ros::Duration& watchdog); protected: bool dumpState(const cnr_hardware_interface::StatusHw& status); diff --git a/dependencies.rosinstall b/dependencies.rosinstall index 655f5c0..e9294ff 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -19,7 +19,6 @@ - git: local-name: rosdyn uri: https://github.com/CNR-STIIMA-IRAS/rosdyn.git - version: nicola-devel - git: local-name: rosdyn_utilities uri: https://github.com/CNR-STIIMA-IRAS/rosdyn_utilities.git From 7ab9a9000d4b6d25c673b7d68802511496d412bb Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 1 Jun 2022 18:18:15 +0200 Subject: [PATCH 16/32] cross compile windows --- .../cnr_hardware_driver_interface.h | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) 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..ceb4bc7 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 @@ -1,3 +1,4 @@ + /* * Software License Agreement (New BSD License) * @@ -77,7 +78,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 +94,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 +113,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 +143,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 +166,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; From 6593a1fc2a55537870037303a77bd4fc8f44f7e5 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 8 Jun 2022 15:50:36 +0200 Subject: [PATCH 17/32] bugfix in handles --- .../cnr_controller_interface/internal/cnr_handles.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 0481067..a930f8c 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 @@ -29,8 +29,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_; @@ -43,6 +44,7 @@ struct HandlerBase initialized_ = true; } +public: virtual void init(const rosdyn::Chain& chain) = 0; }; @@ -53,8 +55,8 @@ struct Handler : public HandlerBase std::map handles_; void init(const rosdyn::Chain& chain) { HandlerBase::init(handles_, chain); } - void flush(rosdyn::ChainState& /*ks*/, const rosdyn::Chain& /*chain*/) {} - void update(const rosdyn::ChainState& /*ks*/, const rosdyn::Chain& /*chain*/) {} + void flush(rosdyn::ChainState& /*ks*/) {} + void update(const rosdyn::ChainState& /*ks*/) {} }; @@ -82,7 +84,7 @@ struct Handler Date: Wed, 8 Jun 2022 15:59:55 +0200 Subject: [PATCH 18/32] bugfix in handles --- .../cnr_controller_interface/CMakeLists.txt | 41 +- .../cmake/cnrConfigMacros.cmake | 55 ++ ...multi_chain_command_controller_interface.h | 144 ++++ .../cnr_multi_chain_controller_interface.h | 145 ++++ ...nr_multi_chain_controller_interface_impl.h | 633 ++++++++++++++++++ .../cnr_controller_interface/test/test.cpp | 8 +- .../test/test.cpp.bak | 142 ++++ 7 files changed, 1157 insertions(+), 11 deletions(-) create mode 100644 cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake create mode 100644 cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h create mode 100644 cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_controller_interface.h create mode 100644 cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_multi_chain_controller_interface_impl.h create mode 100644 cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak diff --git a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt index 23f5475..c604fae 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,8 +37,27 @@ 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 + + + + + + + + + + + + + + + ) +endif() + ############# ## Install ## @@ -79,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*") @@ -89,3 +105,12 @@ if(CATKIN_ENABLE_TESTING) endif() 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} +) \ No newline at end of file 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..ffb4555 --- /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 + 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_multi_chain_command_controller_interface.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h new file mode 100644 index 0000000..01c9f5f --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h @@ -0,0 +1,144 @@ +/* + * 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__JOINT_COMMAND_CONTROLLER_INTERFACE_H +#define CNR_CONTROLLER_INTERFACE__JOINT_COMMAND_CONTROLLER_INTERFACE_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + + +namespace cnr +{ +namespace control +{ + +/** + * + * + * + * Base class to log the controller status + */ +template +class JointCommandController: public cnr::control::JointController +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum InputType {Q_PRIORITY, QD_PRIORITY, NONE}; + + JointCommandController() = default; + virtual ~JointCommandController(); + + 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 enterUpdate() override; + virtual bool exitUpdate() override; + virtual bool exitStopping() override; + +protected: + const rosdyn::ChainState& chainCommand() const; + rosdyn::ChainState& chainCommand(); + + const rosdyn::VectorXd& getCommandPosition ( ) const; + const rosdyn::VectorXd& getCommandVelocity ( ) const; + const rosdyn::VectorXd& getCommandAcceleration( ) const; + const rosdyn::VectorXd& getCommandEffort ( ) const; + + double getCommandPosition (size_t idx) const; + double getCommandVelocity (size_t idx) const; + double getCommandAcceleration(size_t idx) const; + double getCommandEffort (size_t idx) const; + + void setCommandPosition (const rosdyn::VectorXd& in); + void setCommandVelocity (const rosdyn::VectorXd& in); + void setCommandAcceleration (const rosdyn::VectorXd& in); + void setCommandEffort (const rosdyn::VectorXd& in); + + void setCommandPosition (const double& in, size_t idx); + void setCommandVelocity (const double& in, size_t idx); + void setCommandAcceleration (const double& in, size_t idx); + void setCommandEffort (const double& in, size_t idx); + + virtual double getTargetOverride() const; + + void setPriority( const InputType& priority ) { m_priority = priority; } + + mutable std::mutex m_mtx; + +private: + InputType m_priority; + rosdyn::ChainState m_target; + rosdyn::ChainState m_last_target; + rosdyn::ChainStatePublisherPtr m_target_pub; + + + double m_override; + void overrideCallback(const std_msgs::Int64ConstPtr& msg); + + double m_safe_override_1; + double m_safe_override_2; + double m_max_velocity_multiplier; + 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); +}; + +} // namespace control +} // namespace cnr + +#include + +#endif // CNR_CONTROLLER_INTERFACE__JOINT_COMMAND_CONTROLLER_INTERFACE_H + + 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..0bf3bfe --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_controller_interface.h @@ -0,0 +1,145 @@ +/* + * 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 + +#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 { return m_chain.at(id).getActiveJointsNumber(); } + const std::vector& jointNames(const std::string& id) const { return m_chain.at(id).getActiveJointsName(); } + + std::map > m_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 mtx_; + + double getKinUpdatePeriod() const { return m_fkin_update_period; } + void setKinUpdatePeriod(const double& fkin_update_period) { m_fkin_update_period = fkin_update_period; } + +private: + rosdyn::LinkPtr m_root_link; //link primitivo da cui parte la catena cinematica(world ad esempio) + std::map m_chain; + 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_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..8c2998c --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_multi_chain_controller_interface_impl.h @@ -0,0 +1,633 @@ +/* + * 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 +#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 + { + 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 + //======================================= + std::vector chain_ids; + if(!ru::get(this->getControllerNamespace() + "/chain_ids", chain_ids, what ) ) + { + if(!ru::get(this->getRootNamespace() + "/chain_ids", 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()!=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()!=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[chain_ids.at(i)].init(error,m_root_link,base_links.at(i), tool_link.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(auto it = m_chain.begin(); it != m_chain.end(); it++) + { + auto & chain = it->second; + auto ajns = chain.getActiveJointsName(); + CNR_DEBUG(this->m_logger, "Default Chain Active Joint Names: " << cnr::control::to_string(ajns)); + + chain_joint_names[it->first] = std::vector{}; + for(auto const & ajn : ajns) + { + auto jt = std::find(joint_names.begin(), joint_names.end()); + if( jt == joint_names.end()) + { + continue; + } + + if( std::find(joint_names_used.begin(), joint_names_used.end(), jt ) ) + { + CNR_ERROR(this->m_logger, "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[it->first].push_back(ajn); + } + + int ok_coherence_jnames_and_chain = chain.at(id).setInputJointsName(chain_joint_names); + if(ok_coherence_jnames_and_chain!=1) + { + CNR_ERROR(this->m_logger, "Mismatch between the chain names and the controlled joint names. Abort."); + CNR_RETURN_FALSE(this->m_logger); + } + if(chain.getActiveJointsNumber()!=joint_names.size()) + { + CNR_ERROR(this->m_logger, "Mismatch of the dimension of the chain names and the controlled joint names. Abort."); + CNR_RETURN_FALSE(this->m_logger); + } + } + //======================================= + + + //======================================= + // KINEMATICS LIMITS + //======================================= + 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 & c : m_chain ) + { + auto & id = c.first; + auto & chain = c.second; + + 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_rstate[id].init(chain); + + for(unsigned int iAx=0; iAx::m_hw) + { + throw std::runtime_error("The HW is malformed!"); + } + if(m_handlers.find(id)!=m_handlers.end()) + { + CNR_ERROR(this->m_logger, " Error: ultiple handlers with same id"); + CNR_RETURN_FALSE(this->m_logger); + } + m_handlers[id].handles_[chain.getActiveJointName(iAx)] = + Controller::m_hw->getHandle(chain.getActiveJointName(iAx)); + } + catch(std::exception& e) + { + CNR_RETURN_FALSE(this->m_logger, + "Controller '" + Controller::getControllerNamespace() + "' failed in init. " + std::string("") + + "The controlled joint named '"+chain.getActiveJointName(iAx)+"' 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 '"+chain.getActiveJointName(iAx)+"' is not managed by hardware_interface"); + } + CNR_DEBUG(this->m_logger, + "Controller '" + Controller::getControllerNamespace() + std::string("'") + + "The controlled joint named '" + chain.getActiveJointName(iAx) + "' 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); + } + + for(auto & c : m_chain) + { + auto & id = c.first; + auto & chain = c.second; + CNR_DEBUG(this->m_logger, "HW Status\n" << std::to_string(m_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() ); + m_handlers.at(chain.first).flush(m_rstate.at(id), chain); + + 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); + } + + for(auto & c : m_chain) + { + auto & id = c.first; + auto & chain = c.second; + m_handlers.at(id).flush(m_rstate.at(id), chain); + // 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->m_logger); + rosdyn::ChainState rstate; + + { + 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()); + } + } + + ros::Rate rt(hz); + while(!stop_update_transformations_) + { + for(auto & rs : m_rstate) + { + auto & id = rs.first; + auto & rstate = rs.second; + rstate.copy(rstate, rstate.ONLY_JOINT); + rstate.updateTransformations(m_chain.at(id), ffwd_kin_type); + rstate.copy(rstate, rstate.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 +{ + return m_chain.at(id); +} + +template +inline rosdyn::Chain& MultiChainController::chainNonConst(const std::string& id) +{ + return m_chain.at(id); +} + +template +inline const rosdyn::ChainState& MultiChainController::chainState(const std::string& id) const +{ + std::lock_guard lock(this->mtx_); + return m_rstate.at(id); +} + +template +inline rosdyn::ChainState& MultiChainController::chainState(const std::string& id) +{ + std::lock_guard lock(this->mtx_); + return m_rstate.at(id); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getPosition(const std::string& id) const +{ + std::lock_guard lock(this->mtx_); + return m_rstate.at(id).q(); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getVelocity(const std::string& id) const +{ + std::lock_guard lock(this->mtx_); + return m_rstate.at(id).qd(); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getAcceleration(const std::string& id) const +{ + std::lock_guard lock(this->mtx_); + return m_rstate.at(id).qdd(); +} + +template +inline const rosdyn::VectorXd& MultiChainController::getEffort(const std::string& id) const +{ + std::lock_guard lock(this->mtx_); + return m_rstate.at(id).effort(); +} + +template +inline double MultiChainController::getPosition(const std::string& id,int idx) const +{ + std::lock_guard lock(this->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->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->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->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->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->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->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->mtx_); + return m_rstate.at(id).toolJacobian(); +} + + + +} // 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 ce3bdeb..b894cab 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -33,18 +33,16 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #include - #include #include - #include #include #include #include #include #include +#include std::shared_ptr root_nh; std::shared_ptr robot_nh; @@ -55,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; @@ -131,6 +131,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("/")); @@ -138,5 +139,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_interface/test/test.cpp.bak b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak new file mode 100644 index 0000000..ce3bdeb --- /dev/null +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak @@ -0,0 +1,142 @@ +/* + * 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. + */ + + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +std::shared_ptr root_nh; +std::shared_ptr robot_nh; +std::shared_ptr ctrl_nh; +std::shared_ptr robot_hw; +std::shared_ptr > ctrl; + + +using JointController = cnr::control::JointController; +using JointCommandController = cnr::control::JointCommandController; + +std::shared_ptr jc_ctrl_x; +std::shared_ptr jc_ctrl_6; +std::shared_ptr jc_ctrl_cmd_x; +std::shared_ptr jc_ctrl_cmd_6; + +std::string to_string(const std::vector& vv) +{ + std::string ret = "[ "; + for(const auto &v : vv ) ret += v +" "; + return ret + "]"; +} + +//! check the interfaces in the HW +TEST(TestSuite, Handles) +{ + EXPECT_TRUE(robot_hw->get()); + EXPECT_TRUE(robot_hw->get()); + EXPECT_TRUE(robot_hw->get()); + EXPECT_TRUE(robot_hw->get()); + EXPECT_TRUE(robot_hw->get()); + EXPECT_TRUE(robot_hw->get()); + + std::cout << "!JointStateInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; + std::cout << "!PositionJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; + std::cout << "!VelocityJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; + std::cout << "!EffortJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; + std::cout << "!PosVelEffJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; + std::cout << "!VelEffJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; +} + +// Declare a test +TEST(TestSuite, GenericControllerConstructor) +{ + EXPECT_NO_FATAL_FAILURE(ctrl.reset(new cnr::control::Controller())); + //EXPECT_FALSE(ctrl->init(robot_hw->get(), *root_nh, *robot_nh)); + EXPECT_TRUE(ctrl->init(robot_hw->get(), *robot_nh, *ctrl_nh)); +} + +TEST(TestSuite, JointControllerXConstructor) +{ + EXPECT_NO_FATAL_FAILURE(jc_ctrl_x.reset(new JointController())); + //EXPECT_FALSE(jc_ctrl_x->init(robot_hw->get(), *root_nh, *robot_nh)); + EXPECT_TRUE(jc_ctrl_x->init(robot_hw->get(), *robot_nh, *ctrl_nh)); +} + +TEST(TestSuite, JointController6Constructor) +{ + EXPECT_NO_FATAL_FAILURE(jc_ctrl_x.reset(new JointController())); + //EXPECT_FALSE(jc_ctrl_x->init(robot_hw->get(), *root_nh, *robot_nh)); + EXPECT_TRUE(jc_ctrl_x->init(robot_hw->get(), *robot_nh, *ctrl_nh)); +} + +TEST(TestSuite, JointCommandControllerXConstructor) +{ + EXPECT_NO_FATAL_FAILURE(jc_ctrl_cmd_x.reset(new JointCommandController())); + //EXPECT_FALSE(jc_ctrl_x->init(robot_hw->get(), *root_nh, *robot_nh)); + EXPECT_TRUE(jc_ctrl_cmd_x->init(robot_hw->get(), *robot_nh, *ctrl_nh)); +} + +TEST(TestSuite, JointCommandControllerC6Constructor) +{ + EXPECT_NO_FATAL_FAILURE(jc_ctrl_cmd_6.reset(new JointCommandController())); + //EXPECT_FALSE(jc_ctrl_6->init(robot_hw->get(), *root_nh, *robot_nh)); + EXPECT_TRUE(jc_ctrl_cmd_6->init(robot_hw->get(), *robot_nh, *ctrl_nh)); +} + +TEST(TestSuite, Desctructor) +{ + EXPECT_NO_FATAL_FAILURE(ctrl.reset()); + EXPECT_NO_FATAL_FAILURE(robot_hw.reset()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "cnr_logger_tester"); + root_nh .reset(new ros::NodeHandle("/")); + robot_nh .reset(new ros::NodeHandle("/ur10_hw")); + 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); + return RUN_ALL_TESTS(); +} From a3ab3900a400ac9a2adbd27b7079454b7f2e3593 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 8 Jun 2022 16:01:11 +0200 Subject: [PATCH 19/32] bugfix in handles --- .../test/test.cpp.bak | 142 ------------------ 1 file changed, 142 deletions(-) delete mode 100644 cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak deleted file mode 100644 index ce3bdeb..0000000 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp.bak +++ /dev/null @@ -1,142 +0,0 @@ -/* - * 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. - */ - - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -std::shared_ptr root_nh; -std::shared_ptr robot_nh; -std::shared_ptr ctrl_nh; -std::shared_ptr robot_hw; -std::shared_ptr > ctrl; - - -using JointController = cnr::control::JointController; -using JointCommandController = cnr::control::JointCommandController; - -std::shared_ptr jc_ctrl_x; -std::shared_ptr jc_ctrl_6; -std::shared_ptr jc_ctrl_cmd_x; -std::shared_ptr jc_ctrl_cmd_6; - -std::string to_string(const std::vector& vv) -{ - std::string ret = "[ "; - for(const auto &v : vv ) ret += v +" "; - return ret + "]"; -} - -//! check the interfaces in the HW -TEST(TestSuite, Handles) -{ - EXPECT_TRUE(robot_hw->get()); - EXPECT_TRUE(robot_hw->get()); - EXPECT_TRUE(robot_hw->get()); - EXPECT_TRUE(robot_hw->get()); - EXPECT_TRUE(robot_hw->get()); - EXPECT_TRUE(robot_hw->get()); - - std::cout << "!JointStateInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; - std::cout << "!PositionJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; - std::cout << "!VelocityJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; - std::cout << "!EffortJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; - std::cout << "!PosVelEffJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; - std::cout << "!VelEffJointInterface :" << to_string(robot_hw->get()->getNames()) << std::endl; -} - -// Declare a test -TEST(TestSuite, GenericControllerConstructor) -{ - EXPECT_NO_FATAL_FAILURE(ctrl.reset(new cnr::control::Controller())); - //EXPECT_FALSE(ctrl->init(robot_hw->get(), *root_nh, *robot_nh)); - EXPECT_TRUE(ctrl->init(robot_hw->get(), *robot_nh, *ctrl_nh)); -} - -TEST(TestSuite, JointControllerXConstructor) -{ - EXPECT_NO_FATAL_FAILURE(jc_ctrl_x.reset(new JointController())); - //EXPECT_FALSE(jc_ctrl_x->init(robot_hw->get(), *root_nh, *robot_nh)); - EXPECT_TRUE(jc_ctrl_x->init(robot_hw->get(), *robot_nh, *ctrl_nh)); -} - -TEST(TestSuite, JointController6Constructor) -{ - EXPECT_NO_FATAL_FAILURE(jc_ctrl_x.reset(new JointController())); - //EXPECT_FALSE(jc_ctrl_x->init(robot_hw->get(), *root_nh, *robot_nh)); - EXPECT_TRUE(jc_ctrl_x->init(robot_hw->get(), *robot_nh, *ctrl_nh)); -} - -TEST(TestSuite, JointCommandControllerXConstructor) -{ - EXPECT_NO_FATAL_FAILURE(jc_ctrl_cmd_x.reset(new JointCommandController())); - //EXPECT_FALSE(jc_ctrl_x->init(robot_hw->get(), *root_nh, *robot_nh)); - EXPECT_TRUE(jc_ctrl_cmd_x->init(robot_hw->get(), *robot_nh, *ctrl_nh)); -} - -TEST(TestSuite, JointCommandControllerC6Constructor) -{ - EXPECT_NO_FATAL_FAILURE(jc_ctrl_cmd_6.reset(new JointCommandController())); - //EXPECT_FALSE(jc_ctrl_6->init(robot_hw->get(), *root_nh, *robot_nh)); - EXPECT_TRUE(jc_ctrl_cmd_6->init(robot_hw->get(), *robot_nh, *ctrl_nh)); -} - -TEST(TestSuite, Desctructor) -{ - EXPECT_NO_FATAL_FAILURE(ctrl.reset()); - EXPECT_NO_FATAL_FAILURE(robot_hw.reset()); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "cnr_logger_tester"); - root_nh .reset(new ros::NodeHandle("/")); - robot_nh .reset(new ros::NodeHandle("/ur10_hw")); - 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); - return RUN_ALL_TESTS(); -} From 5e255413043f3b155eb60a93a4a97cf3a6d38689 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Mon, 13 Jun 2022 11:22:21 +0200 Subject: [PATCH 20/32] test ok --- .../cmake/cnrConfigMacros.cmake | 2 +- .../cmake/cnrConfigMacros.cmake | 2 +- .../cnr_controller_interface.h | 15 + ...multi_chain_command_controller_interface.h | 144 ---------- .../cnr_multi_chain_controller_interface.h | 35 ++- .../internal/cnr_controller_interface_impl.h | 2 + .../internal/cnr_handles.h | 2 +- ...nr_multi_chain_controller_interface_impl.h | 260 ++++++++++++------ .../cnr_controller_interface/test/test.cpp | 6 + .../cmake/cnrConfigMacros.cmake | 2 +- 10 files changed, 221 insertions(+), 249 deletions(-) delete mode 100644 cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h diff --git a/cnr_configuration_manager/cmake/cnrConfigMacros.cmake b/cnr_configuration_manager/cmake/cnrConfigMacros.cmake index 8f6a171..66f88fd 100644 --- a/cnr_configuration_manager/cmake/cnrConfigMacros.cmake +++ b/cnr_configuration_manager/cmake/cnrConfigMacros.cmake @@ -38,7 +38,7 @@ 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 + 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") diff --git a/cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake b/cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake index ffb4555..e9a56da 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake +++ b/cnr_controller_interfaces/cnr_controller_interface/cmake/cnrConfigMacros.cmake @@ -38,7 +38,7 @@ 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 + 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") 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 5226187..c53882c 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() diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h deleted file mode 100644 index 01c9f5f..0000000 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_multi_chain_command_controller_interface.h +++ /dev/null @@ -1,144 +0,0 @@ -/* - * 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__JOINT_COMMAND_CONTROLLER_INTERFACE_H -#define CNR_CONTROLLER_INTERFACE__JOINT_COMMAND_CONTROLLER_INTERFACE_H - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - - -namespace cnr -{ -namespace control -{ - -/** - * - * - * - * Base class to log the controller status - */ -template -class JointCommandController: public cnr::control::JointController -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - enum InputType {Q_PRIORITY, QD_PRIORITY, NONE}; - - JointCommandController() = default; - virtual ~JointCommandController(); - - 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 enterUpdate() override; - virtual bool exitUpdate() override; - virtual bool exitStopping() override; - -protected: - const rosdyn::ChainState& chainCommand() const; - rosdyn::ChainState& chainCommand(); - - const rosdyn::VectorXd& getCommandPosition ( ) const; - const rosdyn::VectorXd& getCommandVelocity ( ) const; - const rosdyn::VectorXd& getCommandAcceleration( ) const; - const rosdyn::VectorXd& getCommandEffort ( ) const; - - double getCommandPosition (size_t idx) const; - double getCommandVelocity (size_t idx) const; - double getCommandAcceleration(size_t idx) const; - double getCommandEffort (size_t idx) const; - - void setCommandPosition (const rosdyn::VectorXd& in); - void setCommandVelocity (const rosdyn::VectorXd& in); - void setCommandAcceleration (const rosdyn::VectorXd& in); - void setCommandEffort (const rosdyn::VectorXd& in); - - void setCommandPosition (const double& in, size_t idx); - void setCommandVelocity (const double& in, size_t idx); - void setCommandAcceleration (const double& in, size_t idx); - void setCommandEffort (const double& in, size_t idx); - - virtual double getTargetOverride() const; - - void setPriority( const InputType& priority ) { m_priority = priority; } - - mutable std::mutex m_mtx; - -private: - InputType m_priority; - rosdyn::ChainState m_target; - rosdyn::ChainState m_last_target; - rosdyn::ChainStatePublisherPtr m_target_pub; - - - double m_override; - void overrideCallback(const std_msgs::Int64ConstPtr& msg); - - double m_safe_override_1; - double m_safe_override_2; - double m_max_velocity_multiplier; - 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); -}; - -} // namespace control -} // namespace cnr - -#include - -#endif // CNR_CONTROLLER_INTERFACE__JOINT_COMMAND_CONTROLLER_INTERFACE_H - - 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 index 0bf3bfe..8296ce0 100644 --- 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 @@ -32,8 +32,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once - #ifndef CNR_CONTROLLER_INTERFACE__MULTI_CHAIN_CONTROLLER_INTERFACE_H #define CNR_CONTROLLER_INTERFACE__MULTI_CHAIN_CONTROLLER_INTERFACE_H @@ -76,11 +74,11 @@ class MultiChainController: public cnr::control::Controller 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 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; @@ -89,10 +87,13 @@ class MultiChainController: public cnr::control::Controller protected: // Accessors, to be used by the inherited classes - const unsigned int& nAx(const std::string& id) const { return m_chain.at(id).getActiveJointsNumber(); } - const std::vector& jointNames(const std::string& id) const { return m_chain.at(id).getActiveJointsName(); } + 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 > m_handlers; + std::map > handlers_; urdf::ModelInterfaceSharedPtr m_urdf_model; const rosdyn::Chain& chain(const std::string& id) const; @@ -123,15 +124,23 @@ class MultiChainController: public cnr::control::Controller std::thread update_transformations_; bool stop_update_transformations_; bool update_transformations_runnig_; - mutable std::mutex mtx_; + mutable std::mutex rstate_mtx_; + std::map rstate_threaded_; + - double getKinUpdatePeriod() const { return m_fkin_update_period; } - void setKinUpdatePeriod(const double& fkin_update_period) { m_fkin_update_period = fkin_update_period; } + 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; }; 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 2236d5b..b0bdca7 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 @@ -601,6 +601,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 a930f8c..6ae0278 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 @@ -13,7 +13,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 ∈ } 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 index 8c2998c..80955ea 100644 --- 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 @@ -40,17 +40,16 @@ #include #include -#include #include #include #include -#include -#include #include -#include #include #include +#include +#include + namespace ru = rosparam_utilities; namespace cnr @@ -110,6 +109,11 @@ bool MultiChainController::enterInit() 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()) @@ -146,10 +150,9 @@ bool MultiChainController::enterInit() //======================================= // CHAINS //======================================= - std::vector chain_ids; - if(!ru::get(this->getControllerNamespace() + "/chain_ids", chain_ids, what ) ) + if(!ru::get(this->getControllerNamespace() + "/chain_ids", m_chain_ids, what ) ) { - if(!ru::get(this->getRootNamespace() + "/chain_ids", 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."); @@ -168,7 +171,7 @@ bool MultiChainController::enterInit() "nor '" + this->getRootNamespace() + "/base_links' are not in rosparam server."); } } - if(base_links.size()!=chain_ids.size()) + if(base_links.size()!=m_chain_ids.size()) { CNR_RETURN_FALSE(this->m_logger, "'Mismatch in dimensionf of 'base_links' and 'chain_ids'."); } @@ -182,7 +185,7 @@ bool MultiChainController::enterInit() "nor '" + this->getRootNamespace() + "/tool_links' are not in rosparam server."); } } - if(tool_links.size()!=chain_ids.size()) + if(tool_links.size()!=m_chain_ids.size()) { CNR_RETURN_FALSE(this->m_logger, "'Mismatch in dimensionf of 'tool_links' and 'chain_ids'."); } @@ -216,14 +219,15 @@ bool MultiChainController::enterInit() 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[chain_ids.at(i)].init(error,m_root_link,base_links.at(i), tool_link.at(i))) + 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); @@ -238,42 +242,52 @@ bool MultiChainController::enterInit() 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(auto it = m_chain.begin(); it != m_chain.end(); it++) + for(const auto & id : this->chainNames()) { - auto & chain = it->second; - auto ajns = chain.getActiveJointsName(); - CNR_DEBUG(this->m_logger, "Default Chain Active Joint Names: " << cnr::control::to_string(ajns)); + 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[it->first] = std::vector{}; + chain_joint_names[id] = std::vector{}; for(auto const & ajn : ajns) { - auto jt = std::find(joint_names.begin(), joint_names.end()); + 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 ) ) + if( std::find(joint_names_used.begin(), joint_names_used.end(), jt ) != joint_names_used.end() ) { - CNR_ERROR(this->m_logger, "The joint '"+ *jt "' is an active joint in multiple chain"); + 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[it->first].push_back(ajn); + chain_joint_names[id].push_back(ajn); } - int ok_coherence_jnames_and_chain = chain.at(id).setInputJointsName(chain_joint_names); + 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, "Mismatch between the chain names and the controlled joint names. Abort."); + CNR_ERROR(this->m_logger, "Chain '"<m_logger); } - if(chain.getActiveJointsNumber()!=joint_names.size()) + 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) { - CNR_ERROR(this->m_logger, "Mismatch of the dimension of the chain names and the controlled joint names. Abort."); - CNR_RETURN_FALSE(this->m_logger); + 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); } //======================================= @@ -281,6 +295,7 @@ bool MultiChainController::enterInit() //======================================= // 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 ) ) { @@ -300,10 +315,10 @@ bool MultiChainController::enterInit() } l = __LINE__; - for(auto & c : m_chain ) + for(auto const & id : m_chain_ids ) { - auto & id = c.first; - auto & chain = c.second; + 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) @@ -315,41 +330,49 @@ l = __LINE__; { CNR_WARN(this->m_logger, "Warning in setting the kin limits.: '" + error + "'"); } + m_chain_threaded.at(id).enforceLimitsFromRobotDescriptionParam(robot_description_planning_param, error); - m_rstate[id].init(chain); + { + 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(m_handlers.find(id)!=m_handlers.end()) + if(handlers_.find(id)!=handlers_.end() + && handlers_.at(id).handles_.find(jn) != handlers_.at(id).handles_.end() ) { - CNR_ERROR(this->m_logger, " Error: ultiple handlers with same id"); + CNR_ERROR(this->m_logger, " Error: multiple handlers with same id"); CNR_RETURN_FALSE(this->m_logger); } - m_handlers[id].handles_[chain.getActiveJointName(iAx)] = - Controller::m_hw->getHandle(chain.getActiveJointName(iAx)); + handlers_[id].handles_[jn] = + Controller::m_hw->getHandle(jn); } catch(std::exception& e) { CNR_RETURN_FALSE(this->m_logger, "Controller '" + Controller::getControllerNamespace() + "' failed in init. " + std::string("") - + "The controlled joint named '"+chain.getActiveJointName(iAx)+"' is not managed by hardware_interface." + + "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 '"+chain.getActiveJointName(iAx)+"' is not managed by hardware_interface"); + + "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 '" + chain.getActiveJointName(iAx) + "' is managed by hardware_interface"); + + "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() )); @@ -377,19 +400,21 @@ bool MultiChainController::enterStarting() CNR_RETURN_FALSE(this->m_logger); } - for(auto & c : m_chain) { - auto & id = c.first; - auto & chain = c.second; - CNR_DEBUG(this->m_logger, "HW Status\n" << std::to_string(m_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() ); - m_handlers.at(chain.first).flush(m_rstate.at(id), chain); - - 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()) ); + 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); } @@ -421,13 +446,14 @@ bool MultiChainController::enterUpdate() CNR_RETURN_FALSE(this->m_logger); } - for(auto & c : m_chain) { - auto & id = c.first; - auto & chain = c.second; - m_handlers.at(id).flush(m_rstate.at(id), chain); - // 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 + 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); @@ -474,28 +500,27 @@ inline void MultiChainController::stopUpdateTransformationsThread() template inline void MultiChainController::updateTransformationsThread(int ffwd_kin_type, double hz) { - CNR_TRACE_START(this->m_logger); - rosdyn::ChainState rstate; - - { - 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()); - } - } - + CNR_TRACE_START(this->logger()); ros::Rate rt(hz); - while(!stop_update_transformations_) + while(!this->stop_update_transformations_) { - for(auto & rs : m_rstate) + for(auto const & id : this->chainNames() ) { - auto & id = rs.first; - auto & rstate = rs.second; - rstate.copy(rstate, rstate.ONLY_JOINT); - rstate.updateTransformations(m_chain.at(id), ffwd_kin_type); - rstate.copy(rstate, rstate.ONLY_CART); + 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_) { @@ -511,82 +536,126 @@ inline void MultiChainController::updateTransformationsThread(int ffwd_kin_ 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + 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->mtx_); + std::lock_guard lock(this->rstate_mtx_); return m_rstate.at(id).effort(idx); } @@ -595,7 +664,8 @@ inline const Eigen::Affine3d& MultiChainController::getToolPose(const std:: { 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->rstate_mtx_); return m_rstate.at(id).toolPose(); } @@ -604,7 +674,8 @@ inline const Eigen::Vector6d& MultiChainController::getTwist(const std::str { 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->rstate_mtx_); return m_rstate.at(id).toolTwist(); } @@ -613,7 +684,8 @@ inline const Eigen::Vector6d& MultiChainController::getTwistd(const std::st { 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->rstate_mtx_); return m_rstate.at(id).toolTwistd(); } @@ -622,10 +694,22 @@ inline const rosdyn::Matrix6Xd& MultiChainController::getJacobian(const std { 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->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 diff --git a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp index b894cab..8b2cbc7 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp +++ b/cnr_controller_interfaces/cnr_controller_interface/test/test.cpp @@ -94,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())); diff --git a/cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake b/cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake index 8f6a171..66f88fd 100644 --- a/cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake +++ b/cnr_hardware_driver_interface/cmake/cnrConfigMacros.cmake @@ -38,7 +38,7 @@ 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 + 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") From e9cfceccb4a8abf2af3977f274e9710b4f9a5aee Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Mon, 13 Jun 2022 14:13:53 +0200 Subject: [PATCH 21/32] test multi-chain ok --- .../internal/cnr_controller_interface_impl.h | 41 ++++++++++--------- .../internal/cnr_handles.h | 26 ++++++------ ...nr_multi_chain_controller_interface_impl.h | 7 ++-- 3 files changed, 39 insertions(+), 35 deletions(-) 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 b0bdca7..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 @@ -261,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 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 6ae0278..6e86aa2 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 @@ -73,7 +73,7 @@ struct Handlerm_logger, " Error: multiple handlers with same id"); CNR_RETURN_FALSE(this->m_logger); } - handlers_[id].handles_[jn] = - Controller::m_hw->getHandle(jn); + handlers_[id].handles_[jn] = Controller::m_hw->getHandle(jn); + handlers_[id].init(m_chain.at(id)); } catch(std::exception& e) { @@ -370,6 +370,7 @@ l = __LINE__; "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"); From 5a0008d59cce20f5ae105aeecfbac99b20712e3a Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 15 Jun 2022 17:34:46 +0200 Subject: [PATCH 22/32] bugfix mutex --- .../cnr_controller_interface.h | 2 +- .../cnr_joint_controller_interface.h | 4 +-- ..._joint_command_controller_interface_impl.h | 29 +++++----------- .../cnr_joint_controller_interface_impl.h | 33 +++++++------------ 4 files changed, 23 insertions(+), 45 deletions(-) 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 c53882c..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 @@ -226,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; 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 37c0458..a2182ef 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 @@ -123,8 +123,8 @@ class JointController: public cnr::control::Controller std::thread update_transformations_; bool stop_update_transformations_; bool update_transformations_runnig_; - mutable std::mutex m_rstate_mtx; - rosdyn::ChainState m_rstate_threaded; + mutable std::mutex m_rstate_mtx; + rosdyn::ChainState m_rstate_threaded; double getKinUpdatePeriod() const { return m_fkin_update_period; } void setKinUpdatePeriod(const double& fkin_update_period) { m_fkin_update_period = fkin_update_period; } diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index 8455e4a..2c0dfe6 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -32,8 +32,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once // workaround clang-tidy in qtcreator - #ifndef CNR_CONTOLLER_INTERFACE__CNR_JOINT_COMMAND_CONTROLLER_INTERFACE_IMPL_H #define CNR_CONTOLLER_INTERFACE__CNR_JOINT_COMMAND_CONTROLLER_INTERFACE_IMPL_H @@ -110,6 +108,7 @@ inline bool JointCommandController::enterInit() m_priority = QD_PRIORITY; { + CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); m_target.init(this->chainNonConst()); m_last_target.init(this->chainNonConst()); @@ -160,6 +159,7 @@ inline bool JointCommandController::enterStarting() m_target.q() = this->getPosition(); { + CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); this->m_handler.update(m_target); } @@ -277,6 +277,7 @@ inline bool JointCommandController::exitUpdate() report<< "ef trg: " << TP(m_target.effort()) << "\n"; { + CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); this->m_handler.update(m_target); } @@ -309,6 +310,7 @@ inline bool JointCommandController::exitStopping() eigen_utils::setZero(m_target.qd()); { + CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); this->m_handler.update(m_target); } @@ -372,8 +374,6 @@ 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->m_target_mtx); return m_target; } @@ -382,119 +382,102 @@ 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->m_target_mtx); return m_target; } template inline const rosdyn::VectorXd& JointCommandController::getCommandPosition( ) const { - std::lock_guard lock(m_mtx); return m_target.q(); } template inline const rosdyn::VectorXd& JointCommandController::getCommandVelocity( ) const { - std::lock_guard lock(m_mtx); return m_target.qd(); } template inline const rosdyn::VectorXd& JointCommandController::getCommandAcceleration( ) const { - std::lock_guard lock(m_mtx); return m_target.qdd(); } template inline const rosdyn::VectorXd& JointCommandController::getCommandEffort( ) const { - std::lock_guard lock(m_mtx); return m_target.effort(); } template inline double JointCommandController::getCommandPosition(size_t idx) const { - std::lock_guard lock(m_mtx); return m_target.q(idx); } template inline double JointCommandController::getCommandVelocity(size_t idx) const { - std::lock_guard lock(m_mtx); return m_target.qd(idx); } template inline double JointCommandController::getCommandAcceleration(size_t idx) const { - std::lock_guard lock(m_mtx); return m_target.qdd(idx); } template inline double JointCommandController::getCommandEffort(size_t idx) const { - std::lock_guard lock(m_mtx); return m_target.effort(idx); } template inline void JointCommandController::setCommandPosition(const rosdyn::VectorXd& in) { - std::lock_guard lock(m_mtx); m_target.q() = in; } template inline void JointCommandController::setCommandVelocity(const rosdyn::VectorXd& in) { - std::lock_guard lock(m_mtx); m_target.qd() = in; } template inline void JointCommandController::setCommandAcceleration(const rosdyn::VectorXd& in) { - std::lock_guard lock(m_mtx); m_target.qdd() = in; } template inline void JointCommandController::setCommandEffort(const rosdyn::VectorXd& in) { - std::lock_guard lock(m_mtx); m_target.effort() = in; } template inline void JointCommandController::setCommandPosition(const double& in, size_t idx) { - std::lock_guard lock(m_mtx); m_target.q(idx) = in; } template inline void JointCommandController::setCommandVelocity(const double& in, size_t idx) { - std::lock_guard lock(m_mtx); m_target.qd(idx) = in; } template inline void JointCommandController::setCommandAcceleration(const double& in, size_t idx) { - std::lock_guard lock(m_mtx); m_target.qdd(idx) = in; } template inline void JointCommandController::setCommandEffort(const double& in, size_t idx) { - std::lock_guard lock(m_mtx); m_target.effort(idx) = in; } @@ -506,20 +489,24 @@ inline void JointCommandController::updateTransformationsThread(int ffwd_ki while(!this->stop_update_transformations_) { { + CNR_DEBUG(this->logger(), "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); this->m_rstate_threaded.copy(this->chainState(), rosdyn::ChainState::ONLY_JOINT); } { + CNR_DEBUG(this->logger(), "lock m_target_mtx"); std::lock_guard lock(this->m_target_mtx); m_target_threaded.copy(this->m_target, rosdyn::ChainState::ONLY_JOINT); } this->m_rstate_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); m_target_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); { + CNR_DEBUG(this->logger(), "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); this->chainState().copy(this->m_rstate_threaded, rosdyn::ChainState::ONLY_CART); } { + CNR_DEBUG(this->logger(), "lock m_target_mtx"); std::lock_guard lock(this->m_target_mtx); this->m_target.copy(m_target_threaded, rosdyn::ChainState::ONLY_CART); } 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 b0494c3..c15c25d 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 @@ -267,6 +267,7 @@ bool JointController::enterInit() l = __LINE__; { + CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_rstate.init(m_chain); m_rstate_threaded.init(m_chain); @@ -333,6 +334,7 @@ bool JointController::enterStarting() CNR_DEBUG(this->m_logger, "First joint name: " << m_chain.getActiveJointsName().front() ); CNR_DEBUG(this->m_logger, "Last joint name: " << m_chain.getActiveJointsName().back() ); { + CNR_DEBUG(this->m_logger, "lock rstate mtx"); 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()) ); @@ -355,12 +357,12 @@ bool JointController::exitStarting() { stop_update_transformations_ = false; update_transformations_runnig_ = false; - CNR_DEBUG(this->logger(), "Creating the fkin update thread"); + CNR_DEBUG(this->m_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"); + CNR_DEBUG(this->m_logger, "Fkin Thread created"); double timeout = 1.0; ros::Time st = ros::Time::now(); @@ -394,6 +396,7 @@ bool JointController::enterUpdate() } { + CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_handler.flush(m_rstate); } @@ -411,12 +414,12 @@ inline bool JointController::startUpdateTransformationsThread(int ffwd_kin_ CNR_TRACE_START(this->m_logger); stop_update_transformations_ = false; update_transformations_runnig_ = false; - CNR_DEBUG(this->logger(), "Creating the fkin update thread"); + CNR_DEBUG(this->m_logger, "Creating the fkin update thread"); update_transformations_ = std::thread( &JointController::updateTransformationsThread, this, ffwd_kin_type, hz); - CNR_DEBUG(this->logger(), "Fkin Thread created"); + CNR_DEBUG(this->m_logger, "Fkin Thread created"); double timeout = 1.0; ros::Time st = ros::Time::now(); @@ -448,11 +451,12 @@ inline void JointController::stopUpdateTransformationsThread() template inline void JointController::updateTransformationsThread(int ffwd_kin_type, double hz) { - CNR_TRACE_START(this->logger()); + CNR_TRACE_START(this->m_logger); ros::Rate rt(hz); while(!this->stop_update_transformations_) { { + CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_rstate_threaded.copy(m_rstate, rosdyn::ChainState::ONLY_JOINT); } @@ -460,18 +464,19 @@ inline void JointController::updateTransformationsThread(int ffwd_kin_type, m_rstate_threaded.updateTransformations(m_chain_threaded, ffwd_kin_type); { + CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_rstate.copy(m_rstate_threaded, rosdyn::ChainState::ONLY_CART); } if(!this->update_transformations_runnig_) { - CNR_INFO(this->logger(), "First state update ;)"); + CNR_INFO(this->m_logger, "First state update ;)"); this->update_transformations_runnig_ = true; } rt.sleep(); } - CNR_RETURN_OK(this->logger(), void()); + CNR_RETURN_OK(this->m_logger, void()); } @@ -507,70 +512,60 @@ inline rosdyn::Chain& JointController::chainNonConst() template inline const rosdyn::ChainState& JointController::chainState() const { - std::lock_guard lock(this->m_rstate_mtx); return m_rstate; } template inline rosdyn::ChainState& JointController::chainState() { - std::lock_guard lock(this->m_rstate_mtx); return m_rstate; } template inline const rosdyn::VectorXd& JointController::getPosition( ) const { - 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->m_rstate_mtx); return m_rstate.qd(); } template inline const rosdyn::VectorXd& JointController::getAcceleration( ) const { - 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->m_rstate_mtx); return m_rstate.effort(); } template inline double JointController::getPosition(int idx) const { - 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->m_rstate_mtx); return m_rstate.qd(idx); } template inline double JointController::getAcceleration(int idx) const { - 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->m_rstate_mtx); return m_rstate.effort(idx); } @@ -579,7 +574,6 @@ 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->m_rstate_mtx); return m_rstate.toolPose(); } @@ -588,7 +582,6 @@ 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->m_rstate_mtx); return m_rstate.toolTwist(); } @@ -597,7 +590,6 @@ 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->m_rstate_mtx); return m_rstate.toolTwistd(); } @@ -606,7 +598,6 @@ 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->m_rstate_mtx); return m_rstate.toolJacobian(); } From 6e44c3f9f4b326f4faf874ad66dc8f10dc0179f6 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 15 Jun 2022 21:23:03 +0200 Subject: [PATCH 23/32] bugfix mutex --- ..._joint_command_controller_interface_impl.h | 43 ++++++++++++------- .../cnr_joint_controller_interface_impl.h | 33 ++++++++------ 2 files changed, 48 insertions(+), 28 deletions(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index 2c0dfe6..6cbd523 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -32,6 +32,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#pragma once // workaround clang-tidy in qtcreator + #ifndef CNR_CONTOLLER_INTERFACE__CNR_JOINT_COMMAND_CONTROLLER_INTERFACE_IMPL_H #define CNR_CONTOLLER_INTERFACE__CNR_JOINT_COMMAND_CONTROLLER_INTERFACE_IMPL_H @@ -108,7 +110,6 @@ inline bool JointCommandController::enterInit() m_priority = QD_PRIORITY; { - CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); m_target.init(this->chainNonConst()); m_last_target.init(this->chainNonConst()); @@ -159,7 +160,6 @@ inline bool JointCommandController::enterStarting() m_target.q() = this->getPosition(); { - CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); this->m_handler.update(m_target); } @@ -277,7 +277,6 @@ inline bool JointCommandController::exitUpdate() report<< "ef trg: " << TP(m_target.effort()) << "\n"; { - CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); this->m_handler.update(m_target); } @@ -310,7 +309,6 @@ inline bool JointCommandController::exitStopping() eigen_utils::setZero(m_target.qd()); { - CNR_DEBUG(this->logger(), "lock target mtx"); std::lock_guard lock(this->m_target_mtx); this->m_handler.update(m_target); } @@ -374,6 +372,8 @@ 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->m_target_mtx); return m_target; } @@ -382,102 +382,119 @@ 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->m_target_mtx); return m_target; } template inline const rosdyn::VectorXd& JointCommandController::getCommandPosition( ) const { + std::lock_guard lock(m_mtx); return m_target.q(); } template inline const rosdyn::VectorXd& JointCommandController::getCommandVelocity( ) const { + std::lock_guard lock(m_mtx); return m_target.qd(); } template inline const rosdyn::VectorXd& JointCommandController::getCommandAcceleration( ) const { + std::lock_guard lock(m_mtx); return m_target.qdd(); } template inline const rosdyn::VectorXd& JointCommandController::getCommandEffort( ) const { + std::lock_guard lock(m_mtx); return m_target.effort(); } template inline double JointCommandController::getCommandPosition(size_t idx) const { + std::lock_guard lock(m_mtx); return m_target.q(idx); } template inline double JointCommandController::getCommandVelocity(size_t idx) const { + std::lock_guard lock(m_mtx); return m_target.qd(idx); } template inline double JointCommandController::getCommandAcceleration(size_t idx) const { + std::lock_guard lock(m_mtx); return m_target.qdd(idx); } template inline double JointCommandController::getCommandEffort(size_t idx) const { + std::lock_guard lock(m_mtx); return m_target.effort(idx); } template inline void JointCommandController::setCommandPosition(const rosdyn::VectorXd& in) { + std::lock_guard lock(m_mtx); m_target.q() = in; } template inline void JointCommandController::setCommandVelocity(const rosdyn::VectorXd& in) { + std::lock_guard lock(m_mtx); m_target.qd() = in; } template inline void JointCommandController::setCommandAcceleration(const rosdyn::VectorXd& in) { + std::lock_guard lock(m_mtx); m_target.qdd() = in; } template inline void JointCommandController::setCommandEffort(const rosdyn::VectorXd& in) { + std::lock_guard lock(m_mtx); m_target.effort() = in; } template inline void JointCommandController::setCommandPosition(const double& in, size_t idx) { + std::lock_guard lock(m_mtx); m_target.q(idx) = in; } template inline void JointCommandController::setCommandVelocity(const double& in, size_t idx) { + std::lock_guard lock(m_mtx); m_target.qd(idx) = in; } template inline void JointCommandController::setCommandAcceleration(const double& in, size_t idx) { + std::lock_guard lock(m_mtx); m_target.qdd(idx) = in; } template inline void JointCommandController::setCommandEffort(const double& in, size_t idx) { + std::lock_guard lock(m_mtx); m_target.effort(idx) = in; } @@ -488,28 +505,22 @@ inline void JointCommandController::updateTransformationsThread(int ffwd_ki ros::Rate rt(hz); while(!this->stop_update_transformations_) { + this->m_rstate_threaded.copy(this->chainState(), rosdyn::ChainState::ONLY_JOINT); + { - CNR_DEBUG(this->logger(), "lock rstate mtx"); - std::lock_guard lock(this->m_rstate_mtx); - this->m_rstate_threaded.copy(this->chainState(), rosdyn::ChainState::ONLY_JOINT); - } - { - CNR_DEBUG(this->logger(), "lock m_target_mtx"); std::lock_guard lock(this->m_target_mtx); m_target_threaded.copy(this->m_target, rosdyn::ChainState::ONLY_JOINT); } this->m_rstate_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); m_target_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); + + this->chainState().copy(this->m_rstate_threaded, rosdyn::ChainState::ONLY_CART); + { - CNR_DEBUG(this->logger(), "lock rstate mtx"); - std::lock_guard lock(this->m_rstate_mtx); - this->chainState().copy(this->m_rstate_threaded, rosdyn::ChainState::ONLY_CART); - } - { - CNR_DEBUG(this->logger(), "lock m_target_mtx"); 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 ;)" 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 c15c25d..b0494c3 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 @@ -267,7 +267,6 @@ bool JointController::enterInit() l = __LINE__; { - CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_rstate.init(m_chain); m_rstate_threaded.init(m_chain); @@ -334,7 +333,6 @@ bool JointController::enterStarting() CNR_DEBUG(this->m_logger, "First joint name: " << m_chain.getActiveJointsName().front() ); CNR_DEBUG(this->m_logger, "Last joint name: " << m_chain.getActiveJointsName().back() ); { - CNR_DEBUG(this->m_logger, "lock rstate mtx"); 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()) ); @@ -357,12 +355,12 @@ bool JointController::exitStarting() { stop_update_transformations_ = false; update_transformations_runnig_ = false; - CNR_DEBUG(this->m_logger, "Creating the fkin update thread"); + 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->m_logger, "Fkin Thread created"); + CNR_DEBUG(this->logger(), "Fkin Thread created"); double timeout = 1.0; ros::Time st = ros::Time::now(); @@ -396,7 +394,6 @@ bool JointController::enterUpdate() } { - CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_handler.flush(m_rstate); } @@ -414,12 +411,12 @@ inline bool JointController::startUpdateTransformationsThread(int ffwd_kin_ CNR_TRACE_START(this->m_logger); stop_update_transformations_ = false; update_transformations_runnig_ = false; - CNR_DEBUG(this->m_logger, "Creating the fkin update thread"); + CNR_DEBUG(this->logger(), "Creating the fkin update thread"); update_transformations_ = std::thread( &JointController::updateTransformationsThread, this, ffwd_kin_type, hz); - CNR_DEBUG(this->m_logger, "Fkin Thread created"); + CNR_DEBUG(this->logger(), "Fkin Thread created"); double timeout = 1.0; ros::Time st = ros::Time::now(); @@ -451,12 +448,11 @@ inline void JointController::stopUpdateTransformationsThread() template inline void JointController::updateTransformationsThread(int ffwd_kin_type, double hz) { - CNR_TRACE_START(this->m_logger); + CNR_TRACE_START(this->logger()); ros::Rate rt(hz); while(!this->stop_update_transformations_) { { - CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_rstate_threaded.copy(m_rstate, rosdyn::ChainState::ONLY_JOINT); } @@ -464,19 +460,18 @@ inline void JointController::updateTransformationsThread(int ffwd_kin_type, m_rstate_threaded.updateTransformations(m_chain_threaded, ffwd_kin_type); { - CNR_DEBUG(this->m_logger, "lock rstate mtx"); std::lock_guard lock(this->m_rstate_mtx); m_rstate.copy(m_rstate_threaded, rosdyn::ChainState::ONLY_CART); } if(!this->update_transformations_runnig_) { - CNR_INFO(this->m_logger, "First state update ;)"); + CNR_INFO(this->logger(), "First state update ;)"); this->update_transformations_runnig_ = true; } rt.sleep(); } - CNR_RETURN_OK(this->m_logger, void()); + CNR_RETURN_OK(this->logger(), void()); } @@ -512,60 +507,70 @@ inline rosdyn::Chain& JointController::chainNonConst() template inline const rosdyn::ChainState& JointController::chainState() const { + std::lock_guard lock(this->m_rstate_mtx); return m_rstate; } template inline rosdyn::ChainState& JointController::chainState() { + std::lock_guard lock(this->m_rstate_mtx); return m_rstate; } template inline const rosdyn::VectorXd& JointController::getPosition( ) const { + 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->m_rstate_mtx); return m_rstate.qd(); } template inline const rosdyn::VectorXd& JointController::getAcceleration( ) const { + 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->m_rstate_mtx); return m_rstate.effort(); } template inline double JointController::getPosition(int idx) const { + 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->m_rstate_mtx); return m_rstate.qd(idx); } template inline double JointController::getAcceleration(int idx) const { + 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->m_rstate_mtx); return m_rstate.effort(idx); } @@ -574,6 +579,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->m_rstate_mtx); return m_rstate.toolPose(); } @@ -582,6 +588,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->m_rstate_mtx); return m_rstate.toolTwist(); } @@ -590,6 +597,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->m_rstate_mtx); return m_rstate.toolTwistd(); } @@ -598,6 +606,7 @@ 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->m_rstate_mtx); return m_rstate.toolJacobian(); } From 70b3d607d27a383f12dbeb48ba3e867e5a87648c Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 15 Jun 2022 22:14:29 +0200 Subject: [PATCH 24/32] bugfix mutex --- .../cnr_joint_command_controller_interface.h | 12 +++++++++- .../cnr_joint_controller_interface.h | 22 ++++++++++++++----- ..._joint_command_controller_interface_impl.h | 6 ++--- .../cnr_joint_controller_interface_impl.h | 12 +++++++++- 4 files changed, 41 insertions(+), 11 deletions(-) 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 bca4f3c..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,6 +116,17 @@ 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; @@ -123,7 +134,6 @@ class JointCommandController: public cnr::control::JointController rosdyn::ChainState m_last_target; rosdyn::ChainStatePublisherPtr m_target_pub; - double m_override; void overrideCallback(const std_msgs::Int64ConstPtr& msg); 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 a2182ef..914fd62 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 @@ -98,6 +98,9 @@ class JointController: public cnr::control::Controller const rosdyn::Chain& chain() const; rosdyn::Chain& chainNonConst(); + const rosdyn::Chain& chainThreaded() const; + rosdyn::Chain& chainThreadedNonConst(); + const rosdyn::ChainState& chainState() const; rosdyn::ChainState& chainState(); @@ -116,25 +119,32 @@ 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_; + bool stop_update_transformations_; + mutable std::mutex m_rstate_mtx; rosdyn::ChainState m_rstate_threaded; - - double getKinUpdatePeriod() const { return m_fkin_update_period; } - void setKinUpdatePeriod(const double& fkin_update_period) { m_fkin_update_period = fkin_update_period; } + 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) mutable std::mutex m_chain_mtx; rosdyn::Chain m_chain; - rosdyn::Chain m_chain_threaded; rosdyn::ChainState m_rstate; diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index 6cbd523..fe4fac4 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -511,8 +511,8 @@ inline void JointCommandController::updateTransformationsThread(int ffwd_ki std::lock_guard lock(this->m_target_mtx); m_target_threaded.copy(this->m_target, rosdyn::ChainState::ONLY_JOINT); } - this->m_rstate_threaded.updateTransformations(this->chainNonConst(), ffwd_kin_type); - m_target_threaded.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); @@ -520,7 +520,7 @@ inline void JointCommandController::updateTransformationsThread(int ffwd_ki 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 ;)" 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 b0494c3..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 @@ -504,6 +504,7 @@ inline rosdyn::Chain& JointController::chainNonConst() return m_chain; } + template inline const rosdyn::ChainState& JointController::chainState() const { @@ -610,7 +611,16 @@ inline const rosdyn::Matrix6Xd& JointController::getJacobian( ) const 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 From 4c2d92a568852b09b503d0c3480d937d5d1e2822 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 15 Jun 2022 22:15:47 +0200 Subject: [PATCH 25/32] bugfix mutex --- .../cnr_controller_interface/cnr_joint_controller_interface.h | 3 --- 1 file changed, 3 deletions(-) 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 914fd62..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 @@ -98,9 +98,6 @@ class JointController: public cnr::control::Controller const rosdyn::Chain& chain() const; rosdyn::Chain& chainNonConst(); - const rosdyn::Chain& chainThreaded() const; - rosdyn::Chain& chainThreadedNonConst(); - const rosdyn::ChainState& chainState() const; rosdyn::ChainState& chainState(); From 260f4152f7705f230f279a5464b04008679c6117 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 15 Jun 2022 22:35:44 +0200 Subject: [PATCH 26/32] bugfix mutex --- .../internal/cnr_joint_command_controller_interface_impl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index fe4fac4..69e0bc2 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -523,7 +523,7 @@ inline void JointCommandController::updateTransformationsThread(int ffwd_ki 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; From 655e07b83a2f6acfaf4d48cddc41b609ac664d26 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Thu, 16 Jun 2022 13:11:59 +0200 Subject: [PATCH 27/32] bugfix && ci dependencies bugfix --- ci.rosinstall | 1 - 1 file changed, 1 deletion(-) diff --git a/ci.rosinstall b/ci.rosinstall index fe531df..a7f07bd 100644 --- a/ci.rosinstall +++ b/ci.rosinstall @@ -19,7 +19,6 @@ - git: local-name: rosdyn uri: https://github.com/CNR-STIIMA-IRAS/rosdyn.git - version: nicola-devel - git: local-name: rosdyn_utilities uri: https://github.com/CNR-STIIMA-IRAS/rosdyn_utilities.git From 5834d73340829c5e5f28e6e9c1a0615f20d439aa Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Thu, 7 Jul 2022 10:22:05 +0200 Subject: [PATCH 28/32] check --- .../cnr_controller_interface/CMakeLists.txt | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt index c604fae..143d664 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt +++ b/cnr_controller_interfaces/cnr_controller_interface/CMakeLists.txt @@ -103,14 +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() -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} -) \ No newline at end of file From e218a15df0b414a00510eb2ec0c9ce9ca415557a Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 14 Dec 2022 14:49:17 +0100 Subject: [PATCH 29/32] pre merge with master --- cnr_configuration_manager/CMakeLists.txt | 3 +++ cnr_configuration_manager/src/dispatcher.cpp | 12 ++++++------ .../cnr_controller_manager_interface_base.cpp | 2 +- .../cnr_hardware_driver_interface.h | 1 - .../cnr_hardware_driver_interface.cpp | 2 +- test_dependencies.rosinstall | 2 +- 6 files changed, 12 insertions(+), 10 deletions(-) diff --git a/cnr_configuration_manager/CMakeLists.txt b/cnr_configuration_manager/CMakeLists.txt index 7f08920..9e5e76f 100644 --- a/cnr_configuration_manager/CMakeLists.txt +++ b/cnr_configuration_manager/CMakeLists.txt @@ -5,6 +5,9 @@ project(cnr_configuration_manager) 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) + find_package(catkin REQUIRED COMPONENTS cmake_modules diff --git a/cnr_configuration_manager/src/dispatcher.cpp b/cnr_configuration_manager/src/dispatcher.cpp index 3d072fe..86596c3 100644 --- a/cnr_configuration_manager/src/dispatcher.cpp +++ b/cnr_configuration_manager/src/dispatcher.cpp @@ -44,7 +44,7 @@ #include -void gazebo_cb(const ros::TimerEvent& ev) +void gazebo_cb(const ros::WallTimerEvent& ev) { ros::NodeHandle nh; std_srvs::Empty srv; @@ -68,7 +68,7 @@ class Dispatch CNR_INFO(m_logger, "Constructor: default configuration " << conf_srv.request.start_configuration); } - void dispatch(const ros::TimerEvent& e) + void dispatch(const ros::WallTimerEvent& e) { CNR_INFO(m_logger, "Timer Callback: starting configuration " << conf_srv.request.start_configuration); m_start_config.call(conf_srv); @@ -98,13 +98,13 @@ int main(int argc, char **argv) if (!nh.getParam("start_gazebo", start_gazebo)) start_gazebo = false; - ros::Timer gazebo_timer; + ros::WallTimer gazebo_timer; if (start_gazebo) { double gazebo_wait_time = 4; if (!nh.getParam("gazebo_bringup_time", gazebo_wait_time)) gazebo_wait_time = 4; - //gazebo_timer = nh.createWallTimer(ros::Duration(gazebo_wait_time), gazebo_cb, true); + gazebo_timer = nh.createWallTimer(ros::WallDuration(gazebo_wait_time), gazebo_cb, true); CNR_INFO(logger, "Waiting " << gazebo_wait_time << "seconds before unpausing gazebo (timer started: " << gazebo_timer.hasStarted() << ") "); @@ -139,11 +139,11 @@ int main(int argc, char **argv) std::vector> dispatches; - std::vector dispatch_timers; + std::vector dispatch_timers; for (const std::pair&p : configurations) { dispatches.push_back(std::make_shared(p.first, start_config, logger)); - dispatch_timers.push_back(nh.createTimer(ros::Duration(p.second), + dispatch_timers.push_back(nh.createWallTimer(ros::WallDuration(p.second), &Dispatch::dispatch, (dispatches.back()).get(), true)) ; 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 e669945..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 @@ -208,7 +208,7 @@ bool ControllerManagerInterfaceBase::switchControllers(const int strictness, 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; 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 ceb4bc7..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 @@ -1,4 +1,3 @@ - /* * Software License Agreement (New BSD License) * 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 915d4af..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 @@ -331,7 +331,7 @@ void RobotHwDriverInterface::diagnosticsThread() updater.add(id + "Error" , m_cmi.get(), &cnr_controller_manager_interface::ControllerManagerInterface::diagnosticsError); updater.add(id + "Timers" , m_cmi.get(), &cnr_controller_manager_interface::ControllerManagerInterface::diagnosticsPerformance); - ros::Duration wd(updater.getPeriod()); + ros::WallDuration wd(updater.getPeriod()); m_diagnostics_thread_running = true; while (ros::ok() && !m_stop_diagnostic_thread) { diff --git a/test_dependencies.rosinstall b/test_dependencies.rosinstall index 5c0b43b..3984437 100644 --- a/test_dependencies.rosinstall +++ b/test_dependencies.rosinstall @@ -3,6 +3,6 @@ uri: https://github.com/mikeferguson/code_coverage.git version: master - git: - local-name: test_dependencies/universal_robot + local-name: test_depend/universal_robot uri: https://github.com/fmauch/universal_robot.git version: calibration_devel From 39b5288e0b02c6230242b2d3fe7a65c5c1e483bf Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 14 Dec 2022 15:17:37 +0100 Subject: [PATCH 30/32] pre-merge with master --- .github/workflows/industrial_ci_action.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 06282e7..c6414cd 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -26,9 +26,9 @@ jobs: CCACHE_DIR: "/home/runner/target_ws/.ccache" #/github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) 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 }} From bb71a1f1774dbd4ab40c28613b7ce4a2106bf9a3 Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 14 Jun 2023 00:23:17 +0200 Subject: [PATCH 31/32] wip --- .../internal/cnr_joint_command_controller_interface_impl.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index 69e0bc2..9100038 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include @@ -239,7 +239,7 @@ inline bool JointCommandController::exitUpdate() if (m_priority != NONE) { ll = __LINE__; - if(rosdyn::saturateSpeed(this->chain(), saturated_qd, m_last_target.qd(), m_last_target.q(), + if(rdyn::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; From 294da24afca4cc92ea812acffd83533191ea20ba Mon Sep 17 00:00:00 2001 From: Nicola Pedrocchi Date: Wed, 26 Jul 2023 09:54:43 +0200 Subject: [PATCH 32/32] thanks michele --- .../include/cnr_controller_interface/internal/cnr_handles.h | 1 + .../internal/cnr_joint_command_controller_interface_impl.h | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) 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 6e86aa2..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 diff --git a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h index 9100038..69e0bc2 100644 --- a/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h +++ b/cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_joint_command_controller_interface_impl.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include @@ -239,7 +239,7 @@ inline bool JointCommandController::exitUpdate() if (m_priority != NONE) { ll = __LINE__; - if(rdyn::saturateSpeed(this->chain(), saturated_qd, m_last_target.qd(), m_last_target.q(), + 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;