OneOneLiu
commented
3 weeks ago I managed to resolve the issue based on this comment. The solution involved setting up a separate executor within the class for spinning the MoveGroup interface, while keeping the main program's node running in a separate spin. This allowed me to fetch the robot's current state successfully from a service request.
However, I'm still somewhat confused about why this resolves the issue. I'll continue digging into the ROS 2 internals to better understand what's going on. In the meantime, here's the updated version of the code that works:
#include <rclcpp/rclcpp.hpp>
#include "test_joint_state_fetch/srv/print_pose.hpp" // Header file for the custom service
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>
// Combined RobotMover class, which includes service and pose printing functionality
class RobotMover : public rclcpp::Node {
public:
// Constructor to initialize the node, MoveGroupInterface, and executor
RobotMover(const rclcpp::NodeOptions &options)
: rclcpp::Node("robot_control", options), // Initialize the node with the name "robot_control"
node_(std::make_shared<rclcpp::Node>("move_group_interface")), // Create an additional ROS node
move_group_interface_(node_, "panda_arm"), // Initialize MoveGroupInterface for controlling the arm
executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>()) // Create a single-threaded executor
{
// Create the service for printing the current pose
service_ = this->create_service<test_joint_state_fetch::srv::PrintPose>(
"print_current_pose",
std::bind(&RobotMover::handlePrintRequest, this, std::placeholders::_1, std::placeholders::_2)
);
// Add the node to the executor and start the executor thread
executor_->add_node(node_);
executor_thread_ = std::thread([this]() {
RCLCPP_INFO(node_->get_logger(), "Starting executor thread"); // Log message indicating the thread start
executor_->spin(); // Run the executor to process callbacks
});
}
// Function to print the current end-effector pose
void printCurrentPose() {
auto current_pose = move_group_interface_.getCurrentPose().pose; // Get the current pose
std::cout << "Current Pose:" << std::endl;
std::cout << "Position: (" << current_pose.position.x << ", "
<< current_pose.position.y << ", "
<< current_pose.position.z << ")" << std::endl;
std::cout << "Orientation: (" << current_pose.orientation.x << ", "
<< current_pose.orientation.y << ", "
<< current_pose.orientation.z << ", "
<< current_pose.orientation.w << ")" << std::endl;
}
private:
// Service callback function to handle pose printing requests
void handlePrintRequest(const std::shared_ptr<test_joint_state_fetch::srv::PrintPose::Request> /*request*/,
std::shared_ptr<test_joint_state_fetch::srv::PrintPose::Response> response) {
// Print the current pose in the service callback
RCLCPP_INFO(node_->get_logger(), "Service Callback: About to call printCurrentPose in Private");
printCurrentPose(); // Print the robot's current pose
// Set the response to indicate success
response->success = true;
}
// Member variables
rclcpp::Node::SharedPtr node_; // Additional ROS node pointer
moveit::planning_interface::MoveGroupInterface move_group_interface_; // MoveIt interface for controlling the arm
rclcpp::Service<test_joint_state_fetch::srv::PrintPose>::SharedPtr service_; // Service pointer for pose requests
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_; // Single-threaded executor
std::thread executor_thread_; // Thread to run the executor
};
// Main function - Entry point of the program
int main(int argc, char** argv) {
rclcpp::init(argc, argv); // Initialize ROS 2
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true); // Allow automatic parameter declaration
node_options.use_intra_process_comms(false); // Disable intra-process communication
auto node = std::make_shared<RobotMover>(node_options); // Create the RobotMover object and start the node
rclcpp::spin(node); // Spin the main thread to process callbacks
rclcpp::shutdown(); // Shutdown the ROS 2 system
return 0; // Exit the program
}
Description
I'm experiencing inconsistent behavior when attempting to fetch the current robot state using MoveIt in ROS2 Humble on Ubuntu 22.04. Specifically, the robot state fetching succeeded in the main function but failed with timestamp errors when called in a service class in the same program, despite the received time being valid (within the 1-second window).
Your environment
HumbleUbuntu 22.04inDockerMoveit2build from sourcefaf42b0Issue Details
I have a class
RobotMoverthat uses MoveIt'smove_group_interface_.getCurrentPose().poseto print the current robot pose (sourcecpp file is provided below). I'm calling theprintCurrentPose()method in three distinct contexts in the same program:RobotMoverServiceclass, just after setting up the service.RobotMoverandRobotMoverServiceinstances.ros2 service call.Each call yields different results, with two of the three calls failing to fetch the current robot state.
Public Context (Failed, and the received time is 0.000):
Main Context (direct call from main, successful):
The third one is most confusing to me, it said it didn't receive the robot state within 1 second, but the requested time and received time difference is obviously less than 1 sec.
I have checked existing issues and find some might be relevant, https://github.com/moveit/moveit2/issues/775, https://github.com/moveit/moveit2_tutorials/issues/587, https://github.com/moveit/moveit2/issues/496, the most relevant one is this issue https://github.com/moveit/moveit2/issues/1399, as the author mentioned:
Question:
Does anyone know how to resolve this issue, or am I doing something wrong in my implementation?
Thanks in advance!
Steps to reproduce
clone the repo to
~/humble_ws/src/and build the imageLaunch
moveit2_tutorial/demo.launchin the containerIn a new terminal, run the test node
In a new terminal, call the
/print_current_poseserviceExpected behaviour
All the three calls can fetch and print the current robot states.
Console output
Source file