diff --git a/scitos2_core/include/scitos2_core/module.hpp b/scitos2_core/include/scitos2_core/module.hpp index faa0ecb..90ce313 100644 --- a/scitos2_core/include/scitos2_core/module.hpp +++ b/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 rpc = sharedAuthority->callService("/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 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 rpc = sharedAuthority->callService( "/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 rpc = sharedAuthority->callService( "/robot/Robot#builtin", std::string("getProperty"), param_name); diff --git a/scitos2_mira/include/scitos2_mira/mira_framework.hpp b/scitos2_mira/include/scitos2_mira/mira_framework.hpp index 91a1f9e..1de7016 100644 --- a/scitos2_mira/include/scitos2_mira/mira_framework.hpp +++ b/scitos2_mira/include/scitos2_mira/mira_framework.hpp @@ -109,6 +109,7 @@ class MiraFramework : public nav2_util::LifecycleNode // MIRA framework std::unique_ptr framework_; + bool loaded_; // Modules Plugins pluginlib::ClassLoader module_loader_; diff --git a/scitos2_mira/src/mira_framework.cpp b/scitos2_mira/src/mira_framework.cpp index 0b3f246..f156c55 100644 --- a/scitos2_mira/src/mira_framework.cpp +++ b/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(); diff --git a/scitos2_mira/test/test_mira_framework.cpp b/scitos2_mira/test/test_mira_framework.cpp index f080def..a51ea4d 100644 --- a/scitos2_mira/test/test_mira_framework.cpp +++ b/scitos2_mira/test/test_mira_framework.cpp @@ -32,11 +32,19 @@ TEST(ScitosMiraFrameworkTest, configure) { // Create the node auto node = std::make_shared(); - // 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) diff --git a/scitos2_modules/include/scitos2_modules/drive.hpp b/scitos2_modules/include/scitos2_modules/drive.hpp index 2248e1a..e311fac 100644 --- a/scitos2_modules/include/scitos2_modules/drive.hpp +++ b/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_; diff --git a/scitos2_modules/src/charger.cpp b/scitos2_modules/src/charger.cpp index 1ec13f1..051e844 100644 --- a/scitos2_modules/src/charger.cpp +++ b/scitos2_modules/src/charger.cpp @@ -35,7 +35,8 @@ void Charger::configure( plugin_name_ = name; logger_ = node->get_logger(); - authority_ = std::make_shared("/", plugin_name_); + authority_ = std::make_shared(); + authority_->checkin("/", plugin_name_); // Create ROS publishers battery_pub_ = node->create_publisher("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(); } diff --git a/scitos2_modules/src/display.cpp b/scitos2_modules/src/display.cpp index 283e9b0..fb0c605 100644 --- a/scitos2_modules/src/display.cpp +++ b/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("/", name); + authority_ = std::make_shared(); + authority_->checkin("/", plugin_name_); // Create publisher display_data_pub_ = node->create_publisher("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(); } diff --git a/scitos2_modules/src/drive.cpp b/scitos2_modules/src/drive.cpp index cddf182..3c4d6c0 100644 --- a/scitos2_modules/src/drive.cpp +++ b/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("/", name); + authority_ = std::make_shared(); + 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 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(speed)); } diff --git a/scitos2_modules/src/ebc.cpp b/scitos2_modules/src/ebc.cpp index 770d98f..1ac45fa 100644 --- a/scitos2_modules/src/ebc.cpp +++ b/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("/", plugin_name_); + authority_ = std::make_shared(); + 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(