ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

How to extract position of the gripper in ROS2/MoveIt2

asked 2023-07-06 01:33:51 -0600

fadi eid gravatar image

updated 2023-07-06 01:37:31 -0600

I created a URDF for my custom robot and gone through MoveIt Setup Assistant successfully generating RVIZ launch files for ROS2/Moveit2 Humble distro. I can drag the interactive marker and plan/execute the final end effector position. I want to be able to extract the current end effector position and orientation so I can know where the gripper is in respect to the base of the arm (Forward kinematics). Any way I can do it in a simple way? Any guidance would be great.

My workspace is similar to this as per the packages and functionalities: link text

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2023-07-19 12:50:36 -0600

  • 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

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2023-07-06 01:33:51 -0600

Seen: 323 times

Last updated: Jul 19 '23