[ROS Client] waitReachCompleted loops indefinitely

[liesrock]

The above issue is linked with this https://github.com/ADVRHumanoids/perception_thesis/issues/13. To briefly recap, the problem is related to the fact that the waitReachCompleted function called after a setTergetPose (on the same ee) from the ROS client side does not return, even if the ROS CartesianInterface server accepts and successfully completes the requested goal.

The debug carried out is linked with the actionlib provided by ROS and used inside the CartesianInterface: the underlying mechanisms through which action clients & servers interact with one another can be found here http://wiki.ros.org/actionlib/DetailedDescription.

I noticed that using CartesianInterface 2.0-devel package the whole state machine from the client side was not executed at all: this leads to the above infinite waiting inside the waitReachCompleted function. On the server side instead all the mechanism works as expected.

The only way to force the return of the waitReachCompleted function was through the usage of a timeout different from 0 (which is the "hack" I suggest to use meanwhile the new binary release of CartesianInterface will be ready).

Given this I went back to CartesianInterface version 1 inside the bionic_compatibility branch and I noticed that the issue was not appearing, instead the whole client side ROS action mechanism was working as expected, especially the handleTransition function inside the simple action client was getting called (https://github.com/ros/actionlib/blob/9beedb8307a181d63cdeedab389712b229de0b4f/actionlib/include/actionlib/client/simple_action_client.h#L472).

Given this evidence I investigated the callback stack (ActionClient - GoalManager - CommStateMachine) and I noticed that in CartesianInterface version 1 the SimpleActionClient used inside the RosTask class was using the default value for the second argument spin_thread, which is true, of the constructor, as you can see here:

https://github.com/ADVRHumanoids/CartesianInterface/blob/00d4a1b8b4336429383064a05b62394b8bba7d50/src/ros/RosImpl.cpp#L104

While in CartesianInterface version 2 this value was set to false, as you can see here:

https://github.com/ADVRHumanoids/CartesianInterface/blob/cd78b5726ef2fe9a78a05f48b27f7878acaa704a/src/ros/client_api/CartesianRos.cpp#L17

Given the documentation from ROS:

 /**
   * \brief Simple constructor
   *
   * Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface
   * \param name The action name. Defines the namespace in which the action communicates
   * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false,
   *                    then the user has to call ros::spin() themselves. Defaults to True
   */
  SimpleActionClient(const std::string & name, bool spin_thread = true)

I put the argument back to true and I test it again without experiencing the issue.

Here it is the basic source code of the executable used to perform the tests (provided by @aled96) inside Ubuntu 18.04, centauro_cartesion with centauro_car_model stack, new Centauro model (iit-centauro-ros-pkg), CI only mode with RViZ:

#include <ros/ros.h>
#include <string.h>
#include <iostream>
#include <sstream>
#include <cfloat>
#include <math.h>
#include <vector>

#include "Eigen/Core"
#include "Eigen/Geometry"

#include <cartesian_interface/ros/RosClient.h>
// #include <cartesian_interface/ros/RosImpl.h>

#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include "tf_conversions/tf_eigen.h"

#include <std_srvs/Empty.h>

XBot::Cartesian::RosClient ci_client;
// XBot::Cartesian::RosImpl ci_client;

Eigen::Affine3d carframe_T = Eigen::Affine3d::Identity();

Eigen::Affine3d target = Eigen::Affine3d::Identity();

tf::StampedTransform tr;
Eigen::Affine3d temp_aff;

//Move the car_frame to go near the obstacle
bool move(std_srvs::Empty::Request& req,
                     std_srvs::Empty::Response& res){

    Eigen::Affine3d car_target = carframe_T;

    car_target(0,3) += 0.5;

    ci_client.setTargetPose("car_frame", car_target, 5.0);
    ci_client.waitReachCompleted("car_frame"); 

    ROS_INFO("Service ended");
    return true;    
}

int main(int argc, char** argv)
{     
    ros::init(argc, argv, "move_robot");
    ros::NodeHandle nh;

    if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::ServiceServer service = nh.advertiseService("move", move);

    tf::TransformListener listener;
    tf::TransformBroadcaster br;

    tf::Transform transform;

    ros::Rate loop_rate(10);

    while(ros::ok()){

        ros::Time now;
        try {

            now = ros::Time(0);
            listener.waitForTransform ( "ci/world", "ci/car_frame", now, ros::Duration (1.0));
            listener.lookupTransform("ci/world", "ci/car_frame", now , tr);
            tf::transformTFToEigen(tr, carframe_T);           

        } catch ( tf::TransformException ex ) {
            ROS_ERROR ( "%s",ex.what() );
            ros::Duration ( 1.0 ).sleep();
        }

        ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}

fyi @alaurenzi @DavideAntonucci

[alaurenzi]

Thanks for the investigation! Merged & Closed!