How to call srv in class method?
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/include/rclcpp/rclcpp/executors/single_threaded_executor.hpp:69:3: note: declared here
69 | RCLCPP_DISABLE_COPY(SingleThreadedExecutor)
| ^~~~~~~~~~~~~~~~~~~
Asked by sterlingm on 2023-05-15 11:16:56 UTC
Comments