00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00038 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h>
00039 #include <dynamic_reconfigure/server.h>
00040
00041 namespace trajectory_execution_manager
00042 {
00043
00044 const std::string TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC = "trajectory_execution_event";
00045
00046 static const ros::Duration DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE(1.0);
00047 static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5;
00048 static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = 1.1;
00049
00050 using namespace moveit_ros_planning;
00051
00052 class TrajectoryExecutionManager::DynamicReconfigureImpl
00053 {
00054 public:
00055
00056 DynamicReconfigureImpl(TrajectoryExecutionManager *owner) : owner_(owner),
00057 dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))
00058 {
00059 dynamic_reconfigure_server_.setCallback(boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00060 }
00061
00062 private:
00063
00064 void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig &config, uint32_t level)
00065 {
00066 owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
00067 owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
00068 }
00069
00070 TrajectoryExecutionManager *owner_;
00071 dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
00072 };
00073
00074 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel) :
00075 robot_model_(kmodel), node_handle_("~")
00076 {
00077 if (!node_handle_.getParam("moveit_manage_controllers", manage_controllers_))
00078 manage_controllers_ = false;
00079
00080 if (!node_handle_.getParam("allowed_execution_duration_scaling", allowed_execution_duration_scaling_))
00081 allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
00082
00083 if (!node_handle_.getParam("allowed_goal_duration_margin", allowed_goal_duration_margin_))
00084 allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
00085
00086 initialize();
00087 }
00088
00089 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, bool manage_controllers) :
00090 robot_model_(kmodel), node_handle_("~"), manage_controllers_(manage_controllers)
00091 {
00092 initialize();
00093 }
00094
00095 TrajectoryExecutionManager::~TrajectoryExecutionManager()
00096 {
00097 run_continuous_execution_thread_ = false;
00098 stopExecution(true);
00099 delete reconfigure_impl_;
00100 }
00101
00102 void TrajectoryExecutionManager::initialize()
00103 {
00104 reconfigure_impl_ = NULL;
00105 verbose_ = false;
00106 execution_complete_ = true;
00107 stop_continuous_execution_ = false;
00108 current_context_ = -1;
00109 last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00110 run_continuous_execution_thread_ = true;
00111 execution_duration_monitoring_ = true;
00112 execution_velocity_scaling_ = 1.0;
00113
00114
00115 try
00116 {
00117 controller_manager_loader_.reset(new pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>("moveit_core", "moveit_controller_manager::MoveItControllerManager"));
00118 }
00119 catch(pluginlib::PluginlibException& ex)
00120 {
00121 ROS_FATAL_STREAM("Exception while creating controller manager plugin loader: " << ex.what());
00122 return;
00123 }
00124
00125 if (controller_manager_loader_)
00126 {
00127 std::string controller;
00128 if (!node_handle_.getParam("moveit_controller_manager", controller))
00129 {
00130 const std::vector<std::string> &classes = controller_manager_loader_->getDeclaredClasses();
00131 if (classes.size() == 1)
00132 {
00133 controller = classes[0];
00134 ROS_WARN("Parameter '~moveit_controller_manager' is not specified but only one matching plugin was found: '%s'. Using that one.", controller.c_str());
00135 }
00136 else
00137 ROS_FATAL("Parameter '~moveit_controller_manager' not specified. This is needed to identify the plugin to use for interacting with controllers. No paths can be executed.");
00138 }
00139
00140 if (!controller.empty())
00141 try
00142 {
00143 controller_manager_.reset(controller_manager_loader_->createUnmanagedInstance(controller));
00144 }
00145 catch(pluginlib::PluginlibException& ex)
00146 {
00147 ROS_FATAL_STREAM("Exception while loading controller manager '" << controller << "': " << ex.what());
00148 }
00149 }
00150
00151
00152 reloadControllerInformation();
00153
00154 event_topic_subscriber_ = root_node_handle_.subscribe(EXECUTION_EVENT_TOPIC, 100, &TrajectoryExecutionManager::receiveEvent, this);
00155
00156 reconfigure_impl_ = new DynamicReconfigureImpl(this);
00157
00158 if (manage_controllers_)
00159 ROS_INFO("Trajectory execution is managing controllers");
00160 else
00161 ROS_INFO("Trajectory execution is not managing controllers");
00162 }
00163
00164 void TrajectoryExecutionManager::enableExecutionDurationMonitoring(bool flag)
00165 {
00166 execution_duration_monitoring_ = flag;
00167 }
00168
00169 void TrajectoryExecutionManager::setAllowedExecutionDurationScaling(double scaling)
00170 {
00171 allowed_execution_duration_scaling_ = scaling;
00172 }
00173
00174 void TrajectoryExecutionManager::setExecutionVelocityScaling(double scaling)
00175 {
00176 execution_velocity_scaling_ = scaling;
00177 }
00178
00179 bool TrajectoryExecutionManager::isManagingControllers() const
00180 {
00181 return manage_controllers_;
00182 }
00183
00184 const moveit_controller_manager::MoveItControllerManagerPtr& TrajectoryExecutionManager::getControllerManager() const
00185 {
00186 return controller_manager_;
00187 }
00188
00189 void TrajectoryExecutionManager::processEvent(const std::string &event)
00190 {
00191 if (event == "stop")
00192 stopExecution(true);
00193 else
00194 ROS_WARN_STREAM("Unknown event type: '" << event << "'");
00195 }
00196
00197 void TrajectoryExecutionManager::receiveEvent(const std_msgs::StringConstPtr &event)
00198 {
00199 ROS_INFO_STREAM("Received event '" << event->data << "'");
00200 processEvent(event->data);
00201 }
00202
00203 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller)
00204 {
00205 if (controller.empty())
00206 return push(trajectory, std::vector<std::string>());
00207 else
00208 return push(trajectory, std::vector<std::string>(1, controller));
00209 }
00210
00211 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller)
00212 {
00213 if (controller.empty())
00214 return push(trajectory, std::vector<std::string>());
00215 else
00216 return push(trajectory, std::vector<std::string>(1, controller));
00217 }
00218
00219 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers)
00220 {
00221 moveit_msgs::RobotTrajectory traj;
00222 traj.joint_trajectory = trajectory;
00223 return push(traj, controllers);
00224 }
00225
00226 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
00227 {
00228 if (!execution_complete_)
00229 {
00230 ROS_ERROR("Cannot push a new trajectory while another is being executed");
00231 return false;
00232 }
00233
00234 TrajectoryExecutionContext *context = new TrajectoryExecutionContext();
00235 if (configure(*context, trajectory, controllers))
00236 {
00237 if (verbose_)
00238 {
00239 std::stringstream ss;
00240 ss << "Pushed trajectory for execution using controllers [ ";
00241 for (std::size_t i = 0 ; i < context->controllers_.size() ; ++i)
00242 ss << context->controllers_[i] << " ";
00243 ss << "]:" << std::endl;
00244 for (std::size_t i = 0 ; i < context->trajectory_parts_.size() ; ++i)
00245 ss << context->trajectory_parts_[i] << std::endl;
00246 ROS_INFO("%s", ss.str().c_str());
00247 }
00248 trajectories_.push_back(context);
00249 return true;
00250 }
00251 else
00252 {
00253 delete context;
00254 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00255 }
00256
00257 return false;
00258 }
00259
00260 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller)
00261 {
00262 if (controller.empty())
00263 return pushAndExecute(trajectory, std::vector<std::string>());
00264 else
00265 return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
00266 }
00267
00268 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller)
00269 {
00270 if (controller.empty())
00271 return pushAndExecute(trajectory, std::vector<std::string>());
00272 else
00273 return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
00274 }
00275
00276 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState &state, const std::string &controller)
00277 {
00278 if (controller.empty())
00279 return pushAndExecute(state, std::vector<std::string>());
00280 else
00281 return pushAndExecute(state, std::vector<std::string>(1, controller));
00282 }
00283
00284 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers)
00285 {
00286 moveit_msgs::RobotTrajectory traj;
00287 traj.joint_trajectory = trajectory;
00288 return pushAndExecute(traj, controllers);
00289 }
00290
00291 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState &state, const std::vector<std::string> &controllers)
00292 {
00293 moveit_msgs::RobotTrajectory traj;
00294 traj.joint_trajectory.header = state.header;
00295 traj.joint_trajectory.joint_names = state.name;
00296 traj.joint_trajectory.points.resize(1);
00297 traj.joint_trajectory.points[0].positions = state.position;
00298 traj.joint_trajectory.points[0].velocities = state.velocity;
00299 traj.joint_trajectory.points[0].effort = state.effort;
00300 traj.joint_trajectory.points[0].time_from_start = ros::Duration(0, 0);
00301 return pushAndExecute(traj, controllers);
00302 }
00303
00304 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
00305 {
00306 if (!execution_complete_)
00307 {
00308 ROS_ERROR("Cannot push & execute a new trajectory while another is being executed");
00309 return false;
00310 }
00311
00312 TrajectoryExecutionContext *context = new TrajectoryExecutionContext();
00313 if (configure(*context, trajectory, controllers))
00314 {
00315 {
00316 boost::mutex::scoped_lock slock(continuous_execution_mutex_);
00317 continuous_execution_queue_.push_back(context);
00318 if (!continuous_execution_thread_)
00319 continuous_execution_thread_.reset(new boost::thread(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)));
00320 }
00321 last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00322 continuous_execution_condition_.notify_all();
00323 return true;
00324 }
00325 else
00326 {
00327 delete context;
00328 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00329 return false;
00330 }
00331 }
00332
00333 void TrajectoryExecutionManager::continuousExecutionThread()
00334 {
00335 std::set<moveit_controller_manager::MoveItControllerHandlePtr> used_handles;
00336 while (run_continuous_execution_thread_)
00337 {
00338 if (!stop_continuous_execution_)
00339 {
00340 boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
00341 while (continuous_execution_queue_.empty() && run_continuous_execution_thread_ && !stop_continuous_execution_)
00342 continuous_execution_condition_.wait(ulock);
00343 }
00344
00345 if (stop_continuous_execution_ || !run_continuous_execution_thread_)
00346 {
00347 for (std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin() ; uit != used_handles.end() ; ++uit)
00348 if ((*uit)->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::RUNNING)
00349 (*uit)->cancelExecution();
00350 used_handles.clear();
00351 while (!continuous_execution_queue_.empty())
00352 {
00353 TrajectoryExecutionContext *context = continuous_execution_queue_.front();
00354 continuous_execution_queue_.pop_front();
00355 delete context;
00356 }
00357 stop_continuous_execution_ = false;
00358 continue;
00359 }
00360
00361 while (!continuous_execution_queue_.empty())
00362 {
00363 TrajectoryExecutionContext *context = NULL;
00364 {
00365 boost::mutex::scoped_lock slock(continuous_execution_mutex_);
00366 if (continuous_execution_queue_.empty())
00367 break;
00368 context = continuous_execution_queue_.front();
00369 continuous_execution_queue_.pop_front();
00370 if (continuous_execution_queue_.empty())
00371 continuous_execution_condition_.notify_all();
00372 }
00373
00374
00375 std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
00376 while (uit != used_handles.end())
00377 if ((*uit)->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::RUNNING)
00378 {
00379 std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator toErase = uit;
00380 ++uit;
00381 used_handles.erase(toErase);
00382 }
00383 else
00384 ++uit;
00385
00386
00387
00388
00389 if (areControllersActive(context->controllers_))
00390 {
00391
00392 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles(context->controllers_.size());
00393 for (std::size_t i = 0 ; i < context->controllers_.size() ; ++i)
00394 {
00395 moveit_controller_manager::MoveItControllerHandlePtr h;
00396 try
00397 {
00398 h = controller_manager_->getControllerHandle(context->controllers_[i]);
00399 }
00400 catch(...)
00401 {
00402 ROS_ERROR("Exception caught when retrieving controller handle");
00403 }
00404 if (!h)
00405 {
00406 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00407 ROS_ERROR("No controller handle for controller '%s'. Aborting.", context->controllers_[i].c_str());
00408 handles.clear();
00409 break;
00410 }
00411 handles[i] = h;
00412 }
00413
00414 if (stop_continuous_execution_ || !run_continuous_execution_thread_)
00415 {
00416 delete context;
00417 break;
00418 }
00419
00420
00421 if (!handles.empty())
00422 for (std::size_t i = 0 ; i < context->trajectory_parts_.size() ; ++i)
00423 {
00424 bool ok = false;
00425 try
00426 {
00427 ok = handles[i]->sendTrajectory(context->trajectory_parts_[i]);
00428 }
00429 catch(...)
00430 {
00431 ROS_ERROR("Exception caught when sending trajectory to controller");
00432 }
00433 if (!ok)
00434 {
00435 for (std::size_t j = 0 ; j < i ; ++j)
00436 try
00437 {
00438 handles[j]->cancelExecution();
00439 }
00440 catch(...)
00441 {
00442 ROS_ERROR("Exception caught when canceling execution");
00443 }
00444 ROS_ERROR("Failed to send trajectory part %zu of %zu to controller %s", i + 1, context->trajectory_parts_.size(), handles[i]->getName().c_str());
00445 if (i > 0)
00446 ROS_ERROR("Cancelling previously sent trajectory parts");
00447 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00448 handles.clear();
00449 break;
00450 }
00451 }
00452 delete context;
00453
00454
00455 for (std::size_t i = 0 ; i < handles.size() ; ++i)
00456 used_handles.insert(handles[i]);
00457 }
00458 else
00459 {
00460 ROS_ERROR("Not all needed controllers are active. Cannot push and execute. You can try calling ensureActiveControllers() before pushAndExecute()");
00461 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00462 delete context;
00463 }
00464 }
00465 }
00466 }
00467
00468 void TrajectoryExecutionManager::reloadControllerInformation()
00469 {
00470 known_controllers_.clear();
00471 if (controller_manager_)
00472 {
00473 std::vector<std::string> names;
00474 controller_manager_->getControllersList(names);
00475 for (std::size_t i = 0 ; i < names.size() ; ++i)
00476 {
00477 std::vector<std::string> joints;
00478 controller_manager_->getControllerJoints(names[i], joints);
00479 ControllerInformation ci;
00480 ci.name_ = names[i];
00481 ci.joints_.insert(joints.begin(), joints.end());
00482 known_controllers_[ci.name_] = ci;
00483 }
00484
00485 for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
00486 for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin() ; jt != known_controllers_.end() ; ++jt)
00487 if (it != jt)
00488 {
00489 std::vector<std::string> intersect;
00490 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(),
00491 jt->second.joints_.begin(), jt->second.joints_.end(),
00492 std::back_inserter(intersect));
00493 if (!intersect.empty())
00494 {
00495 it->second.overlapping_controllers_.insert(jt->first);
00496 jt->second.overlapping_controllers_.insert(it->first);
00497 }
00498 }
00499 }
00500 }
00501
00502 void TrajectoryExecutionManager::updateControllerState(const std::string &controller, const ros::Duration &age)
00503 {
00504 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
00505 if (it != known_controllers_.end())
00506 updateControllerState(it->second, age);
00507 else
00508 ROS_ERROR("Controller '%s' is not known.", controller.c_str());
00509 }
00510
00511 void TrajectoryExecutionManager::updateControllerState(ControllerInformation &ci, const ros::Duration &age)
00512 {
00513 if (ros::Time::now() - ci.last_update_ >= age)
00514 {
00515 if (controller_manager_)
00516 {
00517 if (verbose_)
00518 ROS_INFO("Updating information for controller '%s'.", ci.name_.c_str());
00519 ci.state_ = controller_manager_->getControllerState(ci.name_);
00520 ci.last_update_ = ros::Time::now();
00521 }
00522 }
00523 else
00524 if (verbose_)
00525 ROS_INFO("Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
00526 }
00527
00528 void TrajectoryExecutionManager::updateControllersState(const ros::Duration &age)
00529 {
00530 for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
00531 updateControllerState(it->second, age);
00532 }
00533
00534 bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string> &selected, const std::set<std::string> &actuated_joints)
00535 {
00536 std::set<std::string> combined_joints;
00537 for (std::size_t i = 0 ; i < selected.size() ; ++i)
00538 {
00539 const ControllerInformation &ci = known_controllers_[selected[i]];
00540 combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
00541 }
00542
00543 if (verbose_)
00544 {
00545 std::stringstream ss, saj, sac;
00546 for (std::size_t i = 0 ; i < selected.size() ; ++i)
00547 ss << selected[i] << " ";
00548 for (std::set<std::string>::const_iterator it = actuated_joints.begin() ; it != actuated_joints.end() ; ++it)
00549 saj << *it << " ";
00550 for (std::set<std::string>::const_iterator it = combined_joints.begin() ; it != combined_joints.end() ; ++it)
00551 sac << *it << " ";
00552 ROS_INFO("Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]", ss.str().c_str(), sac.str().c_str(), saj.str().c_str());
00553 }
00554
00555 return std::includes(combined_joints.begin(), combined_joints.end(),
00556 actuated_joints.begin(), actuated_joints.end());
00557 }
00558
00559 void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
00560 const std::vector<std::string> &available_controllers,
00561 std::vector<std::string> &selected_controllers,
00562 std::vector< std::vector<std::string> > &selected_options,
00563 const std::set<std::string> &actuated_joints)
00564 {
00565 if (selected_controllers.size() == controller_count)
00566 {
00567 if (checkControllerCombination(selected_controllers, actuated_joints))
00568 selected_options.push_back(selected_controllers);
00569 return;
00570 }
00571
00572 for (std::size_t i = start_index ; i < available_controllers.size() ; ++i)
00573 {
00574 bool overlap = false;
00575 const ControllerInformation &ci = known_controllers_[available_controllers[i]];
00576 for (std::size_t j = 0 ; j < selected_controllers.size() && !overlap ; ++j)
00577 {
00578 if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
00579 overlap = true;
00580 }
00581 if (overlap)
00582 continue;
00583 selected_controllers.push_back(available_controllers[i]);
00584 generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers, selected_options, actuated_joints);
00585 selected_controllers.pop_back();
00586 }
00587 }
00588
00589 namespace
00590 {
00591 struct OrderPotentialControllerCombination
00592 {
00593 bool operator()(const std::size_t a, const std::size_t b) const
00594 {
00595
00596 if (nrdefault[a] > nrdefault[b])
00597 return true;
00598 if (nrdefault[a] < nrdefault[b])
00599 return false;
00600
00601
00602 if (nrjoints[a] < nrjoints[b])
00603 return true;
00604 if (nrjoints[a] > nrjoints[b])
00605 return false;
00606
00607
00608 if (nractive[a] < nractive[b])
00609 return true;
00610 if (nractive[a] > nractive[b])
00611 return false;
00612
00613 return false;
00614 }
00615
00616 std::vector< std::vector<std::string> > selected_options;
00617 std::vector<std::size_t> nrdefault;
00618 std::vector<std::size_t> nrjoints;
00619 std::vector<std::size_t> nractive;
00620 };
00621 }
00622
00623 bool TrajectoryExecutionManager::findControllers(const std::set<std::string> &actuated_joints, std::size_t controller_count, const std::vector<std::string> &available_controllers, std::vector<std::string> &selected_controllers)
00624 {
00625
00626 std::vector<std::string> work_area;
00627 OrderPotentialControllerCombination order;
00628 std::vector< std::vector<std::string> > &selected_options = order.selected_options;
00629 generateControllerCombination(0, controller_count, available_controllers, work_area, selected_options, actuated_joints);
00630
00631 if (verbose_)
00632 {
00633 std::stringstream saj;
00634 std::stringstream sac;
00635 for (std::size_t i = 0 ; i < available_controllers.size() ; ++i)
00636 sac << available_controllers[i] << " ";
00637 for (std::set<std::string>::const_iterator it = actuated_joints.begin() ; it != actuated_joints.end() ; ++it)
00638 saj << *it << " ";
00639 ROS_INFO("Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.", controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
00640 }
00641
00642
00643 if (selected_options.empty())
00644 return false;
00645
00646
00647 if (selected_options.size() == 1)
00648 {
00649 selected_controllers.swap(selected_options[0]);
00650 return true;
00651 }
00652
00653
00654
00655
00656
00657 order.nrdefault.resize(selected_options.size(), 0);
00658 order.nrjoints.resize(selected_options.size(), 0);
00659 order.nractive.resize(selected_options.size(), 0);
00660 for (std::size_t i = 0 ; i < selected_options.size() ; ++i)
00661 {
00662 for (std::size_t k = 0 ; k < selected_options[i].size() ; ++k)
00663 {
00664 updateControllerState(selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
00665 const ControllerInformation &ci = known_controllers_[selected_options[i][k]];
00666
00667 if (ci.state_.default_)
00668 order.nrdefault[i]++;
00669 if (ci.state_.active_)
00670 order.nractive[i]++;
00671 order.nrjoints[i] += ci.joints_.size();
00672 }
00673 }
00674
00675
00676 std::vector<std::size_t> bijection(selected_options.size(), 0);
00677 for (std::size_t i = 0 ; i < selected_options.size() ; ++i)
00678 bijection[i] = i;
00679
00680
00681 std::sort(bijection.begin(), bijection.end(), order);
00682
00683
00684
00685 if (!manage_controllers_)
00686 {
00687
00688 for (std::size_t i = 0 ; i < selected_options.size() ; ++i)
00689 if (areControllersActive(selected_options[bijection[i]]))
00690 {
00691 selected_controllers.swap(selected_options[bijection[i]]);
00692 return true;
00693 }
00694 }
00695
00696
00697 selected_controllers.swap(selected_options[bijection[0]]);
00698 return true;
00699 }
00700
00701 bool TrajectoryExecutionManager::isControllerActive(const std::string &controller)
00702 {
00703 return areControllersActive(std::vector<std::string>(1, controller));
00704 }
00705
00706 bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string> &controllers)
00707 {
00708 for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00709 {
00710 updateControllerState(controllers[i], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
00711 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
00712 if (it == known_controllers_.end() || !it->second.state_.active_)
00713 return false;
00714 }
00715 return true;
00716 }
00717
00718 bool TrajectoryExecutionManager::selectControllers(const std::set<std::string> &actuated_joints, const std::vector<std::string> &available_controllers, std::vector<std::string> &selected_controllers)
00719 {
00720 for (std::size_t i = 1 ; i <= available_controllers.size() ; ++i)
00721 if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
00722 {
00723
00724 if (!manage_controllers_ && !areControllersActive(selected_controllers))
00725 {
00726 std::vector<std::string> other_option;
00727 for (std::size_t j = i + 1 ; j <= available_controllers.size() ; ++j)
00728 if (findControllers(actuated_joints, j, available_controllers, other_option))
00729 {
00730 if (areControllersActive(other_option))
00731 {
00732 selected_controllers = other_option;
00733 break;
00734 }
00735 }
00736 }
00737 return true;
00738 }
00739 return false;
00740 }
00741
00742 bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers, std::vector<moveit_msgs::RobotTrajectory> &parts)
00743 {
00744 parts.clear();
00745 parts.resize(controllers.size());
00746
00747 std::set<std::string> actuated_joints_mdof;
00748 actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
00749 trajectory.multi_dof_joint_trajectory.joint_names.end());
00750 std::set<std::string> actuated_joints_single;
00751 for (std::size_t i = 0 ; i < trajectory.joint_trajectory.joint_names.size() ; ++i)
00752 {
00753 const robot_model::JointModel *jm = robot_model_->getJointModel(trajectory.joint_trajectory.joint_names[i]);
00754 if (jm)
00755 {
00756 if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
00757 continue;
00758 actuated_joints_single.insert(jm->getName());
00759 }
00760 }
00761
00762 for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00763 {
00764 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
00765 if (it == known_controllers_.end())
00766 {
00767 ROS_ERROR_STREAM("Controller " << controllers[i] << " not found.");
00768 return false;
00769 }
00770 std::vector<std::string> intersect_mdof;
00771 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(),
00772 actuated_joints_mdof.begin(), actuated_joints_mdof.end(),
00773 std::back_inserter(intersect_mdof));
00774 std::vector<std::string> intersect_single;
00775 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(),
00776 actuated_joints_single.begin(), actuated_joints_single.end(),
00777 std::back_inserter(intersect_single));
00778 if (intersect_mdof.empty() && intersect_single.empty())
00779 ROS_WARN_STREAM("No joints to be distributed for controller " << controllers[i]);
00780 {
00781 if (!intersect_mdof.empty())
00782 {
00783 std::vector<std::string> &jnames = parts[i].multi_dof_joint_trajectory.joint_names;
00784 jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
00785 std::map<std::string, std::size_t> index;
00786 for (std::size_t j = 0 ; j < trajectory.multi_dof_joint_trajectory.joint_names.size() ; ++j)
00787 index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
00788 std::vector<std::size_t> bijection(jnames.size());
00789 for (std::size_t j = 0 ; j < jnames.size() ; ++j)
00790 bijection[j] = index[jnames[j]];
00791
00792 parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
00793 for (std::size_t j = 0 ; j < trajectory.multi_dof_joint_trajectory.points.size() ; ++j)
00794 {
00795 parts[i].multi_dof_joint_trajectory.points[j].time_from_start = trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
00796 parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
00797 for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00798 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] = trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
00799 }
00800 }
00801 if (!intersect_single.empty())
00802 {
00803 std::vector<std::string> &jnames = parts[i].joint_trajectory.joint_names;
00804 jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
00805 parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
00806 std::map<std::string, std::size_t> index;
00807 for (std::size_t j = 0 ; j < trajectory.joint_trajectory.joint_names.size() ; ++j)
00808 index[trajectory.joint_trajectory.joint_names[j]] = j;
00809 std::vector<std::size_t> bijection(jnames.size());
00810 for (std::size_t j = 0 ; j < jnames.size() ; ++j)
00811 bijection[j] = index[jnames[j]];
00812 parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
00813 for (std::size_t j = 0 ; j < trajectory.joint_trajectory.points.size() ; ++j)
00814 {
00815 parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
00816 if (!trajectory.joint_trajectory.points[j].positions.empty())
00817 {
00818 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
00819 for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00820 parts[i].joint_trajectory.points[j].positions[k] = trajectory.joint_trajectory.points[j].positions[bijection[k]];
00821 }
00822 if (!trajectory.joint_trajectory.points[j].velocities.empty())
00823 {
00824 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
00825 for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00826 parts[i].joint_trajectory.points[j].velocities[k] = trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
00827 }
00828 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
00829 {
00830 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
00831 for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00832 parts[i].joint_trajectory.points[j].accelerations[k] = trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
00833 }
00834 if (!trajectory.joint_trajectory.points[j].effort.empty())
00835 {
00836 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
00837 for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00838 parts[i].joint_trajectory.points[j].effort[k] = trajectory.joint_trajectory.points[j].effort[bijection[k]];
00839 }
00840 }
00841 }
00842 }
00843 }
00844 return true;
00845 }
00846
00847 bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
00848 {
00849 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
00850 {
00851 ROS_WARN("The trajectory to execute is empty");
00852 return false;
00853 }
00854 std::set<std::string> actuated_joints;
00855 actuated_joints.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
00856 trajectory.multi_dof_joint_trajectory.joint_names.end());
00857 actuated_joints.insert(trajectory.joint_trajectory.joint_names.begin(),
00858 trajectory.joint_trajectory.joint_names.end());
00859 if (actuated_joints.empty())
00860 {
00861 ROS_WARN("The trajectory to execute specifies no joints");
00862 return false;
00863 }
00864
00865 if (controllers.empty())
00866 {
00867 bool retry = true;
00868 bool reloaded = false;
00869 while (retry)
00870 {
00871 retry = false;
00872 std::vector<std::string> all_controller_names;
00873 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
00874 all_controller_names.push_back(it->first);
00875 if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
00876 {
00877 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
00878 return true;
00879 }
00880 else
00881 {
00882
00883 if (!reloaded)
00884 {
00885 reloadControllerInformation();
00886 reloaded = true;
00887 retry = true;
00888 }
00889 }
00890 }
00891 }
00892 else
00893 {
00894
00895
00896 bool reloaded = false;
00897 for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00898 if (known_controllers_.find(controllers[i]) == known_controllers_.end())
00899 {
00900 reloadControllerInformation();
00901 reloaded = true;
00902 break;
00903 }
00904 if (reloaded)
00905 for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00906 if (known_controllers_.find(controllers[i]) == known_controllers_.end())
00907 {
00908 ROS_ERROR("Controller '%s' is not known", controllers[i].c_str());
00909 return false;
00910 }
00911 if (selectControllers(actuated_joints, controllers, context.controllers_))
00912 {
00913 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
00914 return true;
00915 }
00916 }
00917 std::stringstream ss;
00918 for (std::set<std::string>::const_iterator it = actuated_joints.begin() ; it != actuated_joints.end() ; ++it)
00919 ss << *it << " ";
00920 ROS_ERROR("Unable to identify any set of controllers that can actuate the specified joints: [ %s]", ss.str().c_str());
00921
00922 std::stringstream ss2;
00923 std::map<std::string, ControllerInformation>::const_iterator mi;
00924 for(mi = known_controllers_.begin(); mi != known_controllers_.end(); mi++)
00925 {
00926 ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
00927
00928 std::set<std::string>::const_iterator ji;
00929 for(ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
00930 {
00931 ss2 << " " << *ji << std::endl;
00932 }
00933 }
00934 ROS_ERROR("Known controllers and their joints:\n%s", ss2.str().c_str());
00935 return false;
00936 }
00937
00938 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::executeAndWait(bool auto_clear)
00939 {
00940 execute(ExecutionCompleteCallback(), auto_clear);
00941 return waitForExecution();
00942 }
00943
00944 void TrajectoryExecutionManager::stopExecutionInternal()
00945 {
00946
00947 for (std::size_t i = 0 ; i < active_handles_.size() ; ++i)
00948 try
00949 {
00950 active_handles_[i]->cancelExecution();
00951 }
00952 catch(...)
00953 {
00954 ROS_ERROR("Exception caught when canceling execution.");
00955 }
00956 }
00957
00958 void TrajectoryExecutionManager::stopExecution(bool auto_clear)
00959 {
00960 stop_continuous_execution_ = true;
00961 continuous_execution_condition_.notify_all();
00962
00963 if (!execution_complete_)
00964 {
00965 execution_state_mutex_.lock();
00966 if (!execution_complete_)
00967 {
00968
00969
00970 execution_complete_ = true;
00971 stopExecutionInternal();
00972
00973
00974 last_execution_status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
00975 execution_state_mutex_.unlock();
00976 ROS_INFO("Stopped trajectory execution.");
00977
00978
00979 execution_thread_->join();
00980 execution_thread_.reset();
00981
00982 if (auto_clear)
00983 clear();
00984 }
00985 else
00986 execution_state_mutex_.unlock();
00987 }
00988 else
00989 if (execution_thread_)
00990 {
00991 execution_thread_->join();
00992 execution_thread_.reset();
00993 }
00994 }
00995
00996 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback &callback, bool auto_clear)
00997 {
00998 execute(callback, PathSegmentCompleteCallback(), auto_clear);
00999 }
01000
01001 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
01002 {
01003 stopExecution(false);
01004 execution_complete_ = false;
01005
01006 execution_thread_.reset(new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
01007 }
01008
01009 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::waitForExecution()
01010 {
01011 {
01012 boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
01013 while (!execution_complete_)
01014 execution_complete_condition_.wait(ulock);
01015 }
01016 {
01017 boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
01018 while (!continuous_execution_queue_.empty())
01019 continuous_execution_condition_.wait(ulock);
01020 }
01021
01022
01023 stopExecution(false);
01024
01025 return last_execution_status_;
01026 }
01027
01028 void TrajectoryExecutionManager::clear()
01029 {
01030 if (execution_complete_)
01031 {
01032 for (std::size_t i = 0 ; i < trajectories_.size() ; ++i)
01033 delete trajectories_[i];
01034 trajectories_.clear();
01035 {
01036 boost::mutex::scoped_lock slock(continuous_execution_mutex_);
01037 while (!continuous_execution_queue_.empty())
01038 {
01039 delete continuous_execution_queue_.front();
01040 continuous_execution_queue_.pop_front();
01041 }
01042 }
01043 }
01044 else
01045 ROS_ERROR("Cannot push a new trajectory while another is being executed");
01046 }
01047
01048 void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
01049 {
01050
01051 if (execution_complete_)
01052 {
01053 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01054 if (callback)
01055 callback(last_execution_status_);
01056 return;
01057 }
01058
01059 ROS_DEBUG("Starting trajectory execution ...");
01060
01061 last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
01062
01063
01064
01065 for (std::size_t i = 0 ; i < trajectories_.size() ; ++i)
01066 {
01067 bool epart = executePart(i);
01068 if (epart && part_callback)
01069 part_callback(i);
01070 if (!epart || execution_complete_)
01071 break;
01072 }
01073
01074 ROS_DEBUG("Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());
01075
01076
01077 execution_state_mutex_.lock();
01078 execution_complete_ = true;
01079 execution_state_mutex_.unlock();
01080 execution_complete_condition_.notify_all();
01081
01082
01083 if (auto_clear)
01084 clear();
01085
01086
01087 if (callback)
01088 callback(last_execution_status_);
01089 }
01090
01091 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
01092 {
01093 TrajectoryExecutionContext &context = *trajectories_[part_index];
01094
01095
01096 if (ensureActiveControllers(context.controllers_))
01097 {
01098
01099 if (execution_complete_)
01100 return false;
01101
01102 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
01103 {
01104 boost::mutex::scoped_lock slock(execution_state_mutex_);
01105 if (!execution_complete_)
01106 {
01107
01108 time_index_mutex_.lock();
01109 current_context_ = part_index;
01110 time_index_mutex_.unlock();
01111 active_handles_.resize(context.controllers_.size());
01112 for (std::size_t i = 0 ; i < context.controllers_.size() ; ++i)
01113 {
01114 moveit_controller_manager::MoveItControllerHandlePtr h;
01115 try
01116 {
01117 h = controller_manager_->getControllerHandle(context.controllers_[i]);
01118 }
01119 catch(...)
01120 {
01121 ROS_ERROR("Exception caught when retrieving controller handle");
01122 }
01123 if (!h)
01124 {
01125 active_handles_.clear();
01126 current_context_ = -1;
01127 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01128 ROS_ERROR("No controller handle for controller '%s'. Aborting.", context.controllers_[i].c_str());
01129 return false;
01130 }
01131 active_handles_[i] = h;
01132 }
01133 handles = active_handles_;
01134 for (std::size_t i = 0 ; i < context.trajectory_parts_.size() ; ++i)
01135 {
01136 bool ok = false;
01137 try
01138 {
01139 ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
01140 }
01141 catch(...)
01142 {
01143 ROS_ERROR("Exception caught when sending trajectory to controller");
01144 }
01145 if (!ok)
01146 {
01147 for (std::size_t j = 0 ; j < i ; ++j)
01148 try
01149 {
01150 active_handles_[j]->cancelExecution();
01151 }
01152 catch(...)
01153 {
01154 ROS_ERROR("Exception caught when canceling execution");
01155 }
01156 ROS_ERROR("Failed to send trajectory part %zu of %zu to controller %s", i + 1, context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
01157 if (i > 0)
01158 ROS_ERROR("Cancelling previously sent trajectory parts");
01159 active_handles_.clear();
01160 current_context_ = -1;
01161 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01162 return false;
01163 }
01164 }
01165 }
01166 }
01167
01168
01169 ros::Time current_time = ros::Time::now();
01170 ros::Duration expected_trajectory_duration(0.0);
01171 int longest_part = -1;
01172 for (std::size_t i = 0 ; i < context.trajectory_parts_.size() ; ++i)
01173 {
01174 ros::Duration d(0.0);
01175 if (!context.trajectory_parts_[i].joint_trajectory.points.empty())
01176 {
01177 if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
01178 d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
01179 if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
01180 d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
01181 d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ? ros::Duration(0.0) :
01182 context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
01183 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ? ros::Duration(0.0) :
01184 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
01185
01186 if (longest_part < 0 ||
01187 std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
01188 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
01189 std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
01190 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
01191 longest_part = i;
01192 }
01193 expected_trajectory_duration = std::max(d, expected_trajectory_duration);
01194 }
01195
01196
01197 expected_trajectory_duration = expected_trajectory_duration * allowed_execution_duration_scaling_ + ros::Duration(allowed_goal_duration_margin_);
01198
01199 if (longest_part >= 0)
01200 {
01201 boost::mutex::scoped_lock slock(time_index_mutex_);
01202
01203
01204 if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >= context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
01205 {
01206 ros::Duration d(0.0);
01207 if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
01208 d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
01209 for (std::size_t j = 0 ; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size() ; ++j)
01210 time_index_.push_back(current_time + d + context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
01211 }
01212 else
01213 {
01214 ros::Duration d(0.0);
01215 if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
01216 d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
01217 for (std::size_t j = 0 ; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size() ; ++j)
01218 time_index_.push_back(current_time + d + context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
01219 }
01220 }
01221
01222 bool result = true;
01223 for (std::size_t i = 0 ; i < handles.size() ; ++i)
01224 {
01225 if (execution_duration_monitoring_)
01226 {
01227 if (!handles[i]->waitForExecution(expected_trajectory_duration))
01228 if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
01229 {
01230 ROS_ERROR("Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was %lf seconds). Stopping trajectory.", expected_trajectory_duration.toSec());
01231 {
01232 boost::mutex::scoped_lock slock(execution_state_mutex_);
01233 stopExecutionInternal();
01234 }
01235 last_execution_status_ = moveit_controller_manager::ExecutionStatus::TIMED_OUT;
01236 result = false;
01237 break;
01238 }
01239 }
01240 else
01241 handles[i]->waitForExecution();
01242
01243
01244 if (execution_complete_)
01245 {
01246 result = false;
01247 break;
01248 }
01249 else
01250 if (handles[i]->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::SUCCEEDED)
01251 {
01252 ROS_WARN_STREAM("Controller handle " << handles[i]->getName() << " reports status "
01253 << handles[i]->getLastExecutionStatus().asString());
01254 last_execution_status_ = handles[i]->getLastExecutionStatus();
01255 result = false;
01256 }
01257 }
01258
01259
01260 execution_state_mutex_.lock();
01261 active_handles_.clear();
01262
01263
01264 time_index_mutex_.lock();
01265 time_index_.clear();
01266 current_context_ = -1;
01267 time_index_mutex_.unlock();
01268
01269 execution_state_mutex_.unlock();
01270 return result;
01271 }
01272 else
01273 {
01274 last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01275 return false;
01276 }
01277 }
01278
01279 std::pair<int, int> TrajectoryExecutionManager::getCurrentExpectedTrajectoryIndex() const
01280 {
01281 boost::mutex::scoped_lock slock(time_index_mutex_);
01282 if (current_context_ < 0)
01283 return std::make_pair(-1, -1);
01284 if (time_index_.empty())
01285 return std::make_pair((int)current_context_, -1);
01286 std::vector<ros::Time>::const_iterator it = std::lower_bound(time_index_.begin(), time_index_.end(), ros::Time::now());
01287 int pos = it - time_index_.begin();
01288 return std::make_pair((int)current_context_, pos);
01289 }
01290
01291 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>& TrajectoryExecutionManager::getTrajectories() const
01292 {
01293 return trajectories_;
01294 }
01295
01296 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::getLastExecutionStatus() const
01297 {
01298 return last_execution_status_;
01299 }
01300
01301 bool TrajectoryExecutionManager::ensureActiveControllersForGroup(const std::string &group)
01302 {
01303 const robot_model::JointModelGroup *joint_model_group = robot_model_->getJointModelGroup(group);
01304 if (joint_model_group)
01305 return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
01306 else
01307 return false;
01308 }
01309
01310 bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string> &joints)
01311 {
01312 std::vector<std::string> all_controller_names;
01313 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
01314 all_controller_names.push_back(it->first);
01315 std::vector<std::string> selected_controllers;
01316 std::set<std::string> jset;
01317 for (std::size_t i = 0 ; i < joints.size() ; ++i)
01318 {
01319 const robot_model::JointModel *jm = robot_model_->getJointModel(joints[i]);
01320 if (jm)
01321 {
01322 if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
01323 continue;
01324 jset.insert(joints[i]);
01325 }
01326 }
01327
01328 if (selectControllers(jset, all_controller_names, selected_controllers))
01329 return ensureActiveControllers(selected_controllers);
01330 else
01331 return false;
01332 }
01333
01334 bool TrajectoryExecutionManager::ensureActiveController(const std::string &controller)
01335 {
01336 return ensureActiveControllers(std::vector<std::string>(1, controller));
01337 }
01338
01339 bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string> &controllers)
01340 {
01341 updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
01342
01343 if (manage_controllers_)
01344 {
01345 std::vector<std::string> controllers_to_activate;
01346 std::vector<std::string> controllers_to_deactivate;
01347 std::set<std::string> joints_to_be_activated;
01348 std::set<std::string> joints_to_be_deactivated;
01349 for (std::size_t i = 0 ; i < controllers.size() ; ++i)
01350 {
01351 std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controllers[i]);
01352 if (it == known_controllers_.end())
01353 {
01354 ROS_ERROR_STREAM("Controller " << controllers[i] << " is not known");
01355 return false;
01356 }
01357 if (!it->second.state_.active_)
01358 {
01359 ROS_DEBUG_STREAM("Need to activate " << controllers[i]);
01360 controllers_to_activate.push_back(controllers[i]);
01361 joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
01362 for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin() ;
01363 kt != it->second.overlapping_controllers_.end() ; ++kt)
01364 {
01365 const ControllerInformation &ci = known_controllers_[*kt];
01366 if (ci.state_.active_)
01367 {
01368 controllers_to_deactivate.push_back(*kt);
01369 joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
01370 }
01371 }
01372 }
01373 else
01374 ROS_DEBUG_STREAM("Controller " << controllers[i] << " is already active");
01375 }
01376 std::set<std::string> diff;
01377 std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
01378 joints_to_be_activated.begin(), joints_to_be_activated.end(),
01379 std::inserter(diff, diff.end()));
01380 if (!diff.empty())
01381 {
01382
01383 std::vector<std::string> possible_additional_controllers;
01384 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
01385 {
01386 bool ok = true;
01387 for (std::size_t k = 0 ; k < controllers_to_activate.size() ; ++k)
01388 if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) != it->second.overlapping_controllers_.end())
01389 {
01390 ok = false;
01391 break;
01392 }
01393 if (ok)
01394 possible_additional_controllers.push_back(it->first);
01395 }
01396
01397
01398 std::vector<std::string> additional_controllers;
01399 if (selectControllers(diff, possible_additional_controllers, additional_controllers))
01400 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(), additional_controllers.end());
01401 else
01402 return false;
01403 }
01404 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
01405 {
01406 if (controller_manager_)
01407 {
01408
01409 for (std::size_t a = 0 ; a < controllers_to_activate.size() ; ++a)
01410 {
01411 ControllerInformation &ci = known_controllers_[controllers_to_activate[a]];
01412 ci.last_update_ = ros::Time();
01413 }
01414
01415 for (std::size_t a = 0 ; a < controllers_to_deactivate.size() ; ++a)
01416 known_controllers_[controllers_to_deactivate[a]].last_update_ = ros::Time();
01417 return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
01418 }
01419 else
01420 return false;
01421 }
01422 else
01423 return true;
01424 }
01425 else
01426 {
01427 std::set<std::string> originally_active;
01428 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
01429 if (it->second.state_.active_)
01430 originally_active.insert(it->first);
01431 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
01432 }
01433 }
01434
01435 }