get_current_pose() returns a wrong pose at first call

[karl-exwzd]

Description

When calling get_current_pose(), via moveit_commander, to get the robot pose, as to modify that for an offset Cartesian move, it returns a wrong pose the first time. Subsequent calls return the correct position.

This started happening after the upgrade from ROS Melodic to Noetic.

This results in collisions and unexpected moves.

Current dirty fix is to call get_current_pose() twice every time, fixing the first call issue.

Your environment

• ROS Distro: Noetic
• OS Version: e.g. Ubuntu 20.04
• Binary build
• 1.1.4

Steps to reproduce

Call get_current_pose(), get an offset pose to the expected one.

Expected behaviour

The function should return the correct pose from the first call.

Actual behaviour

The first call, the pose is offset, subsequent calls return the correct current pose.

Backtrace or Console output

This is the output of the following code:

current_pose = self.__move_group.get_current_pose()
rospy.loginfo(current_pose)
current_pose = self.__move_group.get_current_pose()
rospy.loginfo(current_pose)

Note that this is only valid when first executed, all subsequent calls return the correct pose.

[INFO] [1623032619.848195]: Planning frame: world
[INFO] [1623032619.849132]: EEF link name: tip_link
[INFO] [1623032619.982615]: header: 
  seq: 0
  stamp: 
    secs: 1623032619
    nsecs: 982285261
  frame_id: "world"
pose: 
  position: 
    x: 0.5139389149711554
    y: 0.008799999618883054
    z: 0.44463802606351355
  orientation: 
    x: -0.707031398843508
    y: -0.7070314014031805
    z: 0.010324690227373302
    w: 0.010324691324984188
[INFO] [1623032619.992498]: header: 
  seq: 0
  stamp: 
    secs: 1623032619
    nsecs: 992206096
  frame_id: "world"
pose: 
  position: 
    x: 0.2681428171043065
    y: -0.05534326417136634
    z: 0.19578530061293076
  orientation: 
    x: 0.5747665365843956
    y: 0.8182461580531967
    z: 0.010797937861542596
    w: 0.00024039946008543294

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[felixvd]

I think you're encountering the issue I am attempting to fix in https://github.com/ros-planning/moveit/pull/2661 .

Try calling startStateMonitor() after you create your MoveGroupInterface and see if that fixes it. If there is no Python binding for this function, just call get_current_pose() one time instead.

[karl-exwzd]

@felixvd Thanks for the quick reply.

Currently it doesn't seem there is a Python binder (move_group).

Calling get_current_pose() after creating the MoveGroupCommander instance fixes the issue as well.

[felixvd]

Great. I'll leave this open until the PR is merged.

[ravijo]

@karl-exwzd

It seems possible to call startStateMonitor() by getting access to MoveGroupInterface. It is a dirty way though.

self.__move_group._g.start_state_monitor(1.0)  # wait = 1.0

The above line runs perfectly in ROS Melodic.

[cambel]

This bug is still relevant...

[rhaschke]

I cannot confirm this issue on ROS One. Could anyone provide detailed instructions to reproduce the issue (launch command, python script, ...)

I tried: roslaunch panda_moveit_config demo.launch and this basic python script:

import rospy

from moveit_commander import RobotCommander

rospy.init_node("test")
commander = RobotCommander("robot_description")
group = commander.get_group("panda_manipulator")

p1 = group.get_current_pose().pose
p2 = group.get_current_pose().pose
print(p1)
print(p2)

assert(p1 == p2)