OpenRAVE ControllerBase的IsDone()方法阻塞无返回问题咨询

Hey there, let's dig into why your IsDone() method is stuck and never returns. Based on the code snippet you provided and common pitfalls with OpenRAVE's ControllerBase + actionlib trajectory clients, here are the most likely culprits and how to fix them:

Common Causes & Fixes #

1. IsDone() Logic Isn't Tracking Execution State Correctly #

First off, since you didn't share the implementation of IsDone(), this is the most probable issue. If your IsDone() is waiting for a flag or condition that never gets updated during trajectory execution, it'll block forever.

• Check: Add debug prints inside IsDone() to output variables like current trajectory progress, action client state, or any boolean flags you're using to track completion. For example:

  virtual bool IsDone() const override {
      ROS_INFO("Current action state: %d", _traj_client->getState().state_);
      ROS_INFO("Trajectory end time: %.2f, current time: %.2f", _traj_end_time, OpenRAVE::RaveGetTime());
      // Your existing condition here
  }
  

• Fix: Ensure that whenever the trajectory finishes (either via action client feedback indicating completion, or reaching the last trajectory point's timestamp), you update the flag/condition that IsDone() checks.

2. Retimed Trajectory Has Invalid Timing Data #

Your SetPath() uses planningutils::RetimeTrajectory()—if this function fails or produces a trajectory with invalid timestamps (like infinite end time, or NaN values), your controller will never think the trajectory is done.

• Check: Immediately after calling RetimeTrajectory(), check the returned PlannerStatus:

  PlannerStatus status = planningutils::RetimeTrajectory(traj, false, 1.0, 1.0);
  if (status != PlannerStatus::PS_HasSolution) {
      RAVE_WARN("RetimeTrajectory failed! Status: %d\n", status);
      return false;
  }
  // Also validate the trajectory's last point time
  if (traj->GetDuration() <= 0 || std::isnan(traj->GetDuration())) {
      RAVE_WARN("Invalid trajectory duration: %.2f\n", traj->GetDuration());
      return false;
  }
  

• Fix: If retiming fails, don't proceed with the invalid trajectory. Adjust the retiming parameters (max velocity/acceleration) to match your robot's actual limits—using 1.0 for both might be too restrictive or unrealistic for your arm.

3. Action Client Is Stuck Waiting for Server Feedback #

You're using actionlib::SimpleActionClient to send trajectories. If the action server never sends a SUCCEEDED status, your IsDone() (if it's waiting on this) will block indefinitely.

• Check: Verify the action server is running and receiving goals. Use rostopic echo /your_trajectory_action/feedback to see if the server is publishing progress updates. Also, check if the client is properly connected:

  if (!_traj_client->waitForServer(ros::Duration(5.0))) {
      RAVE_ERROR("Could not connect to trajectory action server!\n");
      return false;
  }
  

• Fix: Avoid blocking IsDone() on a synchronous action client call. Instead, use asynchronous callbacks to update a completion flag when the action finishes:

  void doneCb(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr& result) {
      _trajectory_done = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
  }
  
  // When sending the goal:
  _traj_client->sendGoal(goal, boost::bind(&ArmController::doneCb, this, _1, _2));
  

  Then have IsDone() just return _trajectory_done.

4. Simplified Trajectory Lost Critical Endpoint Data #

The SimplifyTrajectory() call might be removing the final trajectory point or altering its timing, making your controller think the trajectory never ends.

• Check: Compare the original trajectory and simplified trajectory's point count and timestamps:

  RAVE_INFO("Original trajectory points: %d, simplified: %d\n", ptraj->GetNumWaypoints(), traj->GetNumWaypoints());
  RAVE_INFO("Original end time: %.2f, simplified end time: %.2f\n", ptraj->GetDuration(), traj->GetDuration());
  

• Fix: If simplifying is causing issues, try disabling it temporarily to see if IsDone() starts working. You can also adjust the simplification parameters (if available) to preserve the endpoint.

5. Controller State Isn't Initialized Properly in SetPath() #

If you're not resetting completion flags or trajectory state variables when a new path is set, IsDone() might be checking stale data from a previous trajectory.

• Fix: At the start of SetPath(), reset any tracking variables:

  virtual bool SetPath(TrajectoryBaseConstPtr ptraj) {
      _trajectory_done = false;
      _current_traj = nullptr;
      // Rest of your code...
  }
  

Quick Test to Isolate the Issue #

As a quick sanity check, temporarily modify IsDone() to always return true:

virtual bool IsDone() const override {
    return true;
}

If your controller stops blocking, you know the problem is definitely in the IsDone() logic or the state it's tracking. If it still blocks, the issue might be elsewhere in your controller's execution loop.

内容的提问来源于stack exchange，提问作者Rafael