2
$\begingroup$

I am working on a simulation environment that uses ROS2 to execute different components and sensors. We currently launch each node with a manager that uses fork/exec. The issue I was having was sending ctrl-c to the manager node would not shut down all other nodes reliably. It seemed that I was maintaining pid of the ROS executable created but not the actual node.

This led me to want to use lifecycle nodes for managing the state of each node. This would also give us benefits for restarting the simulation environment. So I upgraded one of our nodes to a lifecycle node. I also took this time to upgrade the node to a component in order to upgrade our manager.

We used a service call to get a nodes configuration from a configuration node. The configuration node is not part of the composition node as it is written in python currently. The issue is that spin_until_future_complete errors out because the node has already been added to an executor. I would like this service call to complete before the on_configure callback returns. The executor is multithreaded and I created a callback group for the client. The expected results are that the request will be processed while the on_configure callback waits for the future. This caused a deadlock in the program.

So I have tried a few different ideas. I tried working with the future but it would never return. So I moved to a callback to confirm the service was working. This still never entered the callback. I also confirmed the service server is working with CLI tool.

Below is code for my component (InsNode) and my manager (NodeComposer). I changed some variables and strings to sanitize company data

Edit: I changed the InsNode back to a classic node and not a component. I am getting the same behavior. The configuration server see the message but I never get it back into the callback

node_composer.cpp

#include "node_composer.h"

#include <fstream>

#include "common_pkg/json.hpp"

using json = nlohmann::json;

const std::string CONFIG_FILE_PREFIX = "vehicle_config_";
const std::string CONFIG_FILE_EXTENSION = ".json";

NodeComposer::NodeComposer(std::weak_ptr<rclcpp::Executor> executor, std::string node_name,
                           const rclcpp::NodeOptions& node_options)
    : ComponentManager(executor, node_name, node_options)
{
    _available_nodes = std::map<std::string, Node>();
    _node_configs = std::vector<rclcpp_components::ComponentManager::LoadNode::Request::SharedPtr>();
}

NodeComposer::~NodeComposer() {}

void NodeComposer::create_nodes()
{
    for (auto&& request : _node_configs)
    {
        std::shared_ptr<rmw_request_id_t> request_header = std::make_shared<rmw_request_id_t>();
        rclcpp_components::ComponentManager::LoadNode::Response::SharedPtr response =
            std::make_shared<rclcpp_components::ComponentManager::LoadNode::Response>();
        on_load_node(request_header, request, response);
        if (!response->success)
        {
            RCLCPP_ERROR(get_logger(), "Error loading %s", request->plugin_name.c_str());
        }
    }
}

void NodeComposer::loadNodeConfigurations()
{
    std::string root = Util::GetRoot();
    std::string config_folder = root + "/src/configs/active_sim";
    for (auto&& entry : std::filesystem::directory_iterator(config_folder))
    {
        std::string filename = std::string(entry.path().filename());
        std::string extension = std::string(entry.path().extension());
        if (filename.compare(0, CONFIG_FILE_PREFIX.length(), CONFIG_FILE_PREFIX) == 0 &&
            extension.compare(CONFIG_FILE_EXTENSION) == 0)
        {
            std::ifstream config_file(entry.path().c_str());
            json data = json::parse(config_file);
            config_file.close();

            for (auto&& entry : _available_nodes)
            {
                if (data.at(entry.first + "_enabled"))
                {
                    rclcpp_components::ComponentManager::LoadNode::Request::SharedPtr request =
                        std::make_shared<rclcpp_components::ComponentManager::LoadNode::Request>();
                    request->package_name = entry.second.package;
                    request->plugin_name = entry.second.executable;
                    rclcpp::Parameter vehicle_name("vehicle_name", "Boaty Mc'Boat Face");
                    request->parameters.push_back(vehicle_name.to_parameter_msg());
                    _node_configs.push_back(request);
                }
            }
        }
    }
}

void NodeComposer::loadAvailableNodes()
{
    std::string root = MnSCpp::Util::GetUxSRoot();
    std::string path = root + "/src/databases/components/components.json";
    std::ifstream nodes(path);
    json data = json::parse(nodes);
    nodes.close();

    for (auto&& entry : data["components"].items())
    {
        Node new_node = {entry.value()["package_name"].get<std::string>(),
                         entry.value()["executable"].get<std::string>()};
        _available_nodes.insert({entry.value()["config_prefix"].get<std::string>(), new_node});
    }
}

int main(int argc, char const* argv[])
{
    rclcpp::init(argc, argv);
    auto exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
    auto node = std::make_shared<NodeComposer>(exec, "node_composer", rclcpp::NodeOptions());
    exec->add_node(node);
    node->loadAvailableNodes();
    node->loadNodeConfigurations();
    node->create_nodes();
    exec->spin();
    return 0;
}

ins_node.cpp

#include "ins_node.h"

InsNode::InsNode(const rclcpp::NodeOptions& options) : LifecycleNode("ins_node", options)
{
    this->declare_parameter("vehicle_name", "r300");
    veh_name = this->get_parameter("vehicle_name").as_string();
    rclcpp::CallbackGroup::SharedPtr callback_group = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    vehicle_client = this->create_client<uxs_interfaces::srv::VehicleConfig>("vehicle_config", rmw_qos_profile_services_default, callback_group);

}

InsNode::CallbackReturn InsNode::on_configure(const rclcpp_lifecycle::State& state)
{
    LifecycleNode::on_configure(state);
    loadConfig();
    if (Interface && Interface->configure())
    {
        return CallbackReturn::SUCCESS;
    }
    return CallbackReturn::FAILURE;
}

void InsNode::on_config_service(rclcpp::Client<uxs_interfaces::srv::VehicleConfig>::SharedFuture response) {
    uxs_interfaces::srv::VehicleConfig_Response::SharedPtr vehicle_config_response;
    vehicle_config_response = response.get();

    // Call the INS Factory and return the correct INS object
    InsConfigType = vehicle_config_response->ins_type;
    Interface = InsFactory::instance()->createIns(InsConfigType, this, vehicle_config_response);

    // Read script mode status from the config file 
    script_mode = vehicle_config_response->ins_enable_script_mode;
    if (script_mode)
    {
        RCLCPP_INFO(this->get_logger(), "Script mode is activated.");
        setScriptMode();
    }

    // Read power reporting status from the config file 
    power_usage = vehicle_config_response->ins_power_usage;
}

void InsNode::loadConfig(void)
{
    // set up service requests
    RCLCPP_INFO(this->get_logger(), "INS is loading vehicle config JSON.");
    auto vehicle_request = std::make_shared<uxs_interfaces::srv::VehicleConfig::Request>();
    vehicle_request->request_name = veh_name;
    
    // Request vehicle config data ---------------------------------------------------------------
    // Gives one second to wait for the server, if ctrl+c pressed the loop will exit
    while (!vehicle_client->wait_for_service(std::chrono::duration<int>(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for vehicle_config service. Exiting.");
        }
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "vehicle_config service not available, waiting again...");
    }

    // send request for data and wait for the result
    auto vehicle_result = vehicle_client->async_send_request(vehicle_request, std::bind(&InsNode::on_config_service, this, std::placeholders::_1));
    
}
$\endgroup$

1 Answer 1

1
$\begingroup$

I was able to get this working. The issue was creating the callback group in the header. The callback group needs to persist in the class state. The shared pointer is not copied by the service creation so the callback group was being lost when the constructor was out of scope. Which would set the callback group to void and that is the default callback group.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.