server处理exectraj action：MoveGroupExecuteTrajectoryAction

ancientcc
  • DOF(Degrees of Freedom)。自由度。
  • js(JointState)。关节状态。专门指存储在/joint_states话题、最后发送到机器人的sensor_msgs/JointState。

client通过moveit::planning_interface::MoveGroupInterfacemoveit::plan(...)规划出一个轨迹后，调用execute(const Plan& plan)执行该轨迹，内中会发送moveit_msgs::ExecuteTrajectoryGoal请求action。对应到server会触发MoveGroupExecuteTrajectoryAction回调exectraj服务的处理例程：executePathCallback。

  • 控制器(MoveItControllerManager)：moveit四大插件之一。负责把轨迹转换为机器人的关节状态，并发布。默认使用MoveItFakeControllerManager，标识：moveit_fake_controller_manager/MoveItFakeControllerManager。
  • 控制器插件包括两个部分：一个派生于MoveItControllerManager的插件对象，一个派生于MoveItControllerHandle，后者负责把轨迹中的point转换为js，并发布到机器人。
  • 过程中至少会新建一个线程：TrajectoryExecutionManager::executeThread。如果控制器插件MoveItControllerHandle转换point较复杂，会在executeThread线程内再新建一个线程，内中处理转换point和发布，像ThreadedController::execTrajectory。

 

一、MoveGroupExecuteTrajectoryAction::executePathCallback

@goal[IN]。plan生成的轨迹。
void MoveGroupExecuteTrajectoryAction::executePathCallback(const moveit_msgs::ExecuteTrajectoryGoalConstPtr& goal)
{
  moveit_msgs::ExecuteTrajectoryResult action_res;
  if (!context_->trajectory_execution_manager_)
  {

不应该进入这入口。trajectory_execution_manager_类型是TrajectoryExecutionManager，内中有个成员controller_manager_，存储着当前正使用的控制器插件。

    const std::string response = "Cannot execute trajectory since ~allow_trajectory_execution was set to false";
    action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
    execute_action_server_->setAborted(action_res, response);
    return;
  }

  // execute主逻辑，后面有展开。
  executePath(goal, action_res);

  const std::string response = getActionResultString(action_res.error_code, false, false);

getActionResultString把用状态码(action_res.error_code)表示的执行结果转换为字符串表示，不影响主逻辑。

  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
  {
    execute_action_server_->setSucceeded(action_res, response);
  }
  else if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
  {
    execute_action_server_->setPreempted(action_res, response);
  }
  else
  {
    execute_action_server_->setAborted(action_res, response);
  }

  setExecuteTrajectoryState(IDLE);

向client发布feedback话题，表示目前的execute进度是结束了。不影响主逻辑

}

 

二、MoveGroupExecuteTrajectoryAction::executePath

  • @goal[IN]。plan生成的轨迹。内容就一个trajectory字段，其类型moveit_msgs/RobotTrajectory。RobotTrajectory要封装成ExecuteTrajectoryGoal，目的是命名上符合action规范。
  • @action_res[OUT]。存储执行结果。
void MoveGroupExecuteTrajectoryAction::executePath(const moveit_msgs::ExecuteTrajectoryGoalConstPtr& goal,
                                                   moveit_msgs::ExecuteTrajectoryResult& action_res)
{
  ROS_INFO_NAMED(getName(), "Execution request received");
 
  context_->trajectory_execution_manager_->clear();
  if (context_->trajectory_execution_manager_->push(goal->trajectory))
  {
    setExecuteTrajectoryState(MONITOR);
    context_->trajectory_execution_manager_->execute();
    moveit_controller_manager::ExecutionStatus status = context_->trajectory_execution_manager_->waitForExecution();

trajectory_execution_manager_->push会调用configure，内中调用TrajectoryExecutionManager::distributeTrajectory。

setExecuteTrajectoryState只是feedback到client，不影响server逻辑。以下的execute()是主逻辑。distributeTrajectory

顾名思议，waitForExecution作用是同步等待执行结束。它是通过等待信号量execution_complete_condition_，trajectory_execution_manager_->execute()执行结束后，会把它置为有信号。——同步不包括等待ThreadedController::execTrajectory线程发布/joint_states。

    if (status == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
    {
      action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    } else if (...) {

一系列if/else，把存储在status中的状态转换为action_res.error_code.val表示的状态。

    ...
    } else {
      action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
    }
    ROS_INFO_STREAM_NAMED(getName(), "Execution completed: " << status.asString());
  }
  else
  {
    action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
  }
}

 

三、TrajectoryExecutionManager::push

每个新来的执行任务会创建一个私有上下文对象TrajectoryExecutionContext，并放入trajectories_。

bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory,
                                      const std::vector<std::string>& controllers)
{
  if (!execution_complete_)
  {
    ROS_ERROR_NAMED(name_, "Cannot push a new trajectory while another is being executed");
    return false;
  }

  TrajectoryExecutionContext* context = new TrajectoryExecutionContext();
  if (configure(*context, trajectory, controllers))
  {

每个新来的执行任务会创建一个私有上下文对象TrajectoryExecutionContext，configure(...)作用是设置好这个上下文的相关变量。

    ...
    trajectories_.push_back(context);
    return true;
  }
  else
  {
    delete context;
    last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
  }

  return false;
}

 

