Fix authorities and framework

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

scitos2_core/include/scitos2_core/module.hpp

@@ -82,6 +82,14 @@ class Module
       return false;
     }
 
+    // Check if the authority is valid or if the service exists
+    if (!sharedAuthority->isValid() || !sharedAuthority->existsService("/robot/Robot")) {
+      RCLCPP_ERROR_ONCE(
+        rclcpp::get_logger("MIRA"),
+        "MIRA authority is not valid or service does not exist");
+      return false;
+    }
+
     try {
       mira::RPCFuture<void> rpc = sharedAuthority->callService<void>("/robot/Robot", service_name);
       rpc.timedWait(mira::Duration::seconds(1));
@@ -115,6 +123,14 @@ class Module
       return false;
     }
 
+    // Check if the authority is valid or if the service exists
+    if (!sharedAuthority->isValid() || !sharedAuthority->existsService("/robot/Robot")) {
+      RCLCPP_ERROR_ONCE(
+        rclcpp::get_logger("MIRA"),
+        "MIRA authority is not valid or service does not exist");
+      return false;
+    }
+
     try {
       mira::RPCFuture<void> rpc;
       if (request.has_value()) {
@@ -151,6 +167,15 @@ class Module
     if (!sharedAuthority) {
       return false;
     }
+
+    // Check if the authority is valid or if the service exists
+    if (!sharedAuthority->isValid() || !sharedAuthority->existsService("/robot/Robot")) {
+      RCLCPP_ERROR_ONCE(
+        rclcpp::get_logger("MIRA"),
+        "MIRA authority is not valid or service does not exist");
+      return false;
+    }
+
     try {
       mira::RPCFuture<void> rpc = sharedAuthority->callService<void>(
         "/robot/Robot#builtin", std::string("setProperty"), param_name, value);
@@ -181,6 +206,15 @@ class Module
     if (!sharedAuthority) {
       return "";
     }
+
+    // Check if the authority is valid or if the service exists
+    if (!sharedAuthority->isValid() || !sharedAuthority->existsService("/robot/Robot")) {
+      RCLCPP_ERROR_ONCE(
+        rclcpp::get_logger("MIRA"),
+        "MIRA authority is not valid or service does not exist");
+      return "";
+    }
+
     try {
       mira::RPCFuture<std::string> rpc = sharedAuthority->callService<std::string>(
         "/robot/Robot#builtin", std::string("getProperty"), param_name);

scitos2_mira/include/scitos2_mira/mira_framework.hpp

@@ -109,6 +109,7 @@ class MiraFramework : public nav2_util::LifecycleNode
 
   // MIRA framework
   std::unique_ptr<mira::Framework> framework_;
+  bool loaded_;
 
   // Modules Plugins
   pluginlib::ClassLoader<scitos2_core::Module> module_loader_;

scitos2_mira/src/mira_framework.cpp

@@ -28,6 +28,7 @@ namespace scitos2_mira
 
 MiraFramework::MiraFramework(const rclcpp::NodeOptions & options)
 : nav2_util::LifecycleNode("scitos_mira", "", options),
+  loaded_(false),
   module_loader_("scitos2_core", "scitos2_core::Module"),
   default_ids_{"drive"},
   default_types_{"scitos2_modules::Drive"}
@@ -47,6 +48,7 @@ MiraFramework::~MiraFramework()
   if (framework_->isTerminationRequested()) {
     RCLCPP_INFO(get_logger(), "Stopping MIRA framework...");
   }
+  framework_.reset();
 }
 
 nav2_util::CallbackReturn MiraFramework::on_configure(const rclcpp_lifecycle::State &)
@@ -62,12 +64,17 @@ nav2_util::CallbackReturn MiraFramework::on_configure(const rclcpp_lifecycle::St
     rcl_interfaces::msg::ParameterDescriptor()
     .set__description("Configuration of the robot in XML format"));
   this->get_parameter("scitos_config", config);
-  if (!config.empty()) {
-    RCLCPP_INFO(get_logger(), "Loaded scitos config: %s", config.c_str());
-    framework_->load(config);
+  if (!loaded_) {
+    if (!config.empty()) {
+      RCLCPP_INFO(get_logger(), "Loaded scitos config: %s", config.c_str());
+      framework_->load(config);
+      loaded_ = true;
+    } else {
+      RCLCPP_ERROR(get_logger(), "Can't read parameter 'scitos_config'");
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   } else {
-    RCLCPP_ERROR(get_logger(), "Can't read parameter 'scitos_config'");
-    return nav2_util::CallbackReturn::FAILURE;
+    RCLCPP_WARN(get_logger(), "Already loaded scitos config");
   }
 
   nav2_util::declare_parameter_if_not_declared(
@@ -113,8 +120,6 @@ nav2_util::CallbackReturn MiraFramework::on_configure(const rclcpp_lifecycle::St
     get_logger(),
     "MIRA framework has %s modules available.", module_ids_concat_.c_str());
 
-  framework_->start();
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -128,6 +133,9 @@ nav2_util::CallbackReturn MiraFramework::on_activate(const rclcpp_lifecycle::Sta
     it->second->activate();
   }
 
+  // Start the MIRA framework
+  framework_->start();
+
   // Create bond connection
   createBond();
 

scitos2_mira/test/test_mira_framework.cpp

@@ -32,11 +32,19 @@ TEST(ScitosMiraFrameworkTest, configure) {
   // Create the node
   auto node = std::make_shared<MiraFrameworkFixture>();
 
-  // Set the scitos config parameter. In the the robot this should be a XML file
-  std::string pkg = ament_index_cpp::get_package_share_directory("scitos2_mira");
+  // Set an empty scitos config parameter
+  nav2_util::declare_parameter_if_not_declared(node, "scitos_config", rclcpp::ParameterValue(""));
+
+  // Configure the node
+  node->configure();
+  node->activate();
 
-  nav2_util::declare_parameter_if_not_declared(
-    node, "scitos_config", rclcpp::ParameterValue(pkg + "/test/scitos_config.xml"));
+  // Check results: the node should be in the unconfigured state as scitos_config plugins is empty
+  EXPECT_EQ(node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+  // Now, set the scitos config parameter. In the the robot this should be a XML file
+  std::string pkg = ament_index_cpp::get_package_share_directory("scitos2_mira");
+  node->set_parameter(rclcpp::Parameter("scitos_config", pkg + "/test/scitos_config.xml"));
 
   // Configure the node
   node->configure();
@@ -49,17 +57,13 @@ TEST(ScitosMiraFrameworkTest, configure) {
   node->deactivate();
   node->cleanup();
 
-  // Now, set the module plugins as empty
-  node->set_parameter(rclcpp::Parameter("scitos_config", ""));
-
-  // Configure the node
+  // We configure again to show the warning
   node->configure();
   node->activate();
 
-  // Check results: the node should be in the unconfigured state as scitos_config plugins is empty
-  EXPECT_EQ(node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
-
-  node->shutdown();
+  // Cleaning up
+  node->deactivate();
+  node->cleanup();
 }
 
 int main(int argc, char ** argv)

scitos2_modules/include/scitos2_modules/drive.hpp

@@ -360,6 +360,7 @@ class Drive : public scitos2_core::Module
   std::string base_frame_;
   bool emergency_stop_activated_;
   scitos2_msgs::msg::BarrierStatus barrier_status_;
+  bool is_active_;
 
   // Bumper
   bool bumper_activated_;

scitos2_modules/src/charger.cpp

@@ -35,7 +35,8 @@ void Charger::configure(
 
   plugin_name_ = name;
   logger_ = node->get_logger();
-  authority_ = std::make_shared<mira::Authority>("/", plugin_name_);
+  authority_ = std::make_shared<mira::Authority>();
+  authority_->checkin("/", plugin_name_);
 
   // Create ROS publishers
   battery_pub_ = node->create_publisher<sensor_msgs::msg::BatteryState>("battery", 1);
@@ -64,7 +65,6 @@ void Charger::cleanup()
     logger_,
     "Cleaning up module : %s of type scitos2_module::Charger",
     plugin_name_.c_str());
-  authority_->checkout();
   authority_.reset();
   battery_pub_.reset();
   charger_pub_.reset();
@@ -78,6 +78,7 @@ void Charger::activate()
     plugin_name_.c_str());
   battery_pub_->on_activate();
   charger_pub_->on_activate();
+  authority_->start();
 }
 
 void Charger::deactivate()
@@ -86,6 +87,7 @@ void Charger::deactivate()
     logger_,
     "Deactivating module : %s of type scitos2_module::Charger",
     plugin_name_.c_str());
+  authority_->checkout();
   battery_pub_->on_deactivate();
   charger_pub_->on_deactivate();
 }

scitos2_modules/src/display.cpp

@@ -34,7 +34,8 @@ void Display::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
   plugin_name_ = name;
   logger_ = node->get_logger();
   clock_ = node->get_clock();
-  authority_ = std::make_shared<mira::Authority>("/", name);
+  authority_ = std::make_shared<mira::Authority>();
+  authority_->checkin("/", plugin_name_);
 
   // Create publisher
   display_data_pub_ = node->create_publisher<scitos2_msgs::msg::MenuEntry>("user_menu_selected", 1);
@@ -101,7 +102,6 @@ void Display::cleanup()
     logger_,
     "Cleaning up module : %s of type scitos2_module::Display",
     plugin_name_.c_str());
-  authority_->checkout();
   authority_.reset();
   display_data_pub_.reset();
 }
@@ -113,6 +113,7 @@ void Display::activate()
     "Activating module : %s of type scitos2_module::Display",
     plugin_name_.c_str());
   display_data_pub_->on_activate();
+  authority_->start();
 }
 
 void Display::deactivate()
@@ -121,6 +122,7 @@ void Display::deactivate()
     logger_,
     "Deactivating module : %s of type scitos2_module::Display",
     plugin_name_.c_str());
