How to call srv in class method?

asked 2023-05-15 11:16:56 -0500

sterlingm gravatar image

updated 2023-05-15 11:26:45 -0500

I have a path planning class (called Planner) that I want to be a client for a srv node that will modify paths (called PathMod). I'm trying to set up the communication in a method in Planner, so the method will build a new request, send the request, wait for the result, and then get the result (and do some computation with it). I would like for this to be intra-process so that I have very fast communication.

The issue I'm having is that I think I need to have access to the process' executor in the Planner class in order to spin it to get the result from the srv, but I get "use of deleted function" errors when I try to use the copy constructor or assignment operator.

My code is below.

Main.cpp

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include "path_mod.hpp"
#include "planner.hpp"

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::executors::SingleThreadedExecutor exec;
    rclcpp::NodeOptions options;

    auto pathModSrv = std::make_shared<PathMod>(options);
    exec.add_node(pathModSrv);

    auto plannerCli = std::make_shared<Planner>(options, exec);
    exec.add_node(plannerCli);

    exec.spin();

    rclcpp::shutdown(); 

    std::cout<<"\nExiting normally\n";
    return 0;
}

Planner.cpp

#include "planner.hpp"
using namespace std::chrono_literals;

Planner::Planner(const rclcpp::NodeOptions& options, const rclcpp::executors::SingleThreadedExecutor& ex) : Node("Planner", options), exec(ex) {
    pathModClient_ = create_client<ramp_interfaces::srv::PathModify>("path_modify");
    timer_ = create_wall_timer(2s, std::bind(&Planner::on_timer, this));
}

void Planner::on_timer() {    
    auto req = std::make_shared<ramp_interfaces::srv::PathModify::Request>();

    // Populate request with data

    rclcpp::Client<ramp_interfaces::srv::PathModify>::SharedFuture future_result = pathModClient_->async_send_request(req).future.share();

    // Wait for the result.
    if (exec.spin_until_future_complete(future_result) ==
        rclcpp::FutureReturnCode::SUCCESS)
    {
        ramp_interfaces::srv::PathModify::Response::SharedPtr res = future_result.get();
        // Do stuff with the data
    } else {
        RCLCPP_ERROR(get_logger(), "Failed to call service path_mod");
    }
}


#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(Planner)

Is there a way to store the executor for a process as a class field? If not, then what kind of alternatives should I pursue? I'm converting my codebase from ROS1 to ROS2 where I had a central Planner node that communicated with several service nodes to maintain the state of a Planner class.

I'm using Humble and Ubuntu 22.04. I pasted the gcc error below.

error: use of deleted function ‘rclcpp::executors::SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executors::SingleThreadedExecutor&)’
    4 | Planner::Planner(const rclcpp::NodeOptions& options, const rclcpp::executors::SingleThreadedExecutor& ex) : Node("Planner", options), exec(ex) {
      |                                                                                                                                       ^~~~~~~~
In file included from /opt/ros/humble/include/rclcpp/rclcpp/context.hpp:34,
                 from /opt/ros/humble/include/rclcpp/rclcpp/executor.hpp:34,
                 from /opt/ros/humble/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp:25,
                 from /opt/ros/humble/include/rclcpp/rclcpp/executors.hpp:21,
                 from /opt/ros/humble/include/rclcpp/rclcpp/rclcpp.hpp:155,
                 from /home/sterlingm/ros_ws/src/ramp_nav/include/planner.hpp:4,
                 from /home/sterlingm/ros_ws/src/ramp_nav/src/planner.cpp:1:
/opt/ros/humble ...
(more)
edit retag flag offensive close merge delete