四、TrajectoryExecutionManager::execute

  • @callback[IN]。不填时empty。针对示例，是empty。
  • @auto_clear[IN]。不填时true。针对示例，是true。
void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback, bool auto_clear)
{
  execute(callback, PathSegmentCompleteCallback(), auto_clear);
}

 

  • @callback。
  • @part_callback。
  • @auto_clear。
void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback,
                                         const PathSegmentCompleteCallback& part_callback, bool auto_clear)
{
  stopExecution(false);

  // check whether first trajectory starts at current robot state
  if (!trajectories_.empty() && !validate(*trajectories_.front()))
  {

validate(...)会要求1秒内必须收到过/joint_states话题。

    last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
    if (auto_clear)
      clear();
    if (callback)
      callback(last_execution_status_);
    return;
  }

  // start the execution thread
  execution_complete_ = false;
  execution_thread_ = std::make_unique<boost::thread>(&TrajectoryExecutionManager::executeThread, this, callback,
                                                      part_callback, auto_clear);
}

 

五、TrajectoryExecutionManager::executeThread

在executeThread线程执行操作机械臂。

void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback,
                                               const PathSegmentCompleteCallback& part_callback, bool auto_clear)
{
  // if we already got a stop request before we even started anything, we abort
  if (execution_complete_)
  {
    last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
    if (callback)
      callback(last_execution_status_);
    return;
  }

  ROS_DEBUG_NAMED(name_, "Starting trajectory execution ...");
  // assume everything will be OK
  last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;

  // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
  // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
  std::size_t i = 0;
  for (; i < trajectories_.size(); ++i)
  {
    bool epart = executePart(i);
    if (epart && part_callback)
      part_callback(i);
    if (!epart || execution_complete_)
    {
      ++i;
      break;
    }
  }

  // only report that execution finished successfully when the robot actually stopped moving
  if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
    waitForRobotToStop(*trajectories_[i - 1]);

  ROS_INFO_NAMED(name_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());

  // notify whoever is waiting for the event of trajectory completion
  execution_state_mutex_.lock();
  execution_complete_ = true;
  execution_state_mutex_.unlock();
  execution_complete_condition_.notify_all();

  // clear the paths just executed, if needed
  if (auto_clear)
    clear();

  // call user-specified callback
  if (callback)
    callback(last_execution_status_);
}

 

六、TrajectoryExecutionManager::executePart

“Part”指moveit可能累积了数个exeucte(...)，part是当中的哪一个。part_index指向了此刻要执行的、它在trajectories_这个vector内的索引。

