How to extract position of the gripper in ROS2/MoveIt2

edit

• To get the end effector's pose information, you need to use the Move Group Interface.
  This is done by using the getCurrentPose() function provided by the said interface. However, you cannot access this function directly, as this requires a prior thread to be running to retrieve current state information.

• You need to spin up a SingleThreadExecutor first so that the robot joint states can be relayed onto the current state monitor. This is done as follows:

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  std::thread spinner = std::thread([&executor]() { executor.spin(); });
  

  Then, we can access the getCurrentPose() function. Failure to do so will result in the following error message on your console:

  [INFO] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time, but latest received state has time 0.000000.
  Check clock synchronization if your are running ROS across multiple machines!
  [ERROR] [move_group_interface]: Failed to fetch current robot state
  

• The following is adjusted according to the codebase attached in the question's link:

  /**
  * Program to print end-effector pose
  */
  
  #include <moveit/move_group_interface/move_group_interface.h>
  
  #include <geometry_msgs/msg/pose.hpp>
  #include <rclcpp/rclcpp.hpp>
  
  int main(int argc, char* argv[]) {
    // Initialize ROS
    rclcpp::init(argc, argv);
  
    // Declare Node
    std::shared_ptr<rclcpp::Node> node =
      std::make_shared<rclcpp::Node>("get_eef_pose",
        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(
          true));
  
    // We spin up a SingleThreadedExecutor for the current state monitor to get
    // information about the robot's state.
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    std::thread spinner = std::thread([&executor]() { executor.spin(); });
  
    // Create the MoveIt MoveGroup Interface
    moveit::planning_interface::MoveGroupInterface move_group_interface =
      moveit::planning_interface::MoveGroupInterface(node, "arm");
  
    // print current pose
    geometry_msgs::msg::Pose current_pose =
      move_group_interface.getCurrentPose().pose;
  
    // Print the current pose of the end effector
    RCLCPP_INFO(node->get_logger(), "Current pose: %f %f %f %f %f %f %f",
      current_pose.position.x,
      current_pose.position.y,
      current_pose.position.z,
      current_pose.orientation.x,
      current_pose.orientation.y,
      current_pose.orientation.z,
      current_pose.orientation.w);
  
    // Shutdown ROS
    rclcpp::shutdown();
    spinner.join();
    return 0;
  }
  

References

• MoveIt2! Tutorials Hello MoveIt