OpenRAVE ControllerBase блокируется методом IsDone () и никогда не возвращает

Question

OpenRAVE ControllerBase блокируется методом IsDone () и никогда не возвращает

Я работаю над кастомным 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() метод контроллера.

Есть идеи?

0

c++openrave

Решение

Другие решения

Других решений пока нет …

Источник

Accepted Answer

Я предполагаю, что ваш контроллер конечного эффектора также реализует аналогичный IsDone() метод с аналогичной логикой (т.е. ожидает, пока конечный эффектор достигнет своей цели, чтобы вернуть true, иначе возвращает false).

Поскольку вы используете MultiController WaitForController(0) в вашем скрипте Python смотрит не только на контроллер руки, но и на все контроллеры, подключенные к 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, которые необходимы для нормального выполнения этого контроллера.

0