+  authority_->checkout();
   display_data_pub_->on_deactivate();
 }
 

scitos2_modules/src/drive.cpp

@@ -40,10 +40,12 @@ void Drive::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, s
     throw std::runtime_error("Unable to lock node!");
   }
 
+  is_active_ = false;
   plugin_name_ = name;
   logger_ = node->get_logger();
   clock_ = node->get_clock();
-  authority_ = std::make_shared<mira::Authority>("/", name);
+  authority_ = std::make_shared<mira::Authority>();
+  authority_->checkin("/", plugin_name_);
 
   // Create ROS publishers
   auto latched_profile = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
@@ -161,7 +163,6 @@ void Drive::cleanup()
     logger_,
     "Cleaning up module : %s of type scitos2_module::Drive",
     plugin_name_.c_str());
-  authority_->checkout();
   authority_.reset();
   bumper_pub_.reset();
   bumper_markers_pub_.reset();
@@ -188,6 +189,8 @@ void Drive::activate()
   mileage_pub_->on_activate();
   odometry_pub_->on_activate();
   rfid_pub_->on_activate();
+  authority_->start();
+  is_active_ = true;
 }
 
 void Drive::deactivate()
@@ -196,6 +199,7 @@ void Drive::deactivate()
     logger_,
     "Deactivating module : %s of type scitos2_module::Drive",
     plugin_name_.c_str());
