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<impl> 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<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;

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

simple_action_client.h

template<class ActionSpec> bool SimpleActionClient<ActionSpec>::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; }