Cartesian Task does not go REACHING immediately after setPoseTarget() (at least in python)

[torydebra]

Hi,

I just discovered that the getTaskState() does not return REACHING immediately after the setPoseTarget, but it takes a while to do it.
This cause me issues when I am not waiting for reach completed (waitReachCompleted) but running continuosly the node as in the cpp tutorial

I am using python, I do not know if this is a pythonic issue, or happens also with cpp api. Or am I missing some method to call?

Just simple node to replicate (note that I was just trying to updating multiple times the ci):

#!/usr/bin/env python3

import rospy
import numpy as np

from cartesian_interface.pyci_all import *

class Executor:
    def __init__(self):


        self._cartesio_cli = pyci.CartesianInterfaceRos(namespace="cartesian")
        self._cartesio_cli.update()
        self._right_arm_task = self._cartesio_cli.getTask('right_arm')
        self._waypoints = []

        actual_pose, _, _ = self._right_arm_task.getPoseReference() # just return the pose ref, skip vel & acc
        wp1 = pyci.WayPoint()
        wp1.frame.linear = actual_pose.linear
        wp1.frame.translation = [0.667, -0.245, -0.21] #-0.230 wrt to homing_demo is sort of min height
        wp1.time = 1
        self._waypoints.append(wp1)

    def run(self):

        print ("Before", self._right_arm_task.getTaskState())
        self._cartesio_cli.update()
        self._right_arm_task.setPoseTarget(self._waypoints[0].frame, 8)
        self._cartesio_cli.update()
        print ("After", self._right_arm_task.getTaskState())
        print ("After 2", self._right_arm_task.getTaskState())
        print(" ")

        self._right_arm_task.waitReachCompleted(20.0)


if __name__ == '__main__':

    rospy.init_node('bug', anonymous=True)
    executor = Executor()

    rate = rospy.Rate(100)
    while not rospy.is_shutdown():

        executor.run()
        rate.sleep()

output:

Before State.Online
After State.Online
[ INFO] [1708965606.140478865]: Reach action for task 'right_arm' has become active
After 2 State.Reaching

---

@alaurenzi @liesrock

[torydebra]

I solved with an addional state in my state machine which checks for the task to go REACHING after sending the command