<libros>/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
bool TrajectoryExecutionManager::executePart(std::size_t part_index)
{
  TrajectoryExecutionContext& context = *trajectories_[part_index];
  ...
        for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
        {
          bool ok = false;
          try
          {
            ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
          }
          catch (std::exception& ex)
          {
            ROS_ERROR_NAMED(name_, "Caught %s when sending trajectory to controller", ex.what());
          }
          if (!ok)
          {
            for (std::size_t j = 0; j < i; ++j)
              try
              {
                active_handles_[j]->cancelExecution();
              }
              catch (std::exception& ex)
              {
                ROS_ERROR_NAMED(name_, "Caught %s when canceling execution", ex.what());
              }
            ROS_ERROR_NAMED(name_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
                            context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
            if (i > 0)
              ROS_ERROR_NAMED(name_, "Cancelling previously sent trajectory parts");
            active_handles_.clear();
            current_context_ = -1;
            last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
            return false;
          }
        }
        ...
}

 

七、ThreadedController::sendTrajectory

ThreadedController::sendTrajectory是moveit_fake_controller_manager中私有函数，而且不是插件MoveItControllerManager的虚函数，那TrajectoryExecutionManager是怎么直接调用到它的？——私有控制器除提供派生于MoveItControllerManager的插件对象，还需要提供派生于moveit_controller_manager::MoveItControllerHandle的point处理对象。至于moveit是怎么把同属于该控制器的MoveItControllerManager、MoveItControllerHandle关联在一起？——MoveItControllerManager须实现的虚函数getController。

virtual MoveItControllerHandlePtr getControllerHandle(const std::string& name) = 0;

对如何把轨迹中point转换到能包发布到机器人的js，MoveItFakeControllerManager提供了三种可选方法，何一时刻只能选一种。

  • LastPointController。立即把原值作为js发送向关节。因为操作可立即、快速结束，此种场景不创建第二线程。
  • ViaPointController。每个点有个time_from_start字段，指示该point要发送到机器人的时刻。选择这种时，时间必须已到这个时刻，才把原值为作为js发送向关节。
  • InterpolatingController(默认)。根据前后两个point算出一个均值，作为发送向关节的js。
<libros>/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp
bool ThreadedController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
{
  cancelTrajectory();  // cancel any previous fake motion
  cancel_ = false;
  status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
  thread_ = boost::thread(std::bind(&ThreadedController::execTrajectory, this, t));
  return true;
}

 

八、InterpolatingController::execTrajectory

<libros>/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp
void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
{
  ROS_INFO("Fake execution of trajectory");
  if (t.joint_trajectory.points.empty())
    return;

  sensor_msgs::JointState js;
  js.header = t.joint_trajectory.header;
  js.name = t.joint_trajectory.joint_names;

  const std::vector<trajectory_msgs::JointTrajectoryPoint>& points = t.joint_trajectory.points;
  std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator prev = points.begin(),  // previous via point
      next = points.begin() + 1,  // currently targetted via point
      end = points.end();

  ros::Time start_time = ros::Time::now();
  while (!cancelled())
  {
    ros::Duration elapsed = ros::Time::now() - start_time;
    // hop to next targetted via point
    while (next != end && elapsed > next->time_from_start)
    {
      ++prev;
      ++next;
    }
    if (next == end)
      break;

    double duration = (next->time_from_start - prev->time_from_start).toSec();
    ROS_DEBUG("elapsed: %.3f via points %td,%td / %td  alpha: %.3f", elapsed.toSec(), prev - points.begin(),
              next - points.begin(), end - points.begin(),
              duration > std::numeric_limits<double>::epsilon() ? (elapsed - prev->time_from_start).toSec() / duration :
                                                                  1.0);
    interpolate(js, *prev, *next, elapsed);

根据前、后两个关节算出要插入的关节点。

    js.header.stamp = ros::Time::now();
    pub_.publish(js);
    rate_.sleep();
  }
  if (cancelled())
    return;

  ros::Duration elapsed = ros::Time::now() - start_time;
  ROS_DEBUG("elapsed: %.3f via points %td,%td / %td  alpha: 1.0", elapsed.toSec(), prev - points.begin(),
            next - points.begin(), end - points.begin());

  // publish last point
  interpolate(js, *prev, *prev, prev->time_from_start);
  js.header.stamp = ros::Time::now();
  pub_.publish(js);

  ROS_DEBUG("Fake execution of trajectory: done");
}

 

九、MoveGroupExecuteTrajectoryAction::setExecuteTrajectoryState

  • @state[IN]。要feedback到client的状态。
void MoveGroupExecuteTrajectoryAction::setExecuteTrajectoryState(MoveGroupState state)
{
  moveit_msgs::ExecuteTrajectoryFeedback execute_feedback;
  execute_feedback.state = stateToStr(state);
  execute_action_server_->publishFeedback(execute_feedback);
}

不要被函数名set字眼混淆了，它只是把执行状态通过feedback话题发布到client，不会改变server端MoveGroupExecuteTrajectoryAction状态，可说没有它也不会影响server端的执行逻辑。

 

发布于 2023-12-22 14:14:44

全部评论: 0 条

    写评论: