profile
viewpoint
Victor Lopez v-lopez @pal-robotics Software Engineer @pal-robotics
PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c) {+        in_start_list = true;+        break;+      }+    }++    const bool is_running = is_controller_running(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface+  // https://github.com/ros-controls/ros2_control/issues/152+  controller.c->get_lifecycle_node()->configure();+  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());+  to.emplace_back(controller);++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  return to.back().c;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::get_controller_by_name(+  const std::string & name)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+    if (controller.info.name == name) {+      return controller.c;+    }+  }+  return nullptr;+}++void ControllerManager::manage_switch()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // switch hardware interfaces (if any)+  if (!switch_params_.started) {+    robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);+    switch_params_.started = true;+  }+#endif++  stop_controllers();++  // start controllers once the switch is fully complete+  if (!switch_params_.start_asap) {+    start_controllers();+  } else {+    // start controllers as soon as their required joints are done switching+    start_controllers_asap();+  }+}++void ControllerManager::stop_controllers()+{+  // stop controllers+  for (const auto & request : stop_request_) {+    if (is_controller_running(*request)) {+      const auto new_state = request->get_lifecycle_node()->deactivate();+      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {+        RCLCPP_ERROR(+          get_logger(),+          "After deactivating, controller %s is in state %s, expected Inactive",+          request->get_lifecycle_node()->get_name(),+          new_state.label().c_str());+      }+    }+  }

Done

v-lopez

comment created time in a day

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c) {+        in_start_list = true;+        break;+      }+    }++    const bool is_running = is_controller_running(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface+  // https://github.com/ros-controls/ros2_control/issues/152+  controller.c->get_lifecycle_node()->configure();+  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());+  to.emplace_back(controller);++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  return to.back().c;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::get_controller_by_name(+  const std::string & name)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+    if (controller.info.name == name) {+      return controller.c;+    }+  }+  return nullptr;

Done

v-lopez

comment created time in a day

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c) {+        in_start_list = true;+        break;+      }+    }

Done

v-lopez

comment created time in a day

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the+     * updated list while it's being used+     * @return reference to the updated list+     */+    std::vector<ControllerSpec> & get_used_by_rt_list();++    /**+     * @brief get_unused_list Waits until the "outdated" and "unused by rt"+     *  lists match and returns a reference to it+     * This referenced list can be modified safely until switch_updated_controller_list()+     * is called, at this point the RT thread may start using it at any time+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    std::vector<ControllerSpec> & get_unused_list(+      const std::lock_guard<std::recursive_mutex> & guard);++    /**+     * @brief get_updated_list Returns a const reference to the most updated list,+     * @warning May or may not being used by the realtime thread, read-only reference for safety+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    const std::vector<ControllerSpec> & get_updated_list(+      const std::lock_guard<std::recursive_mutex> & guard) const;++    /**+     * @brief switch_updated_list Switches the "updated" and "outdated" lists, and waits+     *  until the RT thread is using the new "updated" list.+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    void switch_updated_list(const std::lock_guard<std::recursive_mutex> & guard);+++    // Mutex protecting the controllers list+    // must be acquired before using any list other than the "used by rt"+    mutable std::recursive_mutex controllers_lock_;++private:+    int get_other_list(int index) const;

Yeah, it's not obvious how this thing works. I've added a short comment and more documentation to all this class.

If it is still not clear please ask for further stuff, if not feel free to close any conversations you feel are solved.

v-lopez

comment created time in a day

PullRequestReviewEvent

issue commentpal-robotics/talos_robot

Kinematic calibration

Looks good, sorry we couldn't get on this before next week. It's most going to look very similar to this. Very nice idea to use one file per arm to having to deal with side as part of the variable name.

jmirabel

comment created time in a day

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();

I believe I've rewritten and pushed all offending code.

v-lopez

comment created time in 2 days

PullRequestReviewEvent

issue commentpal-robotics/talos_robot

Kinematic calibration

Alright, we had one more discussion about this.

The joint offset we can apply at a lower level, at the motor configuration files of each joint. Therefore on the talos_description_calibration only the XYZ and RPY remains.

These motor calibration files are also accessible to you in case you needed to modify, but ideally after we've calibrated them they should not need to be touched anymore.

jmirabel

comment created time in 2 days

issue commentpal-robotics/talos_robot

Kinematic calibration

Adjusting the xyz and rpy of the joint should be sufficient to handle this issue, shouldn't it ?

No, because the motor is still moving outside it's operating range, at maybe when going over 90º it might be hitting a mechanical limit and not reaching the desired position and therefore overheating.

What about providing 7 parameters (xyz_offset, rpy_offset and transmission_offset) and let the one who calibrates choose the method that suits its need ?

You need both, because they correct different problems. The transmission_offset corrects the motor range of motion. The other compensates for deviations from the manufacturing process.

For a new robot, you need to calibrate xyz_offset and rpy_offset using a precise (but time consuming) method (like MoCap). At this step, you leave the transmission offset to zero. Later on, after some usage of the robot, a transmission offset may appear and one could use another simpler method to estimate those offsets (like finding when the bounds are hit on each side of each joint)

Agree 100%

jmirabel

comment created time in 2 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha b6ea36051cebc152d07ab6343b95e6c9e2d3e4ee

Deduplicate code

view details

push time in 2 days

issue commentpal-robotics/talos_robot

Kinematic calibration

I hadn't thought of that.

The reason for keeping it in the transmission, is that while we don't expect big changes in the roll, pitch and yaw in the physical location of the joint due to assembly, it is expected that the joints might have some offset at the actuator level, some encoder mounting difference or any similar thing. That's why that joint offset was there in the first place.

It's important to differentiate between that external and internal offset. For an external offset we just adjust the x,y,z or r,p,y transform and that's good.

But if the arm is supposed to be moving from 0º to 90º, and it is instead moving from 5º to 95º due to some internal actuator tolerances, we need to correct this 5º offset at the joint level. Otherwise the arm might hit the mechanical limits or cause self collisions.

If we have two separate processes to determine the internal joint offset, and the external mounting offset, then it may make sense to apply all x,y,z and r,p,y at the joint origin transform, and the internal offset at the transmission level.

Please let me know what you think, while we move this internally a bit more.

jmirabel

comment created time in 2 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 0067623a5fde952a9a3995a42a7eb8231142a903

Use find algorithms for finding controllers

view details

push time in 2 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 09671227c7dc5559fbe22cd31a65f506eb938930

Fix remaining camelCase to snake_case

view details

Victor Lopez

commit sha 2805044d5b1bc68f25008d57e607c0ec4d556d03

Update copyright year

view details

Victor Lopez

commit sha ef1187f0bcddcf815aabc68bd2f7b1cc5714d89e

Apply PR suggestions

view details

push time in 2 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 9c05a5e4d1c52cfb7c98b7b58927ca78aa9c8f27

Apply suggestions from code review Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>

view details

push time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {

You are right, fixed.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c) {+        in_start_list = true;+        break;+      }+    }++    const bool is_running = is_controller_running(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface+  // https://github.com/ros-controls/ros2_control/issues/152+  controller.c->get_lifecycle_node()->configure();+  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());+  to.emplace_back(controller);++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  return to.back().c;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::get_controller_by_name(+  const std::string & name)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+    if (controller.info.name == name) {+      return controller.c;+    }+  }+  return nullptr;+}++void ControllerManager::manage_switch()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // switch hardware interfaces (if any)+  if (!switch_params_.started) {+    robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);+    switch_params_.started = true;+  }+#endif++  stop_controllers();++  // start controllers once the switch is fully complete+  if (!switch_params_.start_asap) {+    start_controllers();+  } else {+    // start controllers as soon as their required joints are done switching+    start_controllers_asap();+  }+}++void ControllerManager::stop_controllers()+{+  // stop controllers+  for (const auto & request : stop_request_) {+    if (is_controller_running(*request)) {+      const auto new_state = request->get_lifecycle_node()->deactivate();+      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {+        RCLCPP_ERROR(+          get_logger(),+          "After deactivating, controller %s is in state %s, expected Inactive",+          request->get_lifecycle_node()->get_name(),+          new_state.label().c_str());+      }+    }+  }+}++void ControllerManager::start_controllers()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers+  if (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::DONE) {+    for (const auto & request : start_request_) {+      request->startRequest(time);+    }++    switch_params_.do_switch = false;+  } else if (// NOLINT+    (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::ERROR) ||+    (switch_params_.timeout > 0.0 &&+    (time - switch_params_.init_time).toSec() > switch_params_.timeout))+  {+    // abort controllers in case of error or timeout (if set)+    for (const auto & request : start_request_) {+      request->abortRequest(time);+    }++    switch_params_.do_switch = false;+  } else {+    // wait controllers+    for (const auto & request : start_request_) {+      request->waitRequest(time);+    }+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  for (const auto & request : start_request_) {+    const auto new_state = request->get_lifecycle_node()->activate();+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {+      RCLCPP_ERROR(+        get_logger(),+        "After activating, controller %s is in state %s, expected Active",+        request->get_lifecycle_node()->get_name(),+        new_state.label().c_str());+    }+  }+  // All controllers started, switching done+  switch_params_.do_switch = false;+#endif+}++void ControllerManager::start_controllers_asap()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers if possible+  for (const auto & request : start_request_) {+    if (!isControllerRunning(*request)) {+      // find the info from this controller+      for (const auto & controller : controllers_lists_[current_controllers_list_]) {+        if (request == controller.c.get()) {+          // ready to start+          if (robot_hw_->switchResult(controller.info) ==+            hardware_interface::RobotHW::SwitchState::DONE)+          {+            request->startRequest(time);+          } else if ((robot_hw_->switchResult(controller.info) == // NOLINT+            hardware_interface::RobotHW::SwitchState::ERROR) ||+            (switch_params_.timeout > 0.0 &&+            (time - switch_params_.init_time).toSec() > switch_params_.timeout)) // NOLINT+          {+            // abort on error or timeout (if set)+            request->abortRequest(time);+          } else {+            // controller is waiting+            request->waitRequest(time);+          }+        }+        continue;+      }+    }+  }++  // all needed controllers started or aborted, switch done+  if (std::all_of(+      start_request_.begin(), start_request_.end(),+      [](controller_interface::ControllerBase * request) {+        return request->isRunning() || request->isAborted();+      }))+  {+    switch_params_.do_switch = false;+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  start_controllers();+#endif+}++void ControllerManager::list_controllers_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "list controller service called");+  std::lock_guard<std::mutex> services_guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list controller service locked");++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  response->controller.resize(controllers.size());++  for (size_t i = 0; i < controllers.size(); ++i) {+    controller_manager_msgs::msg::ControllerState & cs = response->controller[i];+    cs.name = controllers[i].info.name;+    cs.type = controllers[i].info.type;+    cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label();++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    cs.claimed_resources.clear();+    typedef std::vector<hardware_interface::InterfaceResources> ClaimedResVec;+    typedef ClaimedResVec::const_iterator ClaimedResIt;+    const ClaimedResVec & c_resources = controllers[i].info.claimed_resources;+    for (const auto & c_resource : c_resources) {+      controller_manager_msgs::HardwareInterfaceResources iface_res;+      iface_res.hardware_interface = c_resource.hardware_interface;+      std::copy(+        c_resource.resources.begin(), c_resource.resources.end(),+        std::back_inserter(iface_res.resources));+      cs.claimed_resources.push_back(iface_res);+    }+#endif+  }++  RCLCPP_DEBUG(get_logger(), "list controller service finished");+}++void ControllerManager::list_controller_types_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response) {-  controller->init(hw_, controller_name);-  executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface());+  // lock services+  RCLCPP_DEBUG(get_logger(), "list types service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list types service locked"); -  loaded_controllers_.emplace_back(controller);-  return loaded_controllers_.back();+  for (const auto & controller_loader : loaders_) {+    std::vector<std::string> cur_types = controller_loader->getDeclaredClasses();+    for (const auto & cur_type : cur_types) {+      response->types.push_back(cur_type);+      response->base_classes.push_back(controller_loader->getName());+      RCLCPP_INFO(get_logger(), cur_type);+    }+  }++  RCLCPP_DEBUG(get_logger(), "list types service finished");+}++void ControllerManager::load_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "loading service called for controller '%s' ", request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "loading service locked");++  response->ok = load_controller(request->name).get();++  RCLCPP_DEBUG(+    get_logger(), "loading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::reload_controller_libraries_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "reload libraries service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "reload libraries service locked");++  // only reload libraries if no controllers are running+  std::vector<std::string> loaded_controllers, running_controllers;+  get_controller_names(loaded_controllers);+  {+    // lock controllers+    std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+    for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+      if (is_controller_running(*controller.c)) {+        running_controllers.push_back(controller.info.name);+      }+    }+  }+  if (!running_controllers.empty() && !request->force_kill) {+    RCLCPP_ERROR(+      get_logger(), "Controller manager: Cannot reload controller libraries because"+      " there are still %i controllers running",+      (int)running_controllers.size());+    response->ok = false;+    return;+  }++  // stop running controllers if requested+  if (!loaded_controllers.empty()) {+    RCLCPP_INFO(get_logger(), "Controller manager: Stopping all running controllers");+    std::vector<std::string> empty;+    if (switch_controller(+        empty, running_controllers,+        controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) !=+      controller_interface::return_type::SUCCESS)+    {+      RCLCPP_ERROR(+        get_logger(),+        "Controller manager: Cannot reload controller libraries because failed to stop "+        "running controllers");+      response->ok = false;+      return;+    }+    for (const auto & controller : loaded_controllers) {+      if (unload_controller(controller) != controller_interface::return_type::SUCCESS) {+        RCLCPP_ERROR(+          get_logger(), "Controller manager: Cannot reload controller libraries because "+          "failed to unload controller '%s'",+          controller.c_str());+        response->ok = false;+        return;+      }+    }+    get_controller_names(loaded_controllers);+  }+  assert(loaded_controllers.empty());++  // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)+  for (const auto & controller_loader : loaders_) {+    controller_loader->reload();+    RCLCPP_INFO(+      get_logger(), "Controller manager: reloaded controller libraries for '%s'",+      controller_loader->getName().c_str());+  }++  response->ok = true;++  RCLCPP_DEBUG(get_logger(), "reload libraries service finished");+}++void ControllerManager::switch_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "switching service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "switching service locked");++  response->ok = switch_controller(+    request->start_controllers, request->stop_controllers, request->strictness,+    request->start_asap, request->timeout) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(get_logger(), "switching service finished");+}++void ControllerManager::unload_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(+    get_logger(), "unloading service called for controller '%s' ",+    request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "unloading service locked");++  response->ok = unload_controller(request->name) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(+    get_logger(), "unloading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::get_controller_names(std::vector<std::string> & names)

Will do.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerLoaderInterface    CONTROLLER_MANAGER_PUBLIC   virtual bool is_available(const std::string & controller_type) const = 0;++  CONTROLLER_MANAGER_PUBLIC+  const std::string & getName() const {return name_;}

Yeah, my bad.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c) {+        in_start_list = true;+        break;+      }+    }++    const bool is_running = is_controller_running(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }

