Deal with ROS2 service & executors to get current robot states

[amadeuszsz]

I am confused about provide ROS2 service with executors (as it mentioned in moveit2 demo).

I already finished my service, unfortunately it works incorrectly. I mean, it is possible to move real robot, but I couldn't get current states, It works proper in demo mentioned above, so it exclude malfunctions on hardware side.

#include <moveit/move_group_interface/move_group_interface.h>
#include <rviz_visual_tools/rviz_visual_tools.hpp>
#include "custom_msgs/srv/move_arm.hpp"

class MoveRobot : public rclcpp::Node {
public:
    explicit MoveRobot(const rclcpp::NodeOptions &options)
            : Node("move_arm", options)
            , move_group_(static_cast<const SharedPtr>(this), "ur_manipulator")
    {
        parameters_client_ = std::make_shared<rclcpp::SyncParametersClient>(this, "move_group");
        service_ = this->create_service<custom_msgs::srv::MoveArm>(
                "move_arm_service", std::bind(&MoveRobot::move, this,
                                          std::placeholders::_1, std::placeholders::_2)
        );
    }

private:
    rclcpp::SyncParametersClient::SharedPtr parameters_client_;
    rclcpp::Service<custom_msgs::srv::MoveArm>::SharedPtr service_;
    moveit::planning_interface::MoveGroupInterface move_group_;

    void move(const std::shared_ptr<custom_msgs::srv::MoveArm::Request> request,
                     std::shared_ptr<custom_msgs::srv::MoveArm::Response> response)
    {
        auto pose = move_group_.getCurrentPose().pose;
        RCLCPP_INFO(this->get_logger(), "x: %f, y: %f, z: %f", pose.position.x, pose.position.y, pose.position.z);
        pose.position.x += request->x;
        pose.position.y += request->y;
        pose.position.z += request->z;
        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        bool success = (move_group_.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        if (success) RCLCPP_INFO(this->get_logger(), "\nPlanning finished successfully.");
        else RCLCPP_INFO(this->get_logger(), "\nPlanning failed.");
        move_group_.move();
        RCLCPP_INFO(this->get_logger(), "\nResponse: %d", response->success);
    }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    auto move_group_node = std::make_shared<MoveRobot>(node_options);
    rclcpp::spin(move_group_node);
    // same result using executor
    // rclcpp::executors::SingleThreadedExecutor executor;
    // executor.add_node(move_group_node);
    // executor.spin();
    rclcpp::shutdown();
    return 0;
}

I am able to execute move_group_.move() while calling service, but move_group_.getCurrentPose() returns zeros and some errors:

Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1623665018.810613138] [move_group_interface]: Failed to fetch current robot state

I guess it may be necessary to use executors, but I couldn't find any documentations, examples around it. I tried to create executor in initialization, directly in service function, main function and so on but usually it kills node immediately, in best scenario nothing changes. Is it expected behavior to make robot states unavailable with spin() but robot move execution possible? Is it necessary to use executors? How to use executor within services?

[YuLiHN]

I have the same problem too. Have you solved it?

[ozangungor12]

Hello @amadeuszsz,
I am facing the exact same problem. Could you share your solution or your source code if you have found a solution.
Thanks a lot!

[amadeuszsz]

@YuLiHN @ozangungortuhh I'm a little bit busy to validate it if it works now, but here is the code of my old project how I made it works:

#ifndef YOUR_PACKAGE_YOUR_HEADER_H
#define YOUR_PACKAGE_YOUR_HEADER_H

#include <moveit/move_group_interface/move_group_interface.h>
// INCLUDE YOUR SRVS 


class MoveRobot : public rclcpp::Node {
public:
    explicit MoveRobot(const rclcpp::NodeOptions &options);

private:
    rclcpp::Service<your_service_msgs::srv::ExampleService>::SharedPtr service_example_server_;
    std::string node_namespace_;
    moveit::planning_interface::MoveGroupInterfacePtr move_group_;
    rclcpp::Node::SharedPtr node_;
    rclcpp::Executor::SharedPtr executor_;
    std::thread executor_thread_;

    void service_example_callback(const std::shared_ptr<your_service_msgs::srv::ExampleService::Request> request,
                                  std::shared_ptr<your_service_msgs::srv::ExampleService::Response> response);
};

