0
$\begingroup$

Rosanswers logo

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


Originally posted by fadi eid on ROS Answers with karma: 15 on 2023-07-06

Post score: 1

$\endgroup$

1 Answer 1

1
$\begingroup$

Rosanswers logo

  • 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


Originally posted by Gaurav Gupta with karma: 276 on 2023-07-19

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.