Right, fixing it.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::cleanup() const   return ret; } +std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list()+{+  used_by_realtime_controllers_index_ = updated_controllers_index_;+  return controllers_lists_[used_by_realtime_controllers_index_];+}++std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_unused_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  // Get the index to the outdated controller list+  int free_controllers_list = get_other_list(updated_controllers_index_);++  // Wait until the outdated controller list is not being used by the realtime thread+  wait_until_rt_not_using(free_controllers_list);+  return controllers_lists_[free_controllers_list];+}++const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list(+  const std::lock_guard<std::recursive_mutex> &) const

Please check the comment of the function on the header, it's the only way I found to make sure that whoever is calling this has a lock guard and has locked this class mutex (the line below).

While technically they could give me a random lock_guard, and enter this function without having the mutex locked, it would be with malice, this is intended to prevent accidental usage without having locked the mutex.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::configure() const }  controller_interface::return_type-ControllerManager::activate() const+ControllerManager::activate() {   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_state = loaded_controller->get_lifecycle_node()->activate();-    if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {-      ret = controller_interface::return_type::ERROR;-    }-  }

Please refer to this discussion: https://github.com/ros-controls/ros2_control/pull/139#discussion_r489310064

The situation is that in ROS2 as it is right now, a stopped controller = inactive controller, and started controller = active controller.

We typically have stopped controllers that conflict with active controllers (torque control vs position control), activating all of them at once makes no sense.

If we separated started/stopped from active/inactive we could activate only the started controllers, but also @bmagyar safety concerns come into play.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::cleanup() const   return ret; } +std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list()+{+  used_by_realtime_controllers_index_ = updated_controllers_index_;+  return controllers_lists_[used_by_realtime_controllers_index_];+}++std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_unused_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  // Get the index to the outdated controller list+  int free_controllers_list = get_other_list(updated_controllers_index_);++  // Wait until the outdated controller list is not being used by the realtime thread+  wait_until_rt_not_using(free_controllers_list);+  return controllers_lists_[free_controllers_list];+}++const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list(+  const std::lock_guard<std::recursive_mutex> &) const+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  return controllers_lists_[updated_controllers_index_];+}++void ControllerManager::RTControllerListWrapper::switch_updated_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  int former_current_controllers_list_ = updated_controllers_index_;+  updated_controllers_index_ = get_other_list(former_current_controllers_list_);+  wait_until_rt_not_using(former_current_controllers_list_);+}++int ControllerManager::RTControllerListWrapper::get_other_list(int index) const+{+  return (index + 1) % 2;+}++void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(int index) const+{+  while (used_by_realtime_controllers_index_ == index) {+    if (!rclcpp::ok()) {+      throw std::runtime_error("rclcpp interrupted");+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));

Adding a default parameter.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();

Agreed, again I expected that everyone would be more comfortable with keeping most of the old codebase and it would make the review process smoother.

I'll take a look at these functions and rewrite them.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot+  // declare paramaters with their names.+  // So when we're told to load a controller by name, we need to declare the parameter if+  // we haven't done so, and then read it.++  // Check if parameter has been declared+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (is_controller_running(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  return rt_controllers_wrapper_.get_updated_list(guard); }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller);+    if (!ct.get()) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c) {+        in_start_list = true;+        break;+      }+    }++    const bool is_running = is_controller_running(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);+  const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface+  // https://github.com/ros-controls/ros2_control/issues/152+  controller.c->get_lifecycle_node()->configure();+  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());+  to.emplace_back(controller);++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  rt_controllers_wrapper_.switch_updated_list(guard);+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(+    guard);+  new_unused_list.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  return to.back().c;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::get_controller_by_name(+  const std::string & name)+{+  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);++  for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+    if (controller.info.name == name) {+      return controller.c;+    }+  }+  return nullptr;+}++void ControllerManager::manage_switch()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // switch hardware interfaces (if any)+  if (!switch_params_.started) {+    robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);+    switch_params_.started = true;+  }+#endif++  stop_controllers();++  // start controllers once the switch is fully complete+  if (!switch_params_.start_asap) {+    start_controllers();+  } else {+    // start controllers as soon as their required joints are done switching+    start_controllers_asap();+  }+}++void ControllerManager::stop_controllers()+{+  // stop controllers+  for (const auto & request : stop_request_) {+    if (is_controller_running(*request)) {+      const auto new_state = request->get_lifecycle_node()->deactivate();+      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {+        RCLCPP_ERROR(+          get_logger(),+          "After deactivating, controller %s is in state %s, expected Inactive",+          request->get_lifecycle_node()->get_name(),+          new_state.label().c_str());+      }+    }+  }+}++void ControllerManager::start_controllers()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers+  if (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::DONE) {+    for (const auto & request : start_request_) {+      request->startRequest(time);+    }++    switch_params_.do_switch = false;+  } else if (// NOLINT+    (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::ERROR) ||+    (switch_params_.timeout > 0.0 &&+    (time - switch_params_.init_time).toSec() > switch_params_.timeout))+  {+    // abort controllers in case of error or timeout (if set)+    for (const auto & request : start_request_) {+      request->abortRequest(time);+    }++    switch_params_.do_switch = false;+  } else {+    // wait controllers+    for (const auto & request : start_request_) {+      request->waitRequest(time);+    }+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  for (const auto & request : start_request_) {+    const auto new_state = request->get_lifecycle_node()->activate();+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {+      RCLCPP_ERROR(+        get_logger(),+        "After activating, controller %s is in state %s, expected Active",+        request->get_lifecycle_node()->get_name(),+        new_state.label().c_str());+    }+  }+  // All controllers started, switching done+  switch_params_.do_switch = false;+#endif+}++void ControllerManager::start_controllers_asap()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers if possible+  for (const auto & request : start_request_) {+    if (!isControllerRunning(*request)) {+      // find the info from this controller+      for (const auto & controller : controllers_lists_[current_controllers_list_]) {+        if (request == controller.c.get()) {+          // ready to start+          if (robot_hw_->switchResult(controller.info) ==+            hardware_interface::RobotHW::SwitchState::DONE)+          {+            request->startRequest(time);+          } else if ((robot_hw_->switchResult(controller.info) == // NOLINT+            hardware_interface::RobotHW::SwitchState::ERROR) ||+            (switch_params_.timeout > 0.0 &&+            (time - switch_params_.init_time).toSec() > switch_params_.timeout)) // NOLINT+          {+            // abort on error or timeout (if set)+            request->abortRequest(time);+          } else {+            // controller is waiting+            request->waitRequest(time);+          }+        }+        continue;+      }+    }+  }++  // all needed controllers started or aborted, switch done+  if (std::all_of(+      start_request_.begin(), start_request_.end(),+      [](controller_interface::ControllerBase * request) {+        return request->isRunning() || request->isAborted();+      }))+  {+    switch_params_.do_switch = false;+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  start_controllers();+#endif+}++void ControllerManager::list_controllers_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "list controller service called");+  std::lock_guard<std::mutex> services_guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list controller service locked");++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  const std::vector<ControllerSpec> & controllers =+    rt_controllers_wrapper_.get_updated_list(guard);+  response->controller.resize(controllers.size());++  for (size_t i = 0; i < controllers.size(); ++i) {+    controller_manager_msgs::msg::ControllerState & cs = response->controller[i];+    cs.name = controllers[i].info.name;+    cs.type = controllers[i].info.type;+    cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label();++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    cs.claimed_resources.clear();+    typedef std::vector<hardware_interface::InterfaceResources> ClaimedResVec;+    typedef ClaimedResVec::const_iterator ClaimedResIt;+    const ClaimedResVec & c_resources = controllers[i].info.claimed_resources;+    for (const auto & c_resource : c_resources) {+      controller_manager_msgs::HardwareInterfaceResources iface_res;+      iface_res.hardware_interface = c_resource.hardware_interface;+      std::copy(+        c_resource.resources.begin(), c_resource.resources.end(),+        std::back_inserter(iface_res.resources));+      cs.claimed_resources.push_back(iface_res);+    }+#endif+  }++  RCLCPP_DEBUG(get_logger(), "list controller service finished");+}++void ControllerManager::list_controller_types_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response) {-  controller->init(hw_, controller_name);-  executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface());+  // lock services+  RCLCPP_DEBUG(get_logger(), "list types service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list types service locked"); -  loaded_controllers_.emplace_back(controller);-  return loaded_controllers_.back();+  for (const auto & controller_loader : loaders_) {+    std::vector<std::string> cur_types = controller_loader->getDeclaredClasses();+    for (const auto & cur_type : cur_types) {+      response->types.push_back(cur_type);+      response->base_classes.push_back(controller_loader->getName());+      RCLCPP_INFO(get_logger(), cur_type);+    }+  }++  RCLCPP_DEBUG(get_logger(), "list types service finished");+}++void ControllerManager::load_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "loading service called for controller '%s' ", request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "loading service locked");++  response->ok = load_controller(request->name).get();++  RCLCPP_DEBUG(+    get_logger(), "loading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::reload_controller_libraries_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "reload libraries service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "reload libraries service locked");++  // only reload libraries if no controllers are running+  std::vector<std::string> loaded_controllers, running_controllers;+  get_controller_names(loaded_controllers);+  {+    // lock controllers+    std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+    for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+      if (is_controller_running(*controller.c)) {+        running_controllers.push_back(controller.info.name);+      }+    }+  }+  if (!running_controllers.empty() && !request->force_kill) {+    RCLCPP_ERROR(+      get_logger(), "Controller manager: Cannot reload controller libraries because"+      " there are still %i controllers running",+      (int)running_controllers.size());+    response->ok = false;+    return;+  }++  // stop running controllers if requested+  if (!loaded_controllers.empty()) {+    RCLCPP_INFO(get_logger(), "Controller manager: Stopping all running controllers");+    std::vector<std::string> empty;+    if (switch_controller(+        empty, running_controllers,+        controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) !=+      controller_interface::return_type::SUCCESS)+    {+      RCLCPP_ERROR(+        get_logger(),+        "Controller manager: Cannot reload controller libraries because failed to stop "+        "running controllers");+      response->ok = false;+      return;+    }+    for (const auto & controller : loaded_controllers) {+      if (unload_controller(controller) != controller_interface::return_type::SUCCESS) {+        RCLCPP_ERROR(+          get_logger(), "Controller manager: Cannot reload controller libraries because "+          "failed to unload controller '%s'",+          controller.c_str());+        response->ok = false;+        return;+      }+    }+    get_controller_names(loaded_controllers);+  }+  assert(loaded_controllers.empty());++  // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)+  for (const auto & controller_loader : loaders_) {+    controller_loader->reload();+    RCLCPP_INFO(+      get_logger(), "Controller manager: reloaded controller libraries for '%s'",+      controller_loader->getName().c_str());+  }++  response->ok = true;++  RCLCPP_DEBUG(get_logger(), "reload libraries service finished");+}++void ControllerManager::switch_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "switching service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "switching service locked");++  response->ok = switch_controller(+    request->start_controllers, request->stop_controllers, request->strictness,+    request->start_asap, request->timeout) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(get_logger(), "switching service finished");+}++void ControllerManager::unload_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(+    get_logger(), "unloading service called for controller '%s' ",+    request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "unloading service locked");++  response->ok = unload_controller(request->name) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(+    get_logger(), "unloading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::get_controller_names(std::vector<std::string> & names)+{+  names.clear();++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);+  for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) {+    names.push_back(controller.info.name);+  } }  controller_interface::return_type ControllerManager::update() {+  std::vector<ControllerSpec> & rt_controller_list =+    rt_controllers_wrapper_.update_and_get_used_by_rt_list();+   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_ret = loaded_controller->update();-    if (controller_ret != controller_interface::return_type::SUCCESS) {-      ret = controller_ret;+  for (auto loaded_controller : rt_controller_list) {+    // TODO(v-lopez) we could cache this information+    // https://github.com/ros-controls/ros2_control/issues/153+    if (is_controller_running(*loaded_controller.c)) {+      auto controller_ret = loaded_controller.c->update();+      if (controller_ret != controller_interface::return_type::SUCCESS) {+        ret = controller_ret;+      }     }   } +  // there are controllers to start/stop+  if (switch_params_.do_switch) {+    manage_switch();+  }   return ret; }  controller_interface::return_type-ControllerManager::configure() const+ControllerManager::configure() {   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_state = loaded_controller->get_lifecycle_node()->configure();++  using namespace std::placeholders;+  list_controllers_service_ = create_service<controller_manager_msgs::srv::ListControllers>(+    "list_controllers", std::bind(+      &ControllerManager::list_controllers_srv_cb, this, _1,+      _2));+  list_controller_types_service_ =+    create_service<controller_manager_msgs::srv::ListControllerTypes>(+    "list_controller_types", std::bind(+      &ControllerManager::list_controller_types_srv_cb, this, _1,+      _2));+  load_controller_service_ = create_service<controller_manager_msgs::srv::LoadController>(+    "load_controller", std::bind(+      &ControllerManager::load_controller_service_cb, this, _1,+      _2));+  reload_controller_libraries_service_ =+    create_service<controller_manager_msgs::srv::ReloadControllerLibraries>(+    "reload_controller_libraries", std::bind(+      &ControllerManager::reload_controller_libraries_service_cb, this, _1,+      _2));+  switch_controller_service_ = create_service<controller_manager_msgs::srv::SwitchController>(+    "switch_controller", std::bind(+      &ControllerManager::switch_controller_service_cb, this, _1,+      _2));+  unload_controller_service_ = create_service<controller_manager_msgs::srv::UnloadController>(+    "unload_controller", std::bind(+      &ControllerManager::unload_controller_service_cb, this, _1,+      _2));

So I'll move them to the constructor.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, const std::string & controller_name,+    const std::string & controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the update_and_get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief update_and_get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, no one should modify the+     * updated list while it's being used+     * @return reference to the updated list+     */+    std::vector<ControllerSpec> & update_and_get_used_by_rt_list();++    /**+     * @brief get_unused_list Waits until the "outdated" and "unused by rt"+     *  lists match and returns a reference to it+     * This referenced list can be modified safely until switch_updated_controller_list()+     * is called, at this point the RT thread may start using it at any time+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    std::vector<ControllerSpec> & get_unused_list(+      const std::lock_guard<std::recursive_mutex> & guard);++    /**+     * @brief get_updated_list Returns a const reference to the most updated list,+     * @warning May or may not being used by the realtime thread, read-only reference for safety+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    const std::vector<ControllerSpec> & get_updated_list(+      const std::lock_guard<std::recursive_mutex> & guard) const;++    /**+     * @brief switch_updated_list Switches the "updated" and "outdated" lists, and waits+     *  until the RT thread is using the new "updated" list.+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    void switch_updated_list(const std::lock_guard<std::recursive_mutex> & guard);+++    // Mutex protecting the controllers list+    // must be acquired before using any list other than the "used by rt"+    mutable std::recursive_mutex controllers_lock_;++private:+    /**+     * @brief get_other_list get the list not pointed by index+     */+    int get_other_list(int index) const;++    void wait_until_rt_not_using(int index) const;++    std::vector<ControllerSpec> controllers_lists_[2];+    /// The index of the controller list with the most updated information+    int updated_controllers_index_ = 0;+    /// The index of the controllers list being used in the real-time thread.+    int used_by_realtime_controllers_index_ = -1;

No, they can be pointing to the same index. From the documentation above:

   * There's always an "updated" list and an "outdated" one
   * There's always an "used by rt" list and an "unused by rt" list

What is missing in that documentation is an explicit mention that these "states" can overlap. I will add this

You can have an:

  • updated + used_by_rt (this is forced at the beginning of update())
  • updated + unused_by_rt (after starting/stopping a controller and only until the next update())
  • outdated + used_by_rt (while on update() but starting/stopping a controller on another thread)
  • outdated + unused_by_rt (the "other" list at the beginning of update())

That's why we need two variables to track this. Yes, they could be two booleans, but right now at least there's one usage for the -1 initial value on the rt list, that I guess can probably be reworked.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;++  // A priori we don't know the name of the controllers that will be loaded, so we cannot

What I wanted to express was that we cannot declare in the controller_manager code the name of the parameters of the controllers that will be loaded in the future, because they are plugins and we cannot be aware of all of them.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type;++  return add_controller_impl(controller_spec);+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name)+{+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());

A user via a launch file, or a configuration file.

Similar to this in ROS1: https://github.com/pal-robotics/tiago_robot/blob/kinetic-devel/tiago_controller_configuration/config/joint_trajectory_controllers.yaml

Or this in ROS2 for costmap layers: https://github.com/ROBOTIS-GIT/turtlebot3/blob/foxy-devel/turtlebot3_navigation2/param/waffle.yaml#L93

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

+// Copyright 2017 Open Source Robotics Foundation, Inc.

Changing the year.

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, const std::string & controller_name,+    const std::string & controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;

I chose to keep the old classes, but we can add them to ControllerInterface.

Should I do this on this PR? Shall I add to string arguments to the Interface constructor and get_name and get_type functions?

v-lopez

comment created time in 2 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::cleanup() const   return ret; } +std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_used_by_rt_list()+{+  used_by_realtime_controllers_index_ = updated_controllers_index_;+  return controllers_lists_[used_by_realtime_controllers_index_];+}++std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_unused_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  // Get the index to the outdated controller list+  int free_controllers_list = get_other_list(updated_controllers_index_);++  // Wait until the outdated controller list is not being used by the realtime thread+  wait_until_rt_not_using(free_controllers_list);+  return controllers_lists_[free_controllers_list];+}++const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list(+  const std::lock_guard<std::recursive_mutex> &) const+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  return controllers_lists_[updated_controllers_index_];+}++void ControllerManager::RTControllerListWrapper::switch_updated_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  int former_current_controllers_list_ = updated_controllers_index_;+  updated_controllers_index_ = get_other_list(former_current_controllers_list_);+  wait_until_rt_not_using(former_current_controllers_list_);+}++int ControllerManager::RTControllerListWrapper::get_other_list(int index) const+{+  return (index + 1) % 2;

Well I believe it's better than having this random operation duplicated in a few places in the code, the function allows for giving the operation a name and some documentation. I could make it a free function so it's not even a private member of the class.

v-lopez

comment created time in 2 days

PullRequestReviewEvent
PullRequestReviewEvent

issue commentros-controls/ros2_control

Implement `controller_manager::checkForConflict()`

The scaffoldings yes, the whole checking for conflict not though. I was under the impression that this was going to be handled with the new resource interface.

bmagyar

comment created time in 2 days

issue commentpal-robotics/talos_robot

Kinematic calibration

If possible we'd like to avoid extra links on the URDF, it could cause unknown effects on lots of parts of the code.

We were thinking something similar to what we have with the joint offsets.

We'd add 6 variables for x,y,z and r,p,y per joint in the existing calibration urdf file. They would replace the current joint offset variable.

In order to apply this variables, we'd add them here: https://github.com/pal-robotics/talos_robot/blob/kinetic-devel/talos_description/urdf/arm/arm.urdf.xacro#L59

So it would become:

      <origin xyz="${0.00000 + x_offset} ${0.15750*reflect  + y_offset} ${0.23200 + z_offset}"
              rpy="${0.0 * deg_to_rad + roll_offset} ${0.0 * deg_to_rad + pitch_offset} ${0.0 * deg_to_rad}"/>
      <axis xyz="0 0 1" />

Notice that I didn't add + yaw_offset. It is because this offset that coincides with the rotation angle of this joint, and it must be added to the transmission like we do already.

So for each joint, you'd apply 5 offsets here, and the one rotation offset that is aligned with the joint rotation axis must be added to the transmissions here

So this bit would become:


  <xacro:talos_arm_simple_transmission name="${name}" side="right" number="1" reduction="1.0" offset_value="${arm_right_1_yaw_offset}" />
  <xacro:talos_arm_simple_transmission name="${name}" side="right" number="2" reduction="1.0" offset_value="${arm_right_2_roll_offset}" />
  <xacro:talos_arm_simple_transmission name="${name}" side="right" number="3" reduction="1.0" offset_value="${arm_right_3_yaw_offset}" />
  <xacro:talos_arm_simple_transmission name="${name}" side="right" number="4" reduction="1.0" offset_value="${arm_right_4_pitch_offset}" />

Does it make sense? We would develop this in the next weeks.

jmirabel

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Added joint_limits_interface - Direct port from ROS 1

+// Copyright 2013, PAL Robotics S.L. All rights reserved.+// All rights reserved.+//+// Software License Agreement (BSD License 2.0)

If it is a problem we have no objections to change it to Apache2.0

ahcorde

comment created time in 3 days

PullRequestReviewEvent

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 62df3aabb89dfd1912dc16cb3ca7409539dd60c0

Documentation for controller_manager RT List wrapper and parameters

view details

Victor Lopez

commit sha bc63827074a5ebba9cc862bf90f5edd56ac3969a

controller_manager activate will not activate controllers

view details

Victor Lopez

commit sha 3650c388817d690e2f0f540fd6ba9857748cdc63

Fix constness in controller_loader

view details

push time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:

Uncrustify forces me to do it :cry:

v-lopez

comment created time in 3 days

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::cleanup() const   return ret; } +std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_used_by_rt_list()+{+  used_by_realtime_controllers_index_ = updated_controllers_index_;

I'm renaming to update_and_get_used_by_rt_list to make it more evident.

The thing is that the only time where we know we can change the RT list to the "updated" list is here, before returning a reference to it.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type;++  return add_controller_impl(controller_spec);+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name)+{+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());

This is a combination of bad naming of has_parameter and my lack of comments here.

has_parameter returns whether a parameter is declared or not... Yeah I know...

I'm adding comments to explain the whole parameter thing, but the idea is based of costmap's implementation. Since a priori we don't know the name of the controllers that will be loaded, we cannot declare them, so when we're told to load a controller by name, we need to declare the parameter if we haven't done so, and then read it.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the+     * updated list while it's being used+     * @return reference to the updated list+     */+    std::vector<ControllerSpec> & get_used_by_rt_list();++    /**+     * @brief get_unused_list Waits until the "outdated" and "unused by rt"+     *  lists match and returns a reference to it+     * This referenced list can be modified safely until switch_updated_controller_list()+     * is called, at this point the RT thread may start using it at any time+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    std::vector<ControllerSpec> & get_unused_list(+      const std::lock_guard<std::recursive_mutex> & guard);++    /**+     * @brief get_updated_list Returns a const reference to the most updated list,+     * @warning May or may not being used by the realtime thread, read-only reference for safety+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    const std::vector<ControllerSpec> & get_updated_list(+      const std::lock_guard<std::recursive_mutex> & guard) const;++    /**+     * @brief switch_updated_list Switches the "updated" and "outdated" lists, and waits+     *  until the RT thread is using the new "updated" list.+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    void switch_updated_list(const std::lock_guard<std::recursive_mutex> & guard);+++    // Mutex protecting the controllers list+    // must be acquired before using any list other than the "used by rt"+    mutable std::recursive_mutex controllers_lock_;++private:+    int get_other_list(int index) const;

Sure thing!

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the

My bad, I actually meant no one

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 controller_interface::return_type ControllerManager::activate() const {   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_state = loaded_controller->get_lifecycle_node()->activate();+  for (auto loaded_controller : controllers_lists_[current_controllers_list_]) {+    auto controller_state = loaded_controller.c->get_lifecycle_node()->activate();

All right, removing the activation of controllers.

v-lopez

comment created time in 3 days

PullRequestReviewEvent
PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

+// Copyright 2017 Open Source Robotics Foundation, Inc.+//+// Licensed under the Apache License, Version 2.0 (the "License");+// you may not use this file except in compliance with the License.+// You may obtain a copy of the License at+//+//     http://www.apache.org/licenses/LICENSE-2.0+//+// Unless required by applicable law or agreed to in writing, software+// distributed under the License is distributed on an "AS IS" BASIS,+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.+// See the License for the specific language governing permissions and+// limitations under the License.++#ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_+#define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_++#include <string>+// TODO(v-lopez)+// #include <vector>+// #include <hardware_interface/interface_resources.h>++namespace hardware_interface+{++/** \brief Controller Information+ *+ * This struct contains information about a given controller.+ *+ */+struct ControllerInfo+{+  /** Controller name. */+  std::string name;++  /** Controller type. */+  std::string type;++  // TODO(v-lopez)+  /** Claimed resources, grouped by the hardware interface they belong to. */+std::map<std::string, std::vector<std::string>> resources;

Indeed I did, thanks for spotting.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the+     * updated list while it's being used+     * @return reference to the updated list+     */+    std::vector<ControllerSpec> & get_used_by_rt_list();

It can't because I'm returning a non const reference to a member.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::cleanup() const   return ret; } +std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_used_by_rt_list()+{+  used_by_realtime_controllers_index_ = updated_controllers_index_;+  return controllers_lists_[used_by_realtime_controllers_index_];+}++std::vector<ControllerSpec> &+ControllerManager::RTControllerListWrapper::get_unused_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  // Get the index to the outdated controller list+  int free_controllers_list = get_other_list(updated_controllers_index_);++  // Wait until the outdated controller list is not being used by the realtime thread+  wait_until_rt_not_using(free_controllers_list);+  return controllers_lists_[free_controllers_list];+}++const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list(+  const std::lock_guard<std::recursive_mutex> &) const+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  return controllers_lists_[updated_controllers_index_];+}++void ControllerManager::RTControllerListWrapper::switch_updated_list(+  const std::lock_guard<std::recursive_mutex> &)+{+  assert(controllers_lock_.try_lock());+  controllers_lock_.unlock();+  int former_current_controllers_list_ = updated_controllers_index_;+  updated_controllers_index_ = get_other_list(former_current_controllers_list_);+  wait_until_rt_not_using(former_current_controllers_list_);+}++int ControllerManager::RTControllerListWrapper::get_other_list(int index) const+{+  return (index + 1) % 2;

Haha no problem, I'm adding a short description for clarity.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the+     * updated list while it's being used+     * @return reference to the updated list+     */+    std::vector<ControllerSpec> & get_used_by_rt_list();++    /**+     * @brief get_unused_list Waits until the "outdated" and "unused by rt"+     *  lists match and returns a reference to it+     * This referenced list can be modified safely until switch_updated_controller_list()+     * is called, at this point the RT thread may start using it at any time+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    std::vector<ControllerSpec> & get_unused_list(+      const std::lock_guard<std::recursive_mutex> & guard);++    /**+     * @brief get_updated_list Returns a const reference to the most updated list,+     * @warning May or may not being used by the realtime thread, read-only reference for safety+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list

Not really, the thing is that the updated list may or may not be the "unused by rt" list, which is the one we need to protect. If it is, you have the guard to ensure you're the only one. If it is not (and is therefore, being used by the rt thread) we're providing only a read only reference.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the+     * updated list while it's being used+     * @return reference to the updated list+     */+    std::vector<ControllerSpec> & get_used_by_rt_list();++    /**+     * @brief get_unused_list Waits until the "outdated" and "unused by rt"+     *  lists match and returns a reference to it+     * This referenced list can be modified safely until switch_updated_controller_list()+     * is called, at this point the RT thread may start using it at any time+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    std::vector<ControllerSpec> & get_unused_list(+      const std::lock_guard<std::recursive_mutex> & guard);++    /**+     * @brief get_updated_list Returns a const reference to the most updated list,+     * @warning May or may not being used by the realtime thread, read-only reference for safety+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list+     */+    const std::vector<ControllerSpec> & get_updated_list(+      const std::lock_guard<std::recursive_mutex> & guard) const;++    /**+     * @brief switch_updated_list Switches the "updated" and "outdated" lists, and waits+     *  until the RT thread is using the new "updated" list.+     * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list

Same as above.

v-lopez

comment created time in 3 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,+    std::string controller_type)   {-    return add_controller_impl(controller, controller_name);+    ControllerSpec controller_spec;+    controller_spec.c = controller;+    controller_spec.info.name = controller_name;+    controller_spec.info.type = controller_type;+    return add_controller_impl(controller_spec);   } +  /**+   * @brief switch_controller Stops some controllers and others.+   * @see Documentation in controller_manager_msgs/SwitchController.srv+   */+  CONTROLLER_MANAGER_PUBLIC+  controller_interface::return_type+  switch_controller(+    const std::vector<std::string> & start_controllers,+    const std::vector<std::string> & stop_controllers,+    int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES,+    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));+   CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type   update();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  configure() const;+  configure();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  activate() const;+  activate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  deactivate() const;+  deactivate();    CONTROLLER_MANAGER_PUBLIC   controller_interface::return_type-  cleanup() const;+  cleanup();  protected:   CONTROLLER_MANAGER_PUBLIC-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller_impl(-    std::shared_ptr<controller_interface::ControllerInterface> controller,-    const std::string & controller_name);+  controller_interface::ControllerInterfaceSharedPtr+  add_controller_impl(const ControllerSpec & controller);++  CONTROLLER_MANAGER_PUBLIC+  controller_interface::ControllerInterfaceSharedPtr get_controller_by_name(+    const std::string & name);++  CONTROLLER_MANAGER_PUBLIC+  void manage_switch();++  CONTROLLER_MANAGER_PUBLIC+  void stop_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers();++  CONTROLLER_MANAGER_PUBLIC+  void start_controllers_asap();++  CONTROLLER_MANAGER_PUBLIC+  void list_controllers_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void list_controller_types_srv_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void load_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void reload_controller_libraries_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void switch_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);++  CONTROLLER_MANAGER_PUBLIC+  void unload_controller_service_cb(+    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);  private:+  void get_controller_names(std::vector<std::string> & names);++   std::shared_ptr<hardware_interface::RobotHardware> hw_;   std::shared_ptr<rclcpp::Executor> executor_;   std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;-  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;++  /**+   * @brief The RTControllerListWrapper class wraps a  double-buffered list of controllers+   * to avoid needing to lock the  real-time thread when switching controllers in+   * the non-real-time thread.+   *+   * There's always an "updated" list and an "outdated" one+   * There's always an "used by rt" list and an "unused by rt" list+   *+   * The updated state changes on the switch_updated_list()+   * The rt usage state changes on the get_used_by_rt_list()+   */+  class RTControllerListWrapper+  {+public:+    /**+     * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list+     * @warning Should only be called by the RT thread, noone should modify the
     * @warning Should only be called by the RT thread, no one should modify the
v-lopez

comment created time in 3 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 34816769d559389dcd72fa9aa34fc62576954b16

Apply suggestions from code review Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com> Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

view details

push time in 3 days

issue commentmozilla/rr

Ryzen 3700x test failures

I won't have access to the PC I was using until tomorrow. But I'm trying today on a different computer with similar specs (Ryzen 3700x) with different kernel 4.15.0-72.

On this PC and with the latest master all tests are passing. Tomorrow I'll confirm on the other one.

v-lopez

comment created time in 3 days

issue commentpal-robotics/talos_robot

Kinematic calibration

Ok, please let me get back to you on Monday.

jmirabel

comment created time in 6 days

issue commentpal-robotics/talos_robot

Kinematic calibration

Hi @jmirabel,

We were beginning to work on calibrating the 3d position in addition to the joint offset of the arms, but have no ETA yet.

If you submit a proposal, we'll be happy to review it and get it into shape quickly as to not delay your work.

May I ask how are you performing the calibration? Do you have any results yet?

Best regards

jmirabel

comment created time in 6 days

issue commentmozilla/rr

Ryzen 3700x test failures

-rwxr-xr-x 1 root root 2030544 abr 16 2018 /lib/x86_64-linux-gnu/libc-2.27.so

v-lopez

comment created time in 6 days

issue commentmozilla/rr

Ryzen 3700x test failures

Ok that makes more sense, my bad.

Here it is:


(gdb) print copied
$1 = -1
(gdb) print km
$2 = (rr::KernelMapping &) @0x555555eebe90: {<rr::MemoryRange> = {start_ = {ptr = 140737326112768}, end_ = {ptr = 140737328209920}}, 
  static map_flags_mask = 147747, static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, 
  fsname_ = "/lib/x86_64-linux-gnu/libc-2.27.so", device_ = 2085, inode_ = 3544013, prot_ = 0, flags_ = 2, offset = 1994752}

v-lopez

comment created time in 6 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 64ff2b2ab85c5f9088f8625bac16bbbc47991cde

Apply suggestions from code review Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

view details

push time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerLoaderInterface    CONTROLLER_MANAGER_PUBLIC   virtual bool is_available(const std::string & controller_type) const = 0;++  CONTROLLER_MANAGER_PUBLIC+  const std::string & getName() const {return name_;}++  CONTROLLER_MANAGER_PUBLIC+  virtual std::vector<std::string> getDeclaredClasses() = 0;

Certainly, I'm not too trigger happy with consts on Interface classes, since I don't know what mechanisms will all subclasses need to perform an operation.

But I agree that this is pretty harmless.

v-lopez

comment created time in 6 days

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  return controllers_lists_[current_controllers_list_]; }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);++  controller_interface::ControllerInterface * ct;

Fixed in https://github.com/ros-controls/ros2_control/pull/139/commits/e4e226c8c2c3d7d828d46b6025fbe8bb932bf0c7

v-lopez

comment created time in 6 days

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();

I have refactored the whole thing here: 4c6a609

Resolving this discussion and we can follow up on https://github.com/ros-controls/ros2_control/pull/139#discussion_r489320563

v-lopez

comment created time in 6 days

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;

I have refactored the whole thing here: 4c6a609

Resolving this discussion and we can follow up on https://github.com/ros-controls/ros2_control/pull/139#discussion_r489320563

v-lopez

comment created time in 6 days

PullRequestReviewEvent

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;

I have refactored the whole thing here: https://github.com/ros-controls/ros2_control/pull/139/commits/4c6a609282f21e1fe8d33adce0dc523cb28eadf3

Hopefully it makes the original code easier to understand.

v-lopez

comment created time in 6 days

PullRequestReviewEvent

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 2b81d5d13140e32df3b85aaa606498923be698d3

Rename isControllerRunning function

view details

Victor Lopez

commit sha e4e226c8c2c3d7d828d46b6025fbe8bb932bf0c7

Don't use raw pointers in switch controller and get_controller_by_name

view details

push time in 6 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 4c6a609282f21e1fe8d33adce0dc523cb28eadf3

Refactor rt double buffered list for easier comprehension and safety

view details

push time in 6 days

issue commentmozilla/rr

Ryzen 3700x test failures

I can print more if you need:

(gdb) p km
$1 = {<rr::MemoryRange> = {start_ = {ptr = 1744830464}, end_ = {ptr = 1746927616}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "", device_ = 0, inode_ = 0, prot_ = 7, 
  flags_ = 2, offset = 0}
$2 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/usr/local/share/rr/rr_page_64", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$3 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "0\237\356UUU\000\000al/share/rr/rr_page_64", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$4 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "0\237\356UUU\000\000al/share/rr/rr_page_64", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$5 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "0\237\356UUU\000\000al/share/rr/rr_page_64", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$6 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "0\237\356UUU\000\000al/share/rr/rr_page_64", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$7 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "0\237\356UUU\000\000al/share/rr/rr_page_64", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$8 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = " \245\356UUU\000\000_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$9 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$10 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, 
  fsname_ = "\200\227\356UUU\000\000_64-linux-gnu/libc-2.2", device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$11 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$12 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$13 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$14 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$15 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$16 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$17 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$18 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = " \245\356UUU\000\000_64-linux-gnu/libc-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$19 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = " \245\356UUU\000\000_64-linux-gnu/libm-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$20 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = " \245\356UUU\000\000_64-linux-gnu/libm-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$21 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libm-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$22 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libm-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$23 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libm-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}
$24 = {<rr::MemoryRange> = {start_ = {ptr = 1879048192}, end_ = {ptr = 1879052288}}, static map_flags_mask = 147747, 
  static checkable_flags_mask = 3, static NO_DEVICE = 0, static NO_INODE = 0, fsname_ = "/lib/x86_64-linux-gnu/libm-2.2", 
  device_ = 2085, inode_ = 10630546, prot_ = 5, flags_ = 2, offset = 0}

v-lopez

comment created time in 6 days

issue commentmozilla/rr

Ryzen 3700x test failures

Yeah https://github.com/mozilla/rr/commit/49dd3af7ff1a92bec334db3102f6f3a91b2a4473 makes me skip the test.

I am having trouble understanding how to run nested_detach test. Where should .../nested-detach in your comment point to?

v-lopez

comment created time in 6 days

push eventpal-robotics-forks/ros2_control

Victor Lopez

commit sha 81a8d213709d62d0a2376e16ea46af382a612b8b

Remove whitespaces

view details

Victor Lopez

commit sha 160a7c7381fe908f3d9d6a9bd743dbe625c7f41a

Reference issues for todos added during services PR

view details

push time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  return controllers_lists_[current_controllers_list_]; }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);++  controller_interface::ControllerInterface * ct;+  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  const auto & controllers = controllers_lists_[current_controllers_list_];+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c.get()) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c.get()) {+        in_start_list = true;+        break;+      }+    }+++    const bool is_running = isControllerRunning(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c.get()), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c.get()), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return nullptr;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface+  controller.c->get_lifecycle_node()->configure();+  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());+  to.emplace_back(controller);++  // Destroys the old controllers list when the realtime thread is finished with it.+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return nullptr;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  from.clear();++  return to.back().c;+}++controller_interface::ControllerInterface * ControllerManager::get_controller_by_name(+  const std::string & name) {-  controller->init(hw_, controller_name);-  executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface());+  // Lock recursive mutex in this context+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_); -  loaded_controllers_.emplace_back(controller);-  return loaded_controllers_.back();+  for (const auto & controller : controllers_lists_[current_controllers_list_]) {+    if (controller.info.name == name) {+      return controller.c.get();+    }+  }+  return nullptr;+}++void ControllerManager::manage_switch()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // switch hardware interfaces (if any)+  if (!switch_params_.started) {+    robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);+    switch_params_.started = true;+  }+#endif++  stop_controllers();++  // start controllers once the switch is fully complete+  if (!switch_params_.start_asap) {+    start_controllers();+  } else {+    // start controllers as soon as their required joints are done switching+    start_controllers_asap();+  }+}++void ControllerManager::stop_controllers()+{+  // stop controllers+  for (const auto & request : stop_request_) {+    if (isControllerRunning(*request)) {+      const auto new_state = request->get_lifecycle_node()->deactivate();+      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {+        RCLCPP_ERROR(+          get_logger(),+          "After deactivating, controller %s is in state %s, expected Inactive",+          request->get_lifecycle_node()->get_name(),+          new_state.label().c_str());+      }+    }+  }+}++void ControllerManager::start_controllers()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers+  if (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::DONE) {+    for (const auto & request : start_request_) {+      request->startRequest(time);+    }++    switch_params_.do_switch = false;+  } else if (// NOLINT+    (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::ERROR) ||+    (switch_params_.timeout > 0.0 &&+    (time - switch_params_.init_time).toSec() > switch_params_.timeout))+  {+    // abort controllers in case of error or timeout (if set)+    for (const auto & request : start_request_) {+      request->abortRequest(time);+    }++    switch_params_.do_switch = false;+  } else {+    // wait controllers+    for (const auto & request : start_request_) {+      request->waitRequest(time);+    }+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  for (const auto & request : start_request_) {+    const auto new_state = request->get_lifecycle_node()->activate();+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {+      RCLCPP_ERROR(+        get_logger(),+        "After activating, controller %s is in state %s, expected Active",+        request->get_lifecycle_node()->get_name(),+        new_state.label().c_str());+    }+  }+  // All controllers started, switching done+  switch_params_.do_switch = false;+#endif+}++void ControllerManager::start_controllers_asap()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers if possible+  for (const auto & request : start_request_) {+    if (!isControllerRunning(*request)) {+      // find the info from this controller+      for (const auto & controller : controllers_lists_[current_controllers_list_]) {+        if (request == controller.c.get()) {+          // ready to start+          if (robot_hw_->switchResult(controller.info) ==+            hardware_interface::RobotHW::SwitchState::DONE)+          {+            request->startRequest(time);+          } else if ((robot_hw_->switchResult(controller.info) == // NOLINT+            hardware_interface::RobotHW::SwitchState::ERROR) ||+            (switch_params_.timeout > 0.0 &&+            (time - switch_params_.init_time).toSec() > switch_params_.timeout)) // NOLINT+          {+            // abort on error or timeout (if set)+            request->abortRequest(time);+          } else {+            // controller is waiting+            request->waitRequest(time);+          }+        }+        continue;+      }+    }+  }++  // all needed controllers started or aborted, switch done+  if (std::all_of(+      start_request_.begin(), start_request_.end(),+      [](controller_interface::ControllerBase * request) {+        return request->isRunning() || request->isAborted();+      }))+  {+    switch_params_.do_switch = false;+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  start_controllers();+#endif+}++void ControllerManager::list_controllers_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "list controller service called");+  std::lock_guard<std::mutex> services_guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list controller service locked");++  // lock controllers to get all names/types/states+  std::lock_guard<std::recursive_mutex> controller_guard(controllers_lock_);+  auto & controllers = controllers_lists_[current_controllers_list_];+  response->controller.resize(controllers.size());++  for (size_t i = 0; i < controllers.size(); ++i) {+    controller_manager_msgs::msg::ControllerState & cs = response->controller[i];+    cs.name = controllers[i].info.name;+    cs.type = controllers[i].info.type;+    cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label();++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    cs.claimed_resources.clear();+    typedef std::vector<hardware_interface::InterfaceResources> ClaimedResVec;+    typedef ClaimedResVec::const_iterator ClaimedResIt;+    const ClaimedResVec & c_resources = controllers[i].info.claimed_resources;+    for (const auto & c_resource : c_resources) {+      controller_manager_msgs::HardwareInterfaceResources iface_res;+      iface_res.hardware_interface = c_resource.hardware_interface;+      std::copy(+        c_resource.resources.begin(), c_resource.resources.end(),+        std::back_inserter(iface_res.resources));+      cs.claimed_resources.push_back(iface_res);+    }+#endif+  }++  RCLCPP_DEBUG(get_logger(), "list controller service finished");+}++void ControllerManager::list_controller_types_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "list types service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list types service locked");++  for (const auto & controller_loader : loaders_) {+    std::vector<std::string> cur_types = controller_loader->getDeclaredClasses();+    for (const auto & cur_type : cur_types) {+      response->types.push_back(cur_type);+      response->base_classes.push_back(controller_loader->getName());+      RCLCPP_INFO(get_logger(), cur_type);+    }+  }++  RCLCPP_DEBUG(get_logger(), "list types service finished");+}++void ControllerManager::load_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "loading service called for controller '%s' ", request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "loading service locked");++  response->ok = load_controller(request->name).get();++  RCLCPP_DEBUG(+    get_logger(), "loading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::reload_controller_libraries_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "reload libraries service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "reload libraries service locked");++  // only reload libraries if no controllers are running+  std::vector<std::string> loaded_controllers, running_controllers;+  get_controller_names(loaded_controllers);+  {+    std::lock_guard<std::recursive_mutex> guard(controllers_lock_);+    for (const auto & controller : controllers_lists_[current_controllers_list_]) {+      if (isControllerRunning(*controller.c)) {+        running_controllers.push_back(controller.info.name);+      }+    }+  }+  if (!running_controllers.empty() && !request->force_kill) {+    RCLCPP_ERROR(+      get_logger(), "Controller manager: Cannot reload controller libraries because"+      " there are still %i controllers running",+      (int)running_controllers.size());+    response->ok = false;+    return;+  }++  // stop running controllers if requested+  if (!loaded_controllers.empty()) {+    RCLCPP_INFO(get_logger(), "Controller manager: Stopping all running controllers");+    std::vector<std::string> empty;+    if (switch_controller(+        empty, running_controllers,+        controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) !=+      controller_interface::return_type::SUCCESS)+    {+      RCLCPP_ERROR(+        get_logger(),+        "Controller manager: Cannot reload controller libraries because failed to stop "+        "running controllers");+      response->ok = false;+      return;+    }+    for (const auto & controller : loaded_controllers) {+      if (unload_controller(controller) != controller_interface::return_type::SUCCESS) {+        RCLCPP_ERROR(+          get_logger(), "Controller manager: Cannot reload controller libraries because "+          "failed to unload controller '%s'",+          controller.c_str());+        response->ok = false;+        return;+      }+    }+    get_controller_names(loaded_controllers);+  }+  assert(loaded_controllers.empty());++  // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)+  for (const auto & controller_loader : loaders_) {+    controller_loader->reload();+    RCLCPP_INFO(+      get_logger(), "Controller manager: reloaded controller libraries for '%s'",+      controller_loader->getName().c_str());+  }++  response->ok = true;++  RCLCPP_DEBUG(get_logger(), "reload libraries service finished");+}++void ControllerManager::switch_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "switching service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "switching service locked");++  response->ok = switch_controller(+    request->start_controllers, request->stop_controllers, request->strictness,+    request->start_asap, request->timeout) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(get_logger(), "switching service finished");+}++void ControllerManager::unload_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(+    get_logger(), "unloading service called for controller '%s' ",+    request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "unloading service locked");++  response->ok = unload_controller(request->name) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(+    get_logger(), "unloading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::get_controller_names(std::vector<std::string> & names)+{+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);+  names.clear();+  for (const auto & controller : controllers_lists_[current_controllers_list_]) {+    names.push_back(controller.info.name);+  } }  controller_interface::return_type ControllerManager::update() {+  used_by_realtime_ = current_controllers_list_;+   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_ret = loaded_controller->update();-    if (controller_ret != controller_interface::return_type::SUCCESS) {-      ret = controller_ret;+  for (auto loaded_controller : controllers_lists_[used_by_realtime_]) {+    // TODO(v-lopez) we could cache this information

https://github.com/ros-controls/ros2_control/issues/153

v-lopez

comment created time in 6 days

PullRequestReviewEvent

issue openedros-controls/ros2_control

Cache running controllers to avoid checking status on update() loop

Originally posted by @bmagyar in https://github.com/ros-controls/ros2_control/pull/139#discussion_r490770028

It could also be used for reactivating previously active (or running) controllers on controller_manager activate (https://github.com/ros-controls/ros2_control/pull/139#discussion_r490786849)

created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  return controllers_lists_[current_controllers_list_]; }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);++  controller_interface::ControllerInterface * ct;+  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  const auto & controllers = controllers_lists_[current_controllers_list_];+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c.get()) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c.get()) {+        in_start_list = true;+        break;+      }+    }+++    const bool is_running = isControllerRunning(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c.get()), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c.get()), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return nullptr;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface

https://github.com/ros-controls/ros2_control/issues/152

v-lopez

comment created time in 6 days

PullRequestReviewEvent

issue openedros-controls/ros2_control

Loading a controller should only be allowed after controller_manager has been configured

can you please open an issue for this and reference it in here too?

Originally posted by @bmagyar in https://github.com/ros-controls/ros2_control/pull/139#discussion_r490768871

Probably a trivial issue outside of tests, since the load controller service is enabled on configure.

created time in 6 days

issue commentmozilla/rr

Ryzen 3700x test failures

Yes it does.

FAILED: errno=0 (Success)
clone_vfork_pidfd: /home/victor/rr/src/test/clone_vfork_pidfd.c:29: main: Assertion `"FAILED: !" && check_cond(pidfd == dummy_fds[1] + 1)' failed.
Aborted (core dumped)

pidfd = 99 dummy_fds[0] = 3 dummy_fds[1] = 4

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 class ControllerManager : public rclcpp::Node     typename T,     typename std::enable_if<std::is_convertible<       T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>-  std::shared_ptr<controller_interface::ControllerInterface>-  add_controller(std::shared_ptr<T> controller, std::string controller_name)+  controller_interface::ControllerInterfaceSharedPtr+  add_controller(+    std::shared_ptr<T> controller, std::string controller_name,

Actually I believe this is one of these instances where it's faster to not pass by const reference. Since you'll make a copy anyway later, with copy ellision it boils down to a single copy. With reference it's an address copy + a copy of the string.

All this doesn't matter anyway, because in this case the method is inlined, so the compiler will do whatever it thinks is best.

I can still change it for uniformity if you want.

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;

Sure, I'll add it.

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 controller_interface::return_type ControllerManager::activate() const {   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_state = loaded_controller->get_lifecycle_node()->activate();+  for (auto loaded_controller : controllers_lists_[current_controllers_list_]) {+    auto controller_state = loaded_controller.c->get_lifecycle_node()->activate();

What about activating previously active controllers?

Like you said, in the event of an emergency (which happen quite often with research robots) it's going to be a pain in the ass to reactivate controllers manually, it's something that is not needed in ROS1 right now.

Another alternative is not to deactivate the controllers themselves, just stop updating them because the controller manager is disabled.

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

+// Copyright 2017 Open Source Robotics Foundation, Inc.

Yeah, it's a new file but with most code moved from test_controller_manager.cpp. I can update the year but if you want but we should respect the rest.

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  return controllers_lists_[current_controllers_list_]; }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);++  controller_interface::ControllerInterface * ct;+  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  const auto & controllers = controllers_lists_[current_controllers_list_];+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c.get()) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c.get()) {+        in_start_list = true;+        break;+      }+    }+++    const bool is_running = isControllerRunning(*controller.c);++    if (!is_running && in_stop_list) {  // check for double stop+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not stop controller '" << controller.info.name <<+            "' since it is not running");+        in_stop_list = false;+        stop_request_.erase(+          std::remove(+            stop_request_.begin(), stop_request_.end(),+            controller.c.get()), stop_request_.end());+      }+    }++    if (is_running && !in_stop_list && in_start_list) {  // check for doubled start+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR_STREAM(+          get_logger(),+          "Controller '" << controller.info.name <<+            "' is already running");+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG_STREAM(+          get_logger(),+          "Could not start controller '" << controller.info.name <<+            "' since it is already running ");+        in_start_list = false;+        start_request_.erase(+          std::remove(+            start_request_.begin(), start_request_.end(),+            controller.c.get()), start_request_.end());+      }+    }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    if (is_running && in_stop_list && !in_start_list) {  // running and real stop+      switch_stop_list_.push_back(info);+    } else if (!is_running && !in_stop_list && in_start_list) {  // start, but no restart+      switch_start_list_.push_back(info);+    }++    bool add_to_list = is_running;+    if (in_stop_list) {+      add_to_list = false;+    }+    if (in_start_list) {+      add_to_list = true;+    }++    if (add_to_list) {+      info_list.push_back(info);+    }+#endif+  }++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  bool in_conflict = robot_hw_->checkForConflict(info_list);+  if (in_conflict) {+    RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }++  if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) {+    RCLCPP_ERROR(+      get_logger(),+      "Could not switch controllers. The hardware interface combination "+      "for the requested controllers is unfeasible.");+    stop_request_.clear();+    start_request_.clear();+    return controller_interface::return_type::ERROR;+  }+#endif++  if (start_request_.empty() && stop_request_.empty()) {+    RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch");+    return controller_interface::return_type::SUCCESS;+  }++  // start the atomic controller switching+  switch_params_.strictness = strictness;+  switch_params_.start_asap = start_asap;+  switch_params_.init_time = rclcpp::Clock().now();+  switch_params_.timeout = timeout;+  switch_params_.do_switch = true;+++  // wait until switch is finished+  RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop");+  while (rclcpp::ok() && switch_params_.do_switch) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(100));+  }+  start_request_.clear();+  stop_request_.clear();++  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");+  return controller_interface::return_type::SUCCESS;+}++controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl(-  std::shared_ptr<controller_interface::ControllerInterface> controller,-  const std::string & controller_name)+  const ControllerSpec & controller)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return nullptr;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Copy all controllers from the 'from' list to the 'to' list+  for (const auto & from_controller : from) {+    to.push_back(from_controller);+  }++  // Checks that we're not duplicating controllers+  for (const auto & to_controller : to) {+    if (controller.info.name == to_controller.info.name) {+      to.clear();+      RCLCPP_ERROR(+        get_logger(),+        "A controller named '%s' was already loaded inside the controller manager",+        controller.info.name.c_str());+      return nullptr;+    }+  }++  controller.c->init(hw_, controller.info.name);++  // TODO(v-lopez) this should only be done if controller_manager is configured.+  // Probably the whole load_controller part should fail if the controller_manager+  // is not configured, should it implement a LifecycleNodeInterface+  controller.c->get_lifecycle_node()->configure();+  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());+  to.emplace_back(controller);++  // Destroys the old controllers list when the realtime thread is finished with it.+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return nullptr;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  from.clear();++  return to.back().c;+}++controller_interface::ControllerInterface * ControllerManager::get_controller_by_name(+  const std::string & name) {-  controller->init(hw_, controller_name);-  executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface());+  // Lock recursive mutex in this context+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_); -  loaded_controllers_.emplace_back(controller);-  return loaded_controllers_.back();+  for (const auto & controller : controllers_lists_[current_controllers_list_]) {+    if (controller.info.name == name) {+      return controller.c.get();+    }+  }+  return nullptr;+}++void ControllerManager::manage_switch()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // switch hardware interfaces (if any)+  if (!switch_params_.started) {+    robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);+    switch_params_.started = true;+  }+#endif++  stop_controllers();++  // start controllers once the switch is fully complete+  if (!switch_params_.start_asap) {+    start_controllers();+  } else {+    // start controllers as soon as their required joints are done switching+    start_controllers_asap();+  }+}++void ControllerManager::stop_controllers()+{+  // stop controllers+  for (const auto & request : stop_request_) {+    if (isControllerRunning(*request)) {+      const auto new_state = request->get_lifecycle_node()->deactivate();+      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {+        RCLCPP_ERROR(+          get_logger(),+          "After deactivating, controller %s is in state %s, expected Inactive",+          request->get_lifecycle_node()->get_name(),+          new_state.label().c_str());+      }+    }+  }+}++void ControllerManager::start_controllers()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers+  if (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::DONE) {+    for (const auto & request : start_request_) {+      request->startRequest(time);+    }++    switch_params_.do_switch = false;+  } else if (// NOLINT+    (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::ERROR) ||+    (switch_params_.timeout > 0.0 &&+    (time - switch_params_.init_time).toSec() > switch_params_.timeout))+  {+    // abort controllers in case of error or timeout (if set)+    for (const auto & request : start_request_) {+      request->abortRequest(time);+    }++    switch_params_.do_switch = false;+  } else {+    // wait controllers+    for (const auto & request : start_request_) {+      request->waitRequest(time);+    }+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  for (const auto & request : start_request_) {+    const auto new_state = request->get_lifecycle_node()->activate();+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {+      RCLCPP_ERROR(+        get_logger(),+        "After activating, controller %s is in state %s, expected Active",+        request->get_lifecycle_node()->get_name(),+        new_state.label().c_str());+    }+  }+  // All controllers started, switching done+  switch_params_.do_switch = false;+#endif+}++void ControllerManager::start_controllers_asap()+{+#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // start controllers if possible+  for (const auto & request : start_request_) {+    if (!isControllerRunning(*request)) {+      // find the info from this controller+      for (const auto & controller : controllers_lists_[current_controllers_list_]) {+        if (request == controller.c.get()) {+          // ready to start+          if (robot_hw_->switchResult(controller.info) ==+            hardware_interface::RobotHW::SwitchState::DONE)+          {+            request->startRequest(time);+          } else if ((robot_hw_->switchResult(controller.info) == // NOLINT+            hardware_interface::RobotHW::SwitchState::ERROR) ||+            (switch_params_.timeout > 0.0 &&+            (time - switch_params_.init_time).toSec() > switch_params_.timeout)) // NOLINT+          {+            // abort on error or timeout (if set)+            request->abortRequest(time);+          } else {+            // controller is waiting+            request->waitRequest(time);+          }+        }+        continue;+      }+    }+  }++  // all needed controllers started or aborted, switch done+  if (std::all_of(+      start_request_.begin(), start_request_.end(),+      [](controller_interface::ControllerBase * request) {+        return request->isRunning() || request->isAborted();+      }))+  {+    switch_params_.do_switch = false;+  }+#else+  //  Dummy implementation, replace with the code above when migrated+  start_controllers();+#endif+}++void ControllerManager::list_controllers_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "list controller service called");+  std::lock_guard<std::mutex> services_guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list controller service locked");++  // lock controllers to get all names/types/states+  std::lock_guard<std::recursive_mutex> controller_guard(controllers_lock_);+  auto & controllers = controllers_lists_[current_controllers_list_];+  response->controller.resize(controllers.size());++  for (size_t i = 0; i < controllers.size(); ++i) {+    controller_manager_msgs::msg::ControllerState & cs = response->controller[i];+    cs.name = controllers[i].info.name;+    cs.type = controllers[i].info.type;+    cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label();++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+    cs.claimed_resources.clear();+    typedef std::vector<hardware_interface::InterfaceResources> ClaimedResVec;+    typedef ClaimedResVec::const_iterator ClaimedResIt;+    const ClaimedResVec & c_resources = controllers[i].info.claimed_resources;+    for (const auto & c_resource : c_resources) {+      controller_manager_msgs::HardwareInterfaceResources iface_res;+      iface_res.hardware_interface = c_resource.hardware_interface;+      std::copy(+        c_resource.resources.begin(), c_resource.resources.end(),+        std::back_inserter(iface_res.resources));+      cs.claimed_resources.push_back(iface_res);+    }+#endif+  }++  RCLCPP_DEBUG(get_logger(), "list controller service finished");+}++void ControllerManager::list_controller_types_srv_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request>,+  std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "list types service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "list types service locked");++  for (const auto & controller_loader : loaders_) {+    std::vector<std::string> cur_types = controller_loader->getDeclaredClasses();+    for (const auto & cur_type : cur_types) {+      response->types.push_back(cur_type);+      response->base_classes.push_back(controller_loader->getName());+      RCLCPP_INFO(get_logger(), cur_type);+    }+  }++  RCLCPP_DEBUG(get_logger(), "list types service finished");+}++void ControllerManager::load_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "loading service called for controller '%s' ", request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "loading service locked");++  response->ok = load_controller(request->name).get();++  RCLCPP_DEBUG(+    get_logger(), "loading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::reload_controller_libraries_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "reload libraries service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "reload libraries service locked");++  // only reload libraries if no controllers are running+  std::vector<std::string> loaded_controllers, running_controllers;+  get_controller_names(loaded_controllers);+  {+    std::lock_guard<std::recursive_mutex> guard(controllers_lock_);+    for (const auto & controller : controllers_lists_[current_controllers_list_]) {+      if (isControllerRunning(*controller.c)) {+        running_controllers.push_back(controller.info.name);+      }+    }+  }+  if (!running_controllers.empty() && !request->force_kill) {+    RCLCPP_ERROR(+      get_logger(), "Controller manager: Cannot reload controller libraries because"+      " there are still %i controllers running",+      (int)running_controllers.size());+    response->ok = false;+    return;+  }++  // stop running controllers if requested+  if (!loaded_controllers.empty()) {+    RCLCPP_INFO(get_logger(), "Controller manager: Stopping all running controllers");+    std::vector<std::string> empty;+    if (switch_controller(+        empty, running_controllers,+        controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) !=+      controller_interface::return_type::SUCCESS)+    {+      RCLCPP_ERROR(+        get_logger(),+        "Controller manager: Cannot reload controller libraries because failed to stop "+        "running controllers");+      response->ok = false;+      return;+    }+    for (const auto & controller : loaded_controllers) {+      if (unload_controller(controller) != controller_interface::return_type::SUCCESS) {+        RCLCPP_ERROR(+          get_logger(), "Controller manager: Cannot reload controller libraries because "+          "failed to unload controller '%s'",+          controller.c_str());+        response->ok = false;+        return;+      }+    }+    get_controller_names(loaded_controllers);+  }+  assert(loaded_controllers.empty());++  // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)+  for (const auto & controller_loader : loaders_) {+    controller_loader->reload();+    RCLCPP_INFO(+      get_logger(), "Controller manager: reloaded controller libraries for '%s'",+      controller_loader->getName().c_str());+  }++  response->ok = true;++  RCLCPP_DEBUG(get_logger(), "reload libraries service finished");+}++void ControllerManager::switch_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(get_logger(), "switching service called");+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "switching service locked");++  response->ok = switch_controller(+    request->start_controllers, request->stop_controllers, request->strictness,+    request->start_asap, request->timeout) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(get_logger(), "switching service finished");+}++void ControllerManager::unload_controller_service_cb(+  const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,+  std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response)+{+  // lock services+  RCLCPP_DEBUG(+    get_logger(), "unloading service called for controller '%s' ",+    request->name.c_str());+  std::lock_guard<std::mutex> guard(services_lock_);+  RCLCPP_DEBUG(get_logger(), "unloading service locked");++  response->ok = unload_controller(request->name) == controller_interface::return_type::SUCCESS;++  RCLCPP_DEBUG(+    get_logger(), "unloading service finished for controller '%s' ",+    request->name.c_str());+}++void ControllerManager::get_controller_names(std::vector<std::string> & names)+{+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);+  names.clear();+  for (const auto & controller : controllers_lists_[current_controllers_list_]) {+    names.push_back(controller.info.name);+  } }  controller_interface::return_type ControllerManager::update() {+  used_by_realtime_ = current_controllers_list_;+   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_ret = loaded_controller->update();-    if (controller_ret != controller_interface::return_type::SUCCESS) {-      ret = controller_ret;+  for (auto loaded_controller : controllers_lists_[used_by_realtime_]) {+    // TODO(v-lopez) we could cache this information+    if (isControllerRunning(*loaded_controller.c)) {+      auto controller_ret = loaded_controller.c->update();+      if (controller_ret != controller_interface::return_type::SUCCESS) {+        ret = controller_ret;+      }     }   } +  // there are controllers to start/stop+  if (switch_params_.do_switch) {+    manage_switch();+  }   return ret; }  controller_interface::return_type-ControllerManager::configure() const+ControllerManager::configure() {   auto ret = controller_interface::return_type::SUCCESS;-  for (auto loaded_controller : loaded_controllers_) {-    auto controller_state = loaded_controller->get_lifecycle_node()->configure();+

If I got some sponsors I would put here some advertisements and monetize this a little bit... But in the meantime I'll remove it.

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  return controllers_lists_[current_controllers_list_]; }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);++  controller_interface::ControllerInterface * ct;

No problem, I can fix this in the PR.

There are many details like this where I based heavily on the ROS1 implementation to make the review process smoother by being similar to the old implementation, but those same details are the ones raising up questions.

I guess it was a bad call, with so much mix of new and old code it doesn't make sense to keep these things.

v-lopez

comment created time in 6 days

Pull request review commentros-controls/ros2_control

Add controller manager services

 ControllerManager::load_controller(     [&](auto loader)     {return loader->is_available(controller_type);}); -  std::shared_ptr<controller_interface::ControllerInterface> controller(nullptr);+  controller_interface::ControllerInterfaceSharedPtr controller(nullptr);   if (it != loaders_.cend()) {     controller = (*it)->create(controller_type);   } else {     const std::string error_msg("Loader for controller '" + controller_name + "' not found\n");     RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str());     throw std::runtime_error(error_msg);   }+  ControllerSpec controller_spec;+  controller_spec.c = controller;+  controller_spec.info.name = controller_name;+  controller_spec.info.type = controller_type; -  return add_controller_impl(controller, controller_name);+  return add_controller_impl(controller_spec); } -std::vector<std::shared_ptr<controller_interface::ControllerInterface>>-ControllerManager::get_loaded_controllers() const+controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(+  const std::string & controller_name) {-  return loaded_controllers_;+  const std::string param_name = controller_name + ".type";+  std::string controller_type;+  if (!has_parameter(param_name)) {+    declare_parameter(param_name, rclcpp::ParameterValue());+  }+  if (!get_parameter(param_name, controller_type)) {+    RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str());+    return nullptr;+  }+  return load_controller(controller_name, controller_type);+}++controller_interface::return_type ControllerManager::unload_controller(+  const std::string & controller_name)+{+  // get reference to controller list+  int free_controllers_list = (current_controllers_list_ + 1) % 2;+  while (rclcpp::ok() && free_controllers_list == used_by_realtime_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  std::vector<ControllerSpec>+  & from = controllers_lists_[current_controllers_list_],+  & to = controllers_lists_[free_controllers_list];+  to.clear();++  // Transfers the running controllers over, skipping the one to be removed and the running ones.+  bool removed = false;+  for (const auto & controller : from) {+    if (controller.info.name == controller_name) {+      if (isControllerRunning(*controller.c)) {+        to.clear();+        RCLCPP_ERROR(+          get_logger(),+          "Could not unload controller with name '%s' because it is still running",+          controller_name.c_str());+        return controller_interface::return_type::ERROR;+      }+      RCLCPP_DEBUG(get_logger(), "Cleanup controller");+      controller.c->get_lifecycle_node()->cleanup();+      executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());+      removed = true;+    } else {+      to.push_back(controller);+    }+  }++  // Fails if we could not remove the controllers+  if (!removed) {+    to.clear();+    RCLCPP_ERROR(+      get_logger(),+      "Could not unload controller with name '%s' because no controller with this name exists",+      controller_name.c_str());+    return controller_interface::return_type::ERROR;+  }+++  // Destroys the old controllers list when the realtime thread is finished with it.+  RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list");+  int former_current_controllers_list_ = current_controllers_list_;+  current_controllers_list_ = free_controllers_list;+  while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) {+    if (!rclcpp::ok()) {+      return controller_interface::return_type::ERROR;+    }+    std::this_thread::sleep_for(std::chrono::microseconds(200));+  }+  RCLCPP_DEBUG(get_logger(), "Destruct controller");+  from.clear();+  RCLCPP_DEBUG(get_logger(), "Destruct controller finished");++  RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());+  return controller_interface::return_type::SUCCESS;+}++std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const+{+  return controllers_lists_[current_controllers_list_]; }  void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) {   loaders_.push_back(loader); } -std::shared_ptr<controller_interface::ControllerInterface>+controller_interface::return_type ControllerManager::switch_controller(+  const std::vector<std::string> & start_controllers,+  const std::vector<std::string> & stop_controllers, int strictness, bool start_asap,+  const rclcpp::Duration & timeout)+{+  switch_params_ = SwitchParams();++  if (!stop_request_.empty() || !start_request_.empty()) {+    RCLCPP_FATAL(+      get_logger(),+      "The internal stop and start request lists are not empty at the beginning of the "+      "switchController() call. This should not happen.");+  }++  if (strictness == 0) {+    RCLCPP_WARN(+      get_logger(), "Controller Manager: To switch controllers you need to specify a "+      "strictness level of controller_manager_msgs::SwitchController::STRICT "+      "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",+      controller_manager_msgs::srv::SwitchController::Request::STRICT,+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);+    strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;+  }++  RCLCPP_DEBUG(get_logger(), "switching controllers:");+  for (const auto & controller : start_controllers) {+    RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str());+  }+  for (const auto & controller : stop_controllers) {+    RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str());+  }++  // lock controllers+  std::lock_guard<std::recursive_mutex> guard(controllers_lock_);++  controller_interface::ControllerInterface * ct;+  // list all controllers to stop+  for (const auto & controller : stop_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not stop controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(),+        "Found controller '%s' that needs to be stopped in list of controllers",+        controller.c_str());+      stop_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size());++  // list all controllers to start+  for (const auto & controller : start_controllers) {+    ct = get_controller_by_name(controller);+    if (ct == nullptr) {+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) {+        RCLCPP_ERROR(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+        stop_request_.clear();+        start_request_.clear();+        return controller_interface::return_type::ERROR;+      } else {+        RCLCPP_DEBUG(+          get_logger(),+          "Could not start controller with name '%s' because no controller with this name exists",+          controller.c_str());+      }+    } else {+      RCLCPP_DEBUG(+        get_logger(), "Found controller '%s' that needs to be started in list of controllers",+        controller.c_str());+      start_request_.push_back(ct);+    }+  }+  RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size());++#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING+  // Do the resource management checking+  std::list<hardware_interface::ControllerInfo> info_list;+  switch_start_list_.clear();+  switch_stop_list_.clear();+#endif++  const auto & controllers = controllers_lists_[current_controllers_list_];+  for (const auto & controller : controllers) {+    bool in_stop_list = false;+    for (const auto & request : stop_request_) {+      if (request == controller.c.get()) {+        in_stop_list = true;+        break;+      }+    }++    bool in_start_list = false;+    for (const auto & request : start_request_) {+      if (request == controller.c.get()) {+        in_start_list = true;+        break;+      }+    }+

At least uncrustify didn't and I missed it. Fixing it.

v-lopez

comment created time in 6 days

PullRequestReviewEvent
PullRequestReviewEvent

issue commentmozilla/rr

Ryzen 3700x test failures

The clone_vfork_pidfd has a similar problem to what was fixed in 17aa8239c0a9ffd0e66623fc3627f664b384bf1e, and nested-detach has a different kind of assertion.

Originally posted by @glandium in https://github.com/mozilla/rr/issues/2677#issuecomment-694718007

v-lopez

comment created time in 6 days

issue openedmozilla/rr

Ryzen 3700x test failures

Ryzen 3700x After following the setup instructions, I get 12 failures out of 2487 tests. Which is still great compared to before.

Summary

	 82 - clone_vfork_pidfd (Failed)
	 83 - clone_vfork_pidfd-no-syscallbuf (Failed)
	920 - nested_detach_wait (Failed)
	921 - nested_detach_wait-no-syscallbuf (Failed)
	1140 - nested_detach (Failed)
	1141 - nested_detach-no-syscallbuf (Failed)
	1326 - clone_vfork_pidfd-32 (Failed)
	1327 - clone_vfork_pidfd-32-no-syscallbuf (Failed)
	2162 - nested_detach_wait-32 (Failed)
	2163 - nested_detach_wait-32-no-syscallbuf (Failed)
	2382 - nested_detach-32 (Failed)
	2383 - nested_detach-32-no-syscallbuf (Failed)

full output.txt

These are the .err files of all the tests, I can provide the rest of files, but the tar would be too big to provide all at once. rr-tests.tar.gz

Originally posted by @v-lopez in https://github.com/mozilla/rr/issues/2677#issuecomment-694716090

created time in 6 days

issue commentmozilla/rr

Ryzen 3900X test failures

Ryzen 3700x After following the setup instructions, I get 12 failures out of 2487 tests. Which is still great compared to before.

Summary

	 82 - clone_vfork_pidfd (Failed)
	 83 - clone_vfork_pidfd-no-syscallbuf (Failed)
	920 - nested_detach_wait (Failed)
	921 - nested_detach_wait-no-syscallbuf (Failed)
	1140 - nested_detach (Failed)
	1141 - nested_detach-no-syscallbuf (Failed)
	1326 - clone_vfork_pidfd-32 (Failed)
	1327 - clone_vfork_pidfd-32-no-syscallbuf (Failed)
	2162 - nested_detach_wait-32 (Failed)
	2163 - nested_detach_wait-32-no-syscallbuf (Failed)
	2382 - nested_detach-32 (Failed)
	2383 - nested_detach-32-no-syscallbuf (Failed)

full output.txt

These are the .err files of all the tests, I can provide the rest of files, but the tar would be too big to pr rr-tests.tar.gz ovide all at once.

Manishearth

comment created time in 6 days

issue commentmozilla/rr

Problems running test on AMD Ryzen Threadripper 2950X

No sorry. After the modprobe I now see cpu ids:

$ ls -lah /dev/cpu/
total 0
drwxr-xr-x 18 root root     380 sep 18 08:57 .
drwxr-xr-x 22 root root    5,1K sep 18 07:54 ..
drwxr-xr-x  2 root root      60 sep 18 08:57 0
drwxr-xr-x  2 root root      60 sep 18 08:57 1
drwxr-xr-x  2 root root      60 sep 18 08:57 10
drwxr-xr-x  2 root root      60 sep 18 08:57 11
drwxr-xr-x  2 root root      60 sep 18 08:57 12
drwxr-xr-x  2 root root      60 sep 18 08:57 13
drwxr-xr-x  2 root root      60 sep 18 08:57 14
drwxr-xr-x  2 root root      60 sep 18 08:57 15
drwxr-xr-x  2 root root      60 sep 18 08:57 2
drwxr-xr-x  2 root root      60 sep 18 08:57 3
drwxr-xr-x  2 root root      60 sep 18 08:57 4
drwxr-xr-x  2 root root      60 sep 18 08:57 5
drwxr-xr-x  2 root root      60 sep 18 08:57 6
drwxr-xr-x  2 root root      60 sep 18 08:57 7
drwxr-xr-x  2 root root      60 sep 18 08:57 8
drwxr-xr-x  2 root root      60 sep 18 08:57 9
crw-------  1 root root 10, 184 sep 18 07:54 microcode

And after running the script I don't get the SpecLockMap warning.

After running the output is:

sudo rdmsr -a -x 0xc0011020
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
46404000000000
emoon

comment created time in 6 days

issue commentmozilla/rr

Problems running test on AMD Ryzen Threadripper 2950X

$  sudo rdmsr -a -x 0xc0011020
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
6404000000000
emoon

comment created time in 6 days

issue commentmozilla/rr

Problems running test on AMD Ryzen Threadripper 2950X

Similar situation here with 3700x.

$ ls -lah /dev/cpu
total 0
drwxr-xr-x  2 root root      60 sep 18 07:54 .
drwxr-xr-x 22 root root    5,1K sep 18 07:54 ..
crw-------  1 root root 10, 184 sep 18 07:54 microcode

and sudo rdmsr -a -x 0xc0011020 doesn't provide any output.

After running zen_workaround.py I still get the SpecLockMap warning and I have many test failures, see attached file.

output.txt

emoon

comment created time in 6 days

push eventpal-robotics-forks/ros2_control

Andreas Klintberg

commit sha 0fb501481a5331fdfd84482f462186a0ce56f70c

fixed logging issue and update readme (#126)

view details

Bence Magyar

commit sha 6eee12bc8450aaa59b5ac57f2090fe7dbd7d67e2

Lower test coverage standards (#128)

view details

Denis Štogl

commit sha 3a1fad50b5585ca96d838ad779331fb977020faf

Hardware component interfaces (#121) * Initial version of new package implementing components for controlling the robot. * Add control component interfaces. * Added initial tests for Joint and JointInterface * Extended SensorInterface to work with hardware. Added Example for DummySensor and HW in tests. Added lifecycle management for components #10 * Remove component types since they are not used yet. * Added tests for all interfaces and implemented review comments * Added HardwareInfo as the main structure in the robot's URDF file * Add Hardware and Component base classes and tests. * Split Component and Hardware Infos into two files * Use separated headers for components_info and hardware_info * Style corrections * Add documentation for joint and sensor components * Update hardware_interface/test/test_component_interfaces.cpp * Style corrections * The base classes for Joints and Sensors are implemented * Add more tests to get better coverage * Updated after comments and added helper header * Implementation moved to cpp files * Removed virtual function for checking limits * Use inline functions in helper header and make API-consistent return values * move components into components folder

view details

Jordan Palacios

commit sha b6a50df033319ea87a44f4b6192a0d4a88d87345

Dynamic joint handles (#125) * Removed auto include * Added JointHandle string-based interface and handle-handling using DynamicJointState message * Add typename, otherwise does not build when used from templates * Use Handle common class for Actuator and Joint handle * Moved common code to separate functions * Fixed warnings * Disabling failing tests not compatible with the new API * Register dynamic actuator and joints in test_robot_hardware * Added functions to get actuator/joint registered interface names * Added functions to geti all actuator/joint handles of a given interface

view details

Anas Abou Allaban

commit sha ec5e6500488c21e1074388f111098581715ceceb

Update Docs (#137) Signed-off-by: Anas Abou Allaban <aabouallaban@pm.me>

view details

Jordan Palacios

commit sha b9ea092b47a874e51cf73dfdee9b1aa0c2b8afa8

Add new joint handles to test_robot_hardware (#136) * Added the new joint handles to test_robot_hardware * Disable foxy main CI Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>

view details

Denis Štogl

commit sha 2c3aad1cb8ad4d23925bceeb5c26e22d6cf859ef

New version of component parser (#127) * Added new version of parser. Files checkout from #122 * SensorHardware can handle multiple sensor types

view details

Jordan Palacios

commit sha 9a25721bbec2e8db9410163a7d50160ded0d72c1

Remove old joint state and joint command handles (#134) * Removed joint state handle and joint command handle * Removed old handles from test_robot_hardware

view details

Victor Lopez

commit sha 5deeaf6af3285f8f9865ab9fb9196bfc61437548

Add controller_manager_msgs package

view details

Victor Lopez

commit sha f00ecec63a8b2ef0388eb64a0d712b7039f6b925

Add switch and unload controller functionality

view details

Victor Lopez

commit sha 91e1767e0d2cb3a218be5e3844b1238bd8a7fbdf

Use ControllerSpec for storing controller type and name

view details

Victor Lopez

commit sha bef08e23b378b09d89c4612a2385a9060373e6df

Add more messages and services

view details

Victor Lopez

commit sha 27206eabb01a0d989a1489606a3cb57e3b2847e0

Add list_controllers service

view details

Victor Lopez

commit sha 47b170f6a5de3cd9b90df7157e26d3b14b96b221

Implement load, unload, switch and reload services

view details

push time in 6 days

more