-1

我正在研究ControllerBase实现机器人控制器的自定义 OpenRAVE 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()

有任何想法吗？

4

1 回答 1

0

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

由于您在 Python 脚本中使用MultiController了.WaitForController(0)MultiController

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

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

由于您说其中一个控制器（末端执行器控制器）未在使用中，因此IsDone该控制器的返回false，因此WaitForController(0)MultiController 的 不是在您怀疑的手臂控制器上被阻止，而是在等待这个不是-使用中的末端执行器控制器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 主题，我们会将第二个控制器附加到多控制器。

于 2018-03-29T09:41:03.973 回答