+  authority_->checkout();
   bumper_pub_->on_deactivate();
   bumper_markers_pub_->on_deactivate();
   drive_status_pub_->on_deactivate();
@@ -204,6 +208,7 @@ void Drive::deactivate()
   mileage_pub_->on_deactivate();
   odometry_pub_->on_deactivate();
   rfid_pub_->on_deactivate();
+  is_active_ = false;
 }
 
 rcl_interfaces::msg::SetParametersResult Drive::dynamicParametersCallback(
@@ -309,7 +314,7 @@ void Drive::rfidStatusCallback(mira::ChannelRead<uint64> data)
 
 void Drive::velocityCommandCallback(const geometry_msgs::msg::Twist & msg)
 {
-  if (!emergency_stop_activated_) {
+  if (!emergency_stop_activated_ && is_active_) {
     mira::Velocity2 speed(msg.linear.x, 0, msg.angular.z);
     call_mira_service(authority_, "setVelocity", std::optional<mira::Velocity2>(speed));
   }

scitos2_modules/src/ebc.cpp

@@ -33,7 +33,8 @@ void EBC::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std
 
   plugin_name_ = name;
   logger_ = node->get_logger();
-  authority_ = std::make_shared<mira::Authority>("/", plugin_name_);
+  authority_ = std::make_shared<mira::Authority>();
+  authority_->checkin("/", plugin_name_);
 
   bool port_enabled;
   nav2_util::declare_parameter_if_not_declared(
@@ -271,7 +272,6 @@ void EBC::cleanup()
     logger_,
     "Cleaning up module : %s of type scitos2_module::EBC",
     plugin_name_.c_str());
-  authority_->checkout();
   authority_.reset();
 }
 
@@ -281,6 +281,7 @@ void EBC::activate()
     logger_,
     "Activating module : %s of type scitos2_module::EBC",
     plugin_name_.c_str());
+  authority_->start();
 }
 
 void EBC::deactivate()
@@ -289,6 +290,7 @@ void EBC::deactivate()
     logger_,
     "Deactivating module : %s of type scitos2_module::EBC",
     plugin_name_.c_str());
+  authority_->checkout();
 }
 
 rcl_interfaces::msg::SetParametersResult EBC::dynamicParametersCallback(