OpenRAVE ControllerBase 在 IsDone(） 方法处阻塞并且永远不会返回

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

本文关键字：永远 返回 ControllerBase IsDone 方法 OpenRAVE      更新时间：2023-10-16

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

我将尽量保持代码最少：

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()。

有什么想法吗？

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

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

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

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

由于您说其中一个控制器(末端执行器控制器)未使用，因此该控制器的IsDone返回false，因此多控制器的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 主题。

相关文章：