#endif //YOUR_PACKAGE_YOUR_HEADER_H

#include "your_package/your_header.h"


static const double PLANNING_TIME_S = 5.0;
static const double MAX_VELOCITY_SCALE = 1.0;
static const double MAX_ACCELERATION_SCALE = 1.0;
static const unsigned int PLANNING_ATTEMPTS = 5;
static const double GOAL_TOLERANCE = 1e-3;


MoveRobot::MoveRobot(const rclcpp::NodeOptions &options)
        : Node("example_services_node", options), node_(std::make_shared<rclcpp::Node>("example_group_node")),
          executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>()) {
    node_namespace_ = ((std::string) this->get_namespace()).erase(0, 1);
    
    service_example_server_ = this->create_service<your_service_msgs::srv::ExampleService>(
            "~/example_service",
            std::bind(&MoveRobot::service_example_callback, this, std::placeholders::_1, std::placeholders::_2)
    );

    auto mgi_options = moveit::planning_interface::MoveGroupInterface::Options(
            node_namespace_ + "_ur_manipulator",
            "/" + node_namespace_,
            "robot_description");
    move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, mgi_options);
    //move_group_->setPoseReferenceFrame("base_link_inertia");
    move_group_->setPlanningTime(PLANNING_TIME_S);
    move_group_->setNumPlanningAttempts(PLANNING_ATTEMPTS);
    move_group_->setGoalTolerance(GOAL_TOLERANCE);
    move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE);
    move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE);
    executor_->add_node(node_);
    executor_thread_ = std::thread([this]() { this->executor_->spin(); });
}

void MoveRobot::service_example_callback(const std::shared_ptr<your_service_msgs::srv::ExampleService::Request> request,
                                         std::shared_ptr<your_service_msgs::srv::ExampleService::Response> response) {
    move_group_->setStartStateToCurrentState();
    geometry_msgs::msg::Pose pose_goal = move_group_->getCurrentPose().pose;
    pose_goal.position.x += request->transform.translation.x;
    pose_goal.position.y += request->transform.translation.y;
    pose_goal.position.z += request->transform.translation.z;
    tf2::Quaternion q_orig(pose_goal.orientation.x, pose_goal.orientation.y, pose_goal.orientation.z,
                           pose_goal.orientation.w);
    tf2::Quaternion q_rot(request->transform.rotation.x, request->transform.rotation.y, request->transform.rotation.z,
                          request->transform.rotation.w);
    tf2::Quaternion q_new;
    q_new = q_rot * q_orig;
    q_new.normalize();
    pose_goal.orientation.x = q_new.x();
    pose_goal.orientation.y = q_new.y();
    pose_goal.orientation.z = q_new.z();
    pose_goal.orientation.w = q_new.w();
    move_group_->setPoseTarget(pose_goal);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    RCLCPP_INFO(this->get_logger(), "Planned position x: %f, y: %f, z: %f", pose_goal.position.x, pose_goal.position.y,
                pose_goal.position.z);
    bool success = (move_group_->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    response->success = success;
    move_group_->move();
}

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    auto move_group_node = std::make_shared<MoveRobot>(node_options);
    rclcpp::spin(move_group_node);

    rclcpp::shutdown();
    return 0;
}

AFAIK ppl struggle with same issue. Later, in some spare time I will prepare demo repo.

[ozangungor12]

Thank you so much, it worked for me! @amadeuszsz
Though I find it difficult to understand why we need two initialize our node two times?
Once with auto move_group_node = std::make_shared<MoveRobot>(node_options); and also a class member rclcpp::Node::SharedPtr node_;
Thanks again for your help!

[amadeuszsz]

Thank you so much, it worked for me! @amadeuszsz Though I find it difficult to understand why we need two initialize our node two times? Once with auto move_group_node = std::make_shared<MoveRobot>(node_options); and also a class member rclcpp::Node::SharedPtr node_; Thanks again for your help!

To prevent deadlock. With current knowledge I guess I will solve it a little bit different but until it works you can stay with this approach. I suggest you to look up for ros2 deadlock and ros2 executors to understand it better.