Moveit: "Failed to fetch current robot state" when running getCurrentPose

[umhan35]

Description

I want to get the pose of the UR5 end effecor using moveit's method getCurrentPose, but moveit complains Failed to fetch current robot state even though rostopic echo -n 1 /joint_states outputs

header: 
  seq: 134400
  stamp: 
    secs: 2688
    nsecs: 661000000
  frame_id: ''
name: [elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint,
  wrist_3_joint]
position: [1.979917679317822e-05, 0.0037068883383053475, 0.0011235393977022667, 0.00013686250147504353, -1.039200540375873e-05, 9.013147737491067e-06]
velocity: [4.042045701190683e-05, 0.010381767871009435, -0.0032060928323904194, 0.0002780139965624078, 1.1067063295372274e-05, 1.913505209847656e-05]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Here is the code (see how to reproduce for more detail):

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv) {

    ros::init(argc, argv, "a2");

    ros::NodeHandle node;

    moveit::planning_interface::MoveGroupInterface ur5("manipulator");

    std::cout << ur5.getCurrentPose();

//    ros::spin();

    ros::MultiThreadedSpinner spinner(4);
    spinner.spin();

    return EXIT_SUCCESS;
}

moveit suggests Check clock synchronization if your are running ROS across multiple machines!, but I am sure this is not the cause:

rostopic echo -n 1 /joint_states; rostopic echo -n 1 /clock
header: 
  seq: 134400
  stamp: 
    secs: **2688**
    nsecs: 661000000
  frame_id: ''
name: [elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint,
  wrist_3_joint]
position: [1.979917679317822e-05, 0.0037068883383053475, 0.0011235393977022667, 0.00013686250147504353, -1.039200540375873e-05, 9.013147737491067e-06]
velocity: [4.042045701190683e-05, 0.010381767871009435, -0.0032060928323904194, 0.0002780139965624078, 1.1067063295372274e-05, 1.913505209847656e-05]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
clock: 
  secs: **2689**
  nsecs: 175000000
---

I post this issue here ros-industrial/universal_robot/issues/390, but they direct me here.

Your environment

• ROS Distro: Kinetic
• OS Version: Ubuntu 16.04
• Binary build
• If binary, which release version? 0.9.15-0xenial-20181029-110506-0800

Steps to reproduce

sudo apt-get install ros-kinetic-universal-robot

Assume you have a catkin_ws and an empty package, create a node with the following code. Add the moveit_ros_planning_interface package to CMakeLists.txt and add_executable(${PROJECT_NAME}_node src/a2_node.cpp).

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv) {

    ros::init(argc, argv, "a2");

    ros::NodeHandle node;

    moveit::planning_interface::MoveGroupInterface ur5("manipulator");

    std::cout << ur5.getCurrentPose();

//    ros::spin();

    ros::MultiThreadedSpinner spinner(4);
    spinner.spin();

    return EXIT_SUCCESS;
}

Expected behaviour

It should return a geometry_msgs::PoseStamped with the actual pose

Actual behaviour

It returns the default geometry_msgs::PoseStamped

header: 
  seq: 0
  stamp: 195.452000000
  frame_id: /world
pose: 
  position: 
    x: 0
    y: 0
    z: 0
  orientation: 
    x: 0
    y: 0
    z: 0
    w: 1

Backtrace or Console output

[ INFO] [1541342854.497090382]: Loading robot model 'ur5'...
[ WARN] [1541342854.497134691]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1541342854.497144828]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1541342854.532884977]: Loading robot model 'ur5'...
[ WARN] [1541342854.532905147]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1541342854.532914593]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1541342855.439346859, 194.448000000]: 
Deprecation warning: Trajectory execution service is deprecated (was replaced by an action).
Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch
[ INFO] [1541342855.441354394, 194.450000000]: Ready to take commands for planning group manipulator.
[ INFO] [1541342856.443343975, 195.452000000]: Didn't received robot state (joint angles) with recent timestamp within 1 seconds.
[ERROR] [1541342856.443375090, 195.452000000]: Failed to fetch current robot stateCheck clock synchronization if your are running ROS across multiple machines!

header: 
  seq: 0
  stamp: 195.452000000
  frame_id: /world
pose: 
  position: 
    x: 0
    y: 0
    z: 0
  orientation: 
    x: 0
    y: 0
    z: 0
    w: 1

[welcome[bot]]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[umhan35]

Solved by ros::AsyncSpinner spinner(1); spinner.start();

[Kr1sYe]

Solved by ros::AsyncSpinner spinner(1); spinner.start();

[rhaschke]

Just as an explanation: MoveGroupInterface::getCurrentPose() relies on ROS spinning being active.
Hence, the spinning needs to be started _before_ this call. The only standard way to accomplish this (without blocking) is to use an AsyncSpinner. There is no issue in using more than one thread for the asynchronous spinner - despite the fact that message processing order is not guaranteed anymore.

[Hubert51]

How can we set this flag in python?

[rhaschke]

How can we set this flag in python?

Nothing. For me, the following script just works. Do you observe issues? If so, please provide a minimal example.

import sys
import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize

rospy.init_node('foo', anonymous=False)
roscpp_initialize(sys.argv)

group = MoveGroupCommander('panda_arm')
print group.get_current_pose()
print group.get_current_joint_values()

[Hubert51]

I figured out what's going on. For baxter, we need to remap one rostopic. I remap it in the launch file but it seems not work. I write them inside scripts again.

Before: only remap in the launch file
<remap from="joint_states" to="robot/joint_states"/>

After: remap in launch file and scripts
<remap from="joint_states" to="robot/joint_states"/>
and
```#!/usr/bin/env python
import sys
import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize
import moveit_commander

remap again

joint_state_topic = ['joint_states:=/robot/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.init_node('foo', anonymous=False)
roscpp_initialize(sys.argv)

right_arm = MoveGroupCommander('right_arm')
print right_arm.get_current_pose()
```

Do you have idea why I need remap inside scripts again? @rhaschke
Thank you

[rhaschke]

Remapping needs to be done for each and every ROS node individually.
In this case, the script is running as a separate node and thus needs remapping.
The key question is why Baxter runs within the non-standard prefix robot in the first place...

[TomasMerva]

Hi, I am using ROS Indigo and I have had still problem with the getCurrentPose function. However, I ve used aforementioned AsyncSpinner, I have been still getting an error: Failed to fetch current robot state

geometry_msgs::PoseStamped actual_pose;
while(ros::ok()){
std::cin.get();
actual_pose = group.getCurrentPose();
std::cout << "position.x: " << actual_pose.pose.position.x <<'\n';
}