OpenRAVE ControllerBase is blocking at the IsDone() method and never returns

Question

I am working on a custom OpenRAVE ControllerBase C++ class that implements a robot controller.

I will try to keep the code as minimal as possible:

typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajClient;

class ArmController : public ControllerBase {

    .
    .
    .

    virtual bool SetPath(TrajectoryBaseConstPtr ptraj) {
        if (ptraj != NULL) {
            TrajectoryBasePtr traj = SimplifyTrajectory(ptraj);

            PlannerStatus status = planningutils::RetimeTrajectory(traj, false, 1.0, 1.0, "LinearTrajectoryRetimer");
            if (status != PS_HasSolution) {
                ROS_ERROR("Not executing trajectory because retimer failed.");
                return false;
            }

            trajectory_msgs::JointTrajectory trajectory = FromOpenRaveToRosTrajectory(traj);
            control_msgs::FollowJointTrajectoryGoal goal;
            goal.trajectory = trajectory;
            _ac->sendGoal(goal);
        }

        return true;
    }

    virtual bool IsDone() {
      _ac->waitForResult(ros::Duration(5.0));
      if (_ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
        return true;
      }
    }
}

This is a single controller for the robot arm, and I have implemented another one for the end-effector which is not currently in use (the end-effector is disconnected) in this case but it does some basic operations (like open, close gripper and listen to its joint state).

Then I have a Python script that tries to execute a trajectory on the robot:

robot.multicontroller = RaveCreateMultiController(env, "")
robot.SetController(robot.multicontroller)

robot_controller = RaveCreateController(env, 'arm_controller')
robot.multicontroller.AttachController(robot_controller, [2, 1, 0, 4, 5, 6], 0)

hand_controller = RaveCreateController(env, 'gripper_controller')
robot.multicontroller.AttachController(hand_controller, [3], 0)

trajectory_object = some_trajectory_object

robot.GetController().SetPath(trajectory_object)
robot.WaitForController(0)

The trajectory is being executed on the real robot with success but the code blocks at robot.WaitForController(0) and is actually in an endless loop in IsDone() method of the controller.

Any ideas?

Answer 1

I assume that your end-effector controller also implements a similar IsDone() method with a similar logic (i.e is waiting for the end-effector to reach its goal to return true, otherwise is returning false).

Since you are using a MultiController the WaitForController(0) in your Python script is not looking only to the arm controller but to all of the controllers attached to the MultiController.

From bool OpenRAVE::MultiController::IsDone() virtual

returns true only if all controllers return true.

Since you said that one of the controllers (the end-effector controller) is not in use, the IsDone of this controller returns false and hence the WaitForController(0) of the MultiController is blocked not on the arm controller you are suspecting, but instead is waiting for this not-in-use end-effector controller IsDone to evaluate to true, which will never do because you have it disconnected from the system; hence the blocking.

You have I think three options:

• You either make sure that the end-effector is also connected to the system and hence the end-effector controller is working as expected (i.e the IsDone is returning True).
• You are not attaching the end-effector controller to the multi-controller in your Python script (comment it out).
• You change the logic of the IsDone method of the end-effector controller such that if is not connected, will return true.

Here is a quick solution:

def _is_rostopic_name_exists(self, topic_name):
    if topic_name[0] != "/":
        topic_name = "/" + topic_name

    topics = rospy.get_published_topics()
    for topic in topics:
        if topic[0] == topic_name:
            return True

    return False

if _is_rostopic_name_exists("CModelRobotInput") and _is_rostopic_name_exists("CModelRobotOutput"):
    hand_controller = RaveCreateController(env, 'robotiqcontroller')
    robot.multicontroller.AttachController(hand_controller, [3], 0)
else:
    RaveLogWarn("End-effector controller not attached, topics ('CModelRobotInput' or/and 'CModelRobotOutput') are not available.")

This way we are attaching the second controller to the multi-controller iff certain, required, ROS topics exist that are needed for the normal execution of that controller.