Question

我正在开发一个实现机器人控制器的自定义OpenRAVE ControllerBase C ++类。

我会尝试尽可能减少代码：

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;
      }
    }
}

这是机器人手臂的单个控制器，我已经为末端执行器实现了另一个，在这种情况下当前没有使用（末端执行器断开连接），但是它做了一些基本的操作（比如打开），关闭抓手，听听它的联合状态。）

然后我有一个Python脚本试图在机器人上执行轨迹：

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)

轨迹正在真实机器人上执行但成功但代码阻塞在robot.WaitForController(0)并且实际上处于控制器的IsDone()方法中的无限循环中。

有什么想法吗？

Answer 1

我假设你的末端效应器控制器也实现了一个类似的IsDone()方法，它具有类似的逻辑（即等待末端效应器达到其目标以返回true，否则返回false）。

由于您使用的是MultiController，因此Python脚本中的WaitForController(0)不仅仅是关于arm控制器，而是关注连接到MultiController的所有控制器。

来自bool OpenRAVE::MultiController::IsDone() virtual

仅当所有控制器都返回true时，
才返回true。

由于您说其中一个控制器（末端效应器控制器）未使用，因此该控制器的IsDone返回false，因此MultiController的WaitForController(0)为阻止不在您怀疑的手臂控制器上，而是等待这个未使用的末端执行器控制器IsDone评估为true，这将永远不会，因为您已将它与系统;因此阻塞。

我认为有三种选择：

您要么确保末端效应器也连接到系统，因此末端效应器控制器正在按预期工作（即IsDone返回True）。
您没有将末端效应器控制器连接到Python脚本中的多控制器（注释掉它）。
您更改了末端效应器控制器的IsDone方法的逻辑，如果未连接，则返回true。

这是一个快速解决方案：

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.")

这样我们就可以将第二个控制器连接到多控制器，如果存在正常执行该控制器所需的某些必需的ROS主题。

OpenRAVE ControllerBase在IsDone（）方法中阻塞，永不返回

1 个答案:

OpenRAVE Con​​trollerBase在IsDone（）方法中阻塞，永不返回

1 个答案:

OpenRAVE ControllerBase在IsDone（）方法中阻塞，永不返回