ROS MoveIt的同步和异步

ROS MoveIt的同步和异步

flyfish

当调用MoveIt的同步运动或者异步运动
异步:asyncMove
同步:move

move_group_interface.cpp

moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncMove()
{
  return impl_->move(false);
}

moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::move()
{
  return impl_->move(true);
}

impl_的声明在move_group_interface.h文件中

  class MoveGroupInterfaceImpl;
  MoveGroupInterfaceImpl* impl_;

move_group_interface和MoveGroupInterfaceImpl主要是使用了C++ pimp惯用法以保持接口不变,可移植,最小依赖
class MoveGroupInterface::MoveGroupInterfaceImpl

就像这样

// my_class.h  
class my_class {  
   //  ... all public and protected stuff goes here ...  
private:  
   class impl; unique_ptr pimpl; // opaque type here  
};

// my_class.cpp  
class my_class::impl {  // defined privately here  
  // ... all private data and functions: all of these  
  //     can now change without recompiling callers ...  
};  
my_class::my_class(): pimpl( new impl )  
{  
  // ... set impl values ...   
} 

namespace moveit
{
namespace planning_interface
{
}
}
moveit,planning_interface都是命名空间

move的实现

  MoveItErrorCode move(bool wait)
  {
    if (!move_action_client_)
    {
      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
    }
    if (!move_action_client_->isServerConnected())
    {
      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
    }

    moveit_msgs::MoveGroupGoal goal;
    constructGoal(goal);
    goal.planning_options.plan_only = false;
    goal.planning_options.look_around = can_look_;
    goal.planning_options.replan = can_replan_;
    goal.planning_options.replan_delay = replan_delay_;
    goal.planning_options.planning_scene_diff.is_diff = true;
    goal.planning_options.planning_scene_diff.robot_state.is_diff = true;

    move_action_client_->sendGoal(goal);
    if (!wait)
    {
      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
    }

    if (!move_action_client_->waitForResult())
    {
      ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
    }

    if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      return MoveItErrorCode(move_action_client_->getResult()->error_code);
    }
    else
    {
      ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
                                                        << ": " << move_action_client_->getState().getText());
      return MoveItErrorCode(move_action_client_->getResult()->error_code);
    }
  }

MoveGroupInterface以action的形式(move_action_client_)来接受move_goup的规划结果,

它的类型是
std::unique_ptr > move_action_client_;

如果是异步 move_action_client_->sendGoal(goal);之后就返回
如果同步就要等待执行 完成actionlib::SimpleClientGoalState::SUCCEEDED后返回
代码如下.问题到此解决.

simple_action_client.h

template
bool SimpleActionClient::waitForResult(const ros::Duration & timeout)
{
  if (gh_.isExpired()) {
    ROS_ERROR_NAMED("actionlib",
      "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
    return false;
  }

  if (timeout < ros::Duration(0, 0)) {
    ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
  }

  ros::Time timeout_time = ros::Time::now() + timeout;

  boost::mutex::scoped_lock lock(done_mutex_);

  // Hardcode how often we check for node.ok()
  ros::Duration loop_period = ros::Duration().fromSec(.1);

  while (nh_.ok()) {
    // Determine how long we should wait
    ros::Duration time_left = timeout_time - ros::Time::now();

    // Check if we're past the timeout time
    if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) {
      break;
    }

    if (cur_simple_state_ == SimpleGoalState::DONE) {
      break;
    }


    // Truncate the time left
    if (time_left > loop_period || timeout == ros::Duration()) {
      time_left = loop_period;
    }

    done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
  }

  return cur_simple_state_ == SimpleGoalState::DONE;
}