two bufix: mutexes, and strictness management

cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_loader.h

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

cnr_configuration_manager/include/cnr_configuration_manager/cnr_configuration_manager.h

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

cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_loader.cpp

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

cnr_configuration_manager/src/cnr_configuration_manager/cnr_configuration_manager.cpp

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

cnr_configuration_manager/src/dispatcher.cpp

@@ -44,7 +44,7 @@
 #include <cnr_logger/cnr_logger.h>
 
 
-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<std::shared_ptr<Dispatch>> dispatches;
-  std::vector<ros::WallTimer> dispatch_timers;
+  std::vector<ros::Timer> dispatch_timers;
   for (const std::pair<std::string, double>&p : configurations)
   {
     dispatches.push_back(std::make_shared<Dispatch>(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)) ;

cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/cnr_controller_interface.h

@@ -229,7 +229,7 @@ class Controller: public controller_interface::Controller<T>,
 
   std::vector<std::shared_ptr<void>>            m_sub_notifier;
   std::vector<std::shared_ptr<ros::Subscriber>> m_sub;
-  std::vector<ros_helper::WallTimeMTPtr>        m_sub_time;
+  std::vector<ros_helper::TimeMTPtr>            m_sub_time;
   std::vector<bool>                             m_sub_time_track;
 
   bool callAvailable( );

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<H,T>
 
 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;
 

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<T>
 
 protected:
   // Accessors, to be used by the inherited classes
-  const unsigned int& nAx( ) const { return m_chain.getActiveJointsNumber(); }
-  const std::vector<std::string>& jointNames( ) const { return m_chain.getActiveJointsName(); }
+  const unsigned int& nAx( ) const;
+  const std::vector<std::string>& jointNames( ) const;
 
   Handler<H,T>                   m_handler;
   urdf::ModelInterfaceSharedPtr  m_urdf_model;
@@ -123,15 +123,21 @@ class JointController: public cnr::control::Controller<T>
   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;
 };

cnr_controller_interfaces/cnr_controller_interface/include/cnr_controller_interface/internal/cnr_controller_interface_impl.h

@@ -59,7 +59,12 @@ template<class T>
 Controller<T>::~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<T>::prepareInit(T* hw,
 
     l = __LINE__;
     m_logger.reset(new cnr_logger::TraceLogger());
-    if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, ctrl_name, false, false))
+    std::string what;
+    if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, ctrl_name, false, false, &what))
     {
-      if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, hw_name, false, false))
+      if(!m_logger->init(m_hw_name + "-" + m_ctrl_name, hw_name, false, false, &what))
       {
         std::cerr << cnr_logger::RED() << __PRETTY_FUNCTION__ << ":" << __LINE__ << ": " ;
         std::cerr << "The logger cannot be configured:" << std::endl;
         std::cerr << " root ns (the namespace of the hw): " << hw_name << std::endl;
         std::cerr << " controller ns (the namespace of the ctrl): " << ctrl_name << cnr_logger::RST()
                   << std::endl;
+        std::cerr << "what: " << what << std::endl;
         return false;
       }
     }
@@ -196,7 +203,6 @@ bool Controller<T>::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);
@@ -477,8 +483,8 @@ bool Controller<T>::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<m_sub.size(); idx++)
     {
       if(m_sub_time.at(idx) == nullptr)
@@ -490,7 +496,7 @@ bool Controller<T>::exitUpdate()
         if(m_sub_time_track.at(idx))
         {
           m_sub_time.at(idx)->get(last_message_time);
-          ros::WallDuration time_span = (now - last_message_time);
+          ros::Duration time_span = (now - last_message_time);
           if(time_span.toSec() > m_watchdog)
           {
             CNR_ERROR_THROTTLE(m_logger, 5.0, "Watchdog on subscribed topic '" + m_sub.at(idx)->getTopic()+ "' " +
@@ -515,12 +521,12 @@ template<class T>
 bool Controller<T>::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<class T>