trajectory_execution_manager.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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 ros::Duration DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN(0.5); // allow 0.5s more than the expected execution time before triggering a trajectory cancel (applied after scaling)
00048 static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = 1.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 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   kinematic_model_(kmodel), node_handle_("~")
00076 {
00077   if (!node_handle_.getParam("moveit_manage_controllers", manage_controllers_))
00078     manage_controllers_ = false;
00079   initialize();
00080 }
00081 
00082 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, bool manage_controllers) :
00083   kinematic_model_(kmodel), node_handle_("~"), manage_controllers_(manage_controllers)
00084 {
00085   initialize();
00086 }
00087 
00088 TrajectoryExecutionManager::~TrajectoryExecutionManager()
00089 {
00090   run_continuous_execution_thread_ = false;
00091   stopExecution(true);
00092   delete reconfigure_impl_;
00093 }
00094 
00095 void TrajectoryExecutionManager::initialize()
00096 {
00097   reconfigure_impl_ = NULL;
00098   verbose_ = false;
00099   execution_complete_ = true;
00100   stop_continuous_execution_ = false;
00101   current_context_ = -1;
00102   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00103   run_continuous_execution_thread_ = true;
00104   allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
00105   execution_duration_monitoring_ = true;
00106   execution_velocity_scaling_ = 1.0;
00107 
00108   // load the controller manager plugin
00109   try
00110   {
00111     controller_manager_loader_.reset(new pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>("moveit_core", "moveit_controller_manager::MoveItControllerManager"));
00112   }
00113   catch(pluginlib::PluginlibException& ex)
00114   {
00115     ROS_FATAL_STREAM("Exception while creating controller manager plugin loader: " << ex.what());
00116     return;
00117   }
00118 
00119   if (controller_manager_loader_)
00120   {
00121     std::string controller;
00122     if (!node_handle_.getParam("moveit_controller_manager", controller))
00123     {
00124       const std::vector<std::string> &classes = controller_manager_loader_->getDeclaredClasses();
00125       if (classes.size() == 1)
00126       {
00127         controller = classes[0];
00128         ROS_WARN("Parameter '~moveit_controller_manager' is not specified but only one matching plugin was found: '%s'. Using that one.", controller.c_str());
00129       }
00130       else
00131         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.");
00132     }
00133 
00134     if (!controller.empty())
00135       try
00136       {
00137         controller_manager_.reset(controller_manager_loader_->createUnmanagedInstance(controller));
00138       }
00139       catch(pluginlib::PluginlibException& ex)
00140       {
00141         ROS_FATAL_STREAM("Exception while loading controller manager '" << controller << "': " << ex.what());
00142       }
00143   }
00144 
00145   // other configuration steps
00146   reloadControllerInformation();
00147 
00148   event_topic_subscriber_ = root_node_handle_.subscribe(EXECUTION_EVENT_TOPIC, 100, &TrajectoryExecutionManager::receiveEvent, this);
00149 
00150   reconfigure_impl_ = new DynamicReconfigureImpl(this);
00151 
00152   if (manage_controllers_)
00153     ROS_INFO("Trajectory execution is managing controllers");
00154   else
00155     ROS_INFO("Trajectory execution is not managing controllers");
00156 }
00157 
00158 void TrajectoryExecutionManager::enableExecutionDurationMonitoring(bool flag)
00159 {
00160   execution_duration_monitoring_ = flag;
00161 }
00162 
00163 void TrajectoryExecutionManager::setAllowedExecutionDurationScaling(double scaling)
00164 {
00165   allowed_execution_duration_scaling_ = scaling;
00166 }
00167 
00168 void TrajectoryExecutionManager::setExecutionVelocityScaling(double scaling)
00169 {
00170   execution_velocity_scaling_ = scaling;
00171 }
00172 
00173 bool TrajectoryExecutionManager::isManagingControllers() const
00174 {
00175   return manage_controllers_;
00176 }
00177 
00178 const moveit_controller_manager::MoveItControllerManagerPtr& TrajectoryExecutionManager::getControllerManager() const
00179 {
00180   return controller_manager_;
00181 }
00182 
00183 void TrajectoryExecutionManager::processEvent(const std::string &event)
00184 {
00185   if (event == "stop")
00186     stopExecution(true);
00187   else
00188     ROS_WARN_STREAM("Unknown event type: '" << event << "'");
00189 }
00190 
00191 void TrajectoryExecutionManager::receiveEvent(const std_msgs::StringConstPtr &event)
00192 {
00193   ROS_INFO_STREAM("Received event '" << event->data << "'");
00194   processEvent(event->data);
00195 }
00196 
00197 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller)
00198 {
00199   if (controller.empty())
00200     return push(trajectory, std::vector<std::string>());
00201   else
00202     return push(trajectory, std::vector<std::string>(1, controller));
00203 }
00204 
00205 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller)
00206 {
00207   if (controller.empty())
00208     return push(trajectory, std::vector<std::string>());
00209   else
00210     return push(trajectory, std::vector<std::string>(1, controller));
00211 }
00212 
00213 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers)
00214 {
00215   moveit_msgs::RobotTrajectory traj;
00216   traj.joint_trajectory = trajectory;
00217   return push(traj, controllers);
00218 }
00219 
00220 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
00221 {
00222   if (!execution_complete_)
00223   {
00224     ROS_ERROR("Cannot push a new trajectory while another is being executed");
00225     return false;
00226   }
00227 
00228   TrajectoryExecutionContext *context = new TrajectoryExecutionContext();
00229   if (configure(*context, trajectory, controllers))
00230   {
00231     if (verbose_)
00232     {
00233       std::stringstream ss;
00234       ss << "Pushed trajectory for execution using controllers [ ";
00235       for (std::size_t i = 0 ; i < context->controllers_.size() ; ++i)
00236         ss << context->controllers_[i] << " ";
00237       ss << "]:" << std::endl;
00238       for (std::size_t i = 0 ; i < context->trajectory_parts_.size() ; ++i)
00239         ss << context->trajectory_parts_[i] << std::endl;
00240       ROS_INFO("%s", ss.str().c_str());
00241     }
00242     trajectories_.push_back(context);
00243     return true;
00244   }
00245   else
00246   {
00247     delete context;
00248     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00249   }
00250 
00251   return false;
00252 }
00253 
00254 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller)
00255 {
00256   if (controller.empty())
00257     return pushAndExecute(trajectory, std::vector<std::string>());
00258   else
00259     return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
00260 }
00261 
00262 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller)
00263 {
00264   if (controller.empty())
00265     return pushAndExecute(trajectory, std::vector<std::string>());
00266   else
00267     return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
00268 }
00269 
00270 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState &state, const std::string &controller)
00271 {
00272   if (controller.empty())
00273     return pushAndExecute(state, std::vector<std::string>());
00274   else
00275     return pushAndExecute(state, std::vector<std::string>(1, controller));
00276 }
00277 
00278 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers)
00279 {
00280   moveit_msgs::RobotTrajectory traj;
00281   traj.joint_trajectory = trajectory;
00282   return pushAndExecute(traj, controllers);
00283 }
00284 
00285 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState &state, const std::vector<std::string> &controllers)
00286 {
00287   moveit_msgs::RobotTrajectory traj;
00288   traj.joint_trajectory.header = state.header;
00289   traj.joint_trajectory.joint_names = state.name;
00290   traj.joint_trajectory.points.resize(1);
00291   traj.joint_trajectory.points[0].positions = state.position;
00292   traj.joint_trajectory.points[0].velocities = state.velocity;
00293   traj.joint_trajectory.points[0].time_from_start = ros::Duration(0, 0);
00294   return pushAndExecute(traj, controllers);
00295 }
00296 
00297 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
00298 {
00299   if (!execution_complete_)
00300   {
00301     ROS_ERROR("Cannot push & execute a new trajectory while another is being executed");
00302     return false;
00303   }
00304 
00305   TrajectoryExecutionContext *context = new TrajectoryExecutionContext();
00306   if (configure(*context, trajectory, controllers))
00307   {
00308     {
00309       boost::mutex::scoped_lock slock(continuous_execution_mutex_);
00310       continuous_execution_queue_.push_back(context);
00311       if (!continuous_execution_thread_)
00312         continuous_execution_thread_.reset(new boost::thread(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)));
00313     }
00314     last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00315     continuous_execution_condition_.notify_all();
00316     return true;
00317   }
00318   else
00319   {
00320     delete context;
00321     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00322     return false;
00323   }
00324 }
00325 
00326 void TrajectoryExecutionManager::continuousExecutionThread()
00327 {
00328   std::set<moveit_controller_manager::MoveItControllerHandlePtr> used_handles;
00329   while (run_continuous_execution_thread_)
00330   {
00331     if (!stop_continuous_execution_)
00332     {
00333       boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
00334       while (continuous_execution_queue_.empty() && run_continuous_execution_thread_ && !stop_continuous_execution_)
00335         continuous_execution_condition_.wait(ulock);
00336     }
00337 
00338     if (stop_continuous_execution_ || !run_continuous_execution_thread_)
00339     {
00340       for (std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin() ; uit != used_handles.end() ; ++uit)
00341         if ((*uit)->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::RUNNING)
00342           (*uit)->cancelExecution();
00343       used_handles.clear();
00344       while (!continuous_execution_queue_.empty())
00345       {
00346         TrajectoryExecutionContext *context = continuous_execution_queue_.front();
00347         continuous_execution_queue_.pop_front();
00348         delete context;
00349       }
00350       stop_continuous_execution_ = false;
00351       continue;
00352     }
00353 
00354     while (!continuous_execution_queue_.empty())
00355     {
00356       TrajectoryExecutionContext *context = NULL;
00357       {
00358         boost::mutex::scoped_lock slock(continuous_execution_mutex_);
00359         if (continuous_execution_queue_.empty())
00360           break;
00361         context = continuous_execution_queue_.front();
00362         continuous_execution_queue_.pop_front();
00363         if (continuous_execution_queue_.empty())
00364           continuous_execution_condition_.notify_all();
00365       }
00366 
00367       // remove handles we no longer need
00368       std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
00369       while (uit != used_handles.end())
00370         if ((*uit)->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::RUNNING)
00371         {
00372           std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator toErase = uit;
00373           ++uit;
00374           used_handles.erase(toErase);
00375         }
00376         else
00377           ++uit;
00378 
00379       // now send stuff to controllers
00380 
00381       // first make sure desired controllers are active
00382       if (areControllersActive(context->controllers_))
00383       {
00384         // get the controller handles needed to execute the new trajectory
00385         std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles(context->controllers_.size());
00386         for (std::size_t i = 0 ; i < context->controllers_.size() ; ++i)
00387         {
00388           moveit_controller_manager::MoveItControllerHandlePtr h;
00389           try
00390           {
00391             h = controller_manager_->getControllerHandle(context->controllers_[i]);
00392           }
00393           catch(...)
00394           {
00395             ROS_ERROR("Exception caught when retrieving controller handle");
00396           }
00397           if (!h)
00398           {
00399             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00400             ROS_ERROR("No controller handle for controller '%s'. Aborting.", context->controllers_[i].c_str());
00401             handles.clear();
00402             break;
00403           }
00404           handles[i] = h;
00405         }
00406 
00407         if (stop_continuous_execution_ || !run_continuous_execution_thread_)
00408         {
00409           delete context;
00410           break;
00411         }
00412 
00413         // push all trajectories to all controllers simultaneously
00414         if (!handles.empty())
00415           for (std::size_t i = 0 ; i < context->trajectory_parts_.size() ; ++i)
00416           {
00417             bool ok = false;
00418             try
00419             {
00420               ok = handles[i]->sendTrajectory(context->trajectory_parts_[i]);
00421             }
00422             catch(...)
00423             {
00424               ROS_ERROR("Exception caught when sending trajectory to controller");
00425             }
00426             if (!ok)
00427             {
00428               for (std::size_t j = 0 ; j < i ; ++j)
00429                 try
00430                 {
00431                   handles[j]->cancelExecution();
00432                 }
00433                 catch(...)
00434                 {
00435                   ROS_ERROR("Exception caught when canceling execution");
00436                 }
00437               ROS_ERROR("Failed to send trajectory part %lu of %lu to controller %s", i + 1, context->trajectory_parts_.size(), handles[i]->getName().c_str());
00438               if (i > 0)
00439                 ROS_ERROR("Cancelling previously sent trajectory parts");
00440               last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00441               handles.clear();
00442               break;
00443             }
00444           }
00445         delete context;
00446 
00447         // remember which handles we used
00448         for (std::size_t i = 0 ; i < handles.size() ; ++i)
00449           used_handles.insert(handles[i]);
00450       }
00451       else
00452       {
00453         ROS_ERROR("Not all needed controllers are active. Cannot push and execute. You can try calling ensureActiveControllers() before pushAndExecute()");
00454         last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00455         delete context;
00456       }
00457     }
00458   }
00459 }
00460 
00461 void TrajectoryExecutionManager::reloadControllerInformation()
00462 {
00463   known_controllers_.clear();
00464   if (controller_manager_)
00465   {
00466     std::vector<std::string> names;
00467     controller_manager_->getControllersList(names);
00468     for (std::size_t i = 0 ; i < names.size() ; ++i)
00469     {
00470       std::vector<std::string> joints;
00471       controller_manager_->getControllerJoints(names[i], joints);
00472       ControllerInformation ci;
00473       ci.name_ = names[i];
00474       ci.joints_.insert(joints.begin(), joints.end());
00475       known_controllers_[ci.name_] = ci;
00476     }
00477 
00478     for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
00479       for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin() ; jt != known_controllers_.end() ; ++jt)
00480         if (it != jt)
00481         {
00482           std::vector<std::string> intersect;
00483           std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(),
00484                                 jt->second.joints_.begin(), jt->second.joints_.end(),
00485                                 std::back_inserter(intersect));
00486           if (!intersect.empty())
00487           {
00488             it->second.overlapping_controllers_.insert(jt->first);
00489             jt->second.overlapping_controllers_.insert(it->first);
00490           }
00491         }
00492   }
00493 }
00494 
00495 void TrajectoryExecutionManager::updateControllerState(const std::string &controller, const ros::Duration &age)
00496 {
00497   std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
00498   if (it != known_controllers_.end())
00499     updateControllerState(it->second, age);
00500   else
00501     ROS_ERROR("Controller '%s' is not known.", controller.c_str());
00502 }
00503 
00504 void TrajectoryExecutionManager::updateControllerState(ControllerInformation &ci, const ros::Duration &age)
00505 {
00506   if (ros::Time::now() - ci.last_update_ >= age)
00507   {
00508     if (controller_manager_)
00509     {
00510       if (verbose_)
00511         ROS_INFO("Updating information for controller '%s'.", ci.name_.c_str());
00512       ci.state_ = controller_manager_->getControllerState(ci.name_);
00513       ci.last_update_ = ros::Time::now();
00514     }
00515   }
00516   else
00517     if (verbose_)
00518       ROS_INFO("Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
00519 }
00520 
00521 void TrajectoryExecutionManager::updateControllersState(const ros::Duration &age)
00522 {
00523   for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
00524     updateControllerState(it->second, age);
00525 }
00526 
00527 bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string> &selected, const std::set<std::string> &actuated_joints)
00528 {
00529   std::set<std::string> combined_joints;
00530   for (std::size_t i = 0 ; i < selected.size() ; ++i)
00531   {
00532     const ControllerInformation &ci = known_controllers_[selected[i]];
00533     combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
00534   }
00535 
00536   if (verbose_)
00537   {
00538     std::stringstream ss, saj, sac;
00539     for (std::size_t i = 0 ; i < selected.size() ; ++i)
00540       ss << selected[i] << " ";
00541     for (std::set<std::string>::const_iterator it = actuated_joints.begin() ; it != actuated_joints.end() ; ++it)
00542       saj << *it << " ";
00543     for (std::set<std::string>::const_iterator it = combined_joints.begin() ; it != combined_joints.end() ; ++it)
00544       sac << *it << " ";
00545     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());
00546   }
00547 
00548   return std::includes(combined_joints.begin(), combined_joints.end(),
00549                        actuated_joints.begin(), actuated_joints.end());
00550 }
00551 
00552 void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
00553                                                                const std::vector<std::string> &available_controllers,
00554                                                                std::vector<std::string> &selected_controllers,
00555                                                                std::vector< std::vector<std::string> > &selected_options,
00556                                                                const std::set<std::string> &actuated_joints)
00557 {
00558   if (selected_controllers.size() == controller_count)
00559   {
00560     if (checkControllerCombination(selected_controllers, actuated_joints))
00561       selected_options.push_back(selected_controllers);
00562     return;
00563   }
00564 
00565   for (std::size_t i = start_index ; i < available_controllers.size() ; ++i)
00566   {
00567     bool overlap = false;
00568     const ControllerInformation &ci = known_controllers_[available_controllers[i]];
00569     for (std::size_t j = 0 ; j < selected_controllers.size() && !overlap ; ++j)
00570     {
00571       if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
00572         overlap = true;
00573     }
00574     if (overlap)
00575       continue;
00576     selected_controllers.push_back(available_controllers[i]);
00577     generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers, selected_options, actuated_joints);
00578     selected_controllers.pop_back();
00579   }
00580 }
00581 
00582 namespace
00583 {
00584 struct OrderPotentialControllerCombination
00585 {
00586   bool operator()(const std::size_t a, const std::size_t b) const
00587   {
00588     // preference is given to controllers marked as default
00589     if (nrdefault[a] > nrdefault[b])
00590       return true;
00591     if (nrdefault[a] < nrdefault[b])
00592       return false;
00593 
00594     // and then to ones that operate on fewer joints
00595     if (nrjoints[a] < nrjoints[b])
00596       return true;
00597     if (nrjoints[a] > nrjoints[b])
00598       return false;
00599 
00600     // and then to active ones
00601     if (nractive[a] < nractive[b])
00602       return true;
00603     if (nractive[a] > nractive[b])
00604       return false;
00605 
00606     return false;
00607   }
00608 
00609   std::vector< std::vector<std::string> > selected_options;
00610   std::vector<std::size_t> nrdefault;
00611   std::vector<std::size_t> nrjoints;
00612   std::vector<std::size_t> nractive;
00613 };
00614 }
00615 
00616 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)
00617 {
00618   // generate all combinations of controller_count controllers that operate on disjoint sets of joints
00619   std::vector<std::string> work_area;
00620   OrderPotentialControllerCombination order;
00621   std::vector< std::vector<std::string> > &selected_options = order.selected_options;
00622   generateControllerCombination(0, controller_count, available_controllers, work_area, selected_options, actuated_joints);
00623 
00624   if (verbose_)
00625   {
00626     std::stringstream saj;
00627     std::stringstream sac;
00628     for (std::size_t i = 0 ; i < available_controllers.size() ; ++i)
00629       sac << available_controllers[i] << " ";
00630     for (std::set<std::string>::const_iterator it = actuated_joints.begin() ; it != actuated_joints.end() ; ++it)
00631       saj << *it << " ";
00632     ROS_INFO("Looking for %lu controllers among [ %s] that cover joints [ %s]. Found %ld options.", controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
00633   }
00634 
00635   // if none was found, this is a problem
00636   if (selected_options.empty())
00637     return false;
00638 
00639   // if only one was found, return it
00640   if (selected_options.size() == 1)
00641   {
00642     selected_controllers.swap(selected_options[0]);
00643     return true;
00644   }
00645 
00646   // if more options were found, evaluate them all and return the best one
00647 
00648   // count how many default controllers are used in each reported option, and how many joints are actuated in total by the selected controllers,
00649   // to use that in the ranking of the options
00650   order.nrdefault.resize(selected_options.size(), 0);
00651   order.nrjoints.resize(selected_options.size(), 0);
00652   order.nractive.resize(selected_options.size(), 0);
00653   for (std::size_t i = 0 ; i < selected_options.size() ; ++i)
00654   {
00655     for (std::size_t k = 0 ; k < selected_options[i].size() ; ++k)
00656     {
00657       updateControllerState(selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
00658       const ControllerInformation &ci = known_controllers_[selected_options[i][k]];
00659 
00660       if (ci.state_.default_)
00661         order.nrdefault[i]++;
00662       if (ci.state_.active_)
00663         order.nractive[i]++;
00664       order.nrjoints[i] += ci.joints_.size();
00665     }
00666   }
00667 
00668   // define a bijection to compute the raking of the found options
00669   std::vector<std::size_t> bijection(selected_options.size(), 0);
00670   for (std::size_t i = 0 ; i < selected_options.size() ; ++i)
00671     bijection[i] = i;
00672 
00673   // sort the options
00674   std::sort(bijection.begin(), bijection.end(), order);
00675 
00676   // depending on whether we are allowed to load & unload controllers,
00677   // we have different preference on deciding between options
00678   if (!manage_controllers_)
00679   {
00680     // if we can't load different options at will, just choose one that is already loaded
00681     for (std::size_t i = 0 ; i < selected_options.size() ; ++i)
00682       if (areControllersActive(selected_options[bijection[i]]))
00683       {
00684         selected_controllers.swap(selected_options[bijection[i]]);
00685         return true;
00686       }
00687   }
00688 
00689   // otherwise, just use the first valid option
00690   selected_controllers.swap(selected_options[bijection[0]]);
00691   return true;
00692 }
00693 
00694 bool TrajectoryExecutionManager::isControllerActive(const std::string &controller)
00695 {
00696   return areControllersActive(std::vector<std::string>(1, controller));
00697 }
00698 
00699 bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string> &controllers)
00700 {
00701   for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00702   {
00703     updateControllerState(controllers[i], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
00704     std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
00705     if (it == known_controllers_.end() || !it->second.state_.active_)
00706       return false;
00707   }
00708   return true;
00709 }
00710 
00711 bool TrajectoryExecutionManager::selectControllers(const std::set<std::string> &actuated_joints, const std::vector<std::string> &available_controllers, std::vector<std::string> &selected_controllers)
00712 {
00713   for (std::size_t i = 1 ; i <= available_controllers.size() ; ++i)
00714     if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
00715     {
00716       // if we are not managing controllers, prefer to use active controllers even if there are more of them
00717       if (!manage_controllers_ && !areControllersActive(selected_controllers))
00718       {
00719         std::vector<std::string> other_option;
00720         for (std::size_t j = i + 1 ; j <= available_controllers.size() ; ++j)
00721           if (findControllers(actuated_joints, j, available_controllers, other_option))
00722           {
00723             if (areControllersActive(other_option))
00724             {
00725               selected_controllers = other_option;
00726               break;
00727             }
00728           }
00729       }
00730       return true;
00731     }
00732   return false;
00733 }
00734 
00735 bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers, std::vector<moveit_msgs::RobotTrajectory> &parts)
00736 {
00737   parts.clear();
00738   parts.resize(controllers.size());
00739 
00740   std::set<std::string> actuated_joints_mdof;
00741   actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
00742                               trajectory.multi_dof_joint_trajectory.joint_names.end());
00743   std::set<std::string> actuated_joints_single;
00744   actuated_joints_single.insert(trajectory.joint_trajectory.joint_names.begin(),
00745                                 trajectory.joint_trajectory.joint_names.end());
00746 
00747   for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00748   {
00749     std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
00750     if (it == known_controllers_.end())
00751     {
00752       ROS_ERROR_STREAM("Controller " << controllers[i] << " not found.");
00753       return false;
00754     }
00755     std::vector<std::string> intersect_mdof;
00756     std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(),
00757                           actuated_joints_mdof.begin(), actuated_joints_mdof.end(),
00758                           std::back_inserter(intersect_mdof));
00759     std::vector<std::string> intersect_single;
00760     std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(),
00761                           actuated_joints_single.begin(), actuated_joints_single.end(),
00762                           std::back_inserter(intersect_single));
00763     if (intersect_mdof.empty() && intersect_single.empty())
00764       ROS_WARN_STREAM("No joints to be distributed for controller " << controllers[i]);
00765     {
00766       if (!intersect_mdof.empty())
00767       {
00768         std::vector<std::string> &jnames = parts[i].multi_dof_joint_trajectory.joint_names;
00769         jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
00770         std::map<std::string, std::size_t> index;
00771         for (std::size_t j = 0 ; j < trajectory.multi_dof_joint_trajectory.joint_names.size() ; ++j)
00772           index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
00773         std::vector<std::size_t> bijection(jnames.size());
00774         for (std::size_t j = 0 ; j < jnames.size() ; ++j)
00775           bijection[j] = index[jnames[j]];
00776 
00777         parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
00778         for (std::size_t j = 0 ; j < trajectory.multi_dof_joint_trajectory.points.size() ; ++j)
00779         {
00780           parts[i].multi_dof_joint_trajectory.points[j].time_from_start = trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
00781           parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
00782           for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00783             parts[i].multi_dof_joint_trajectory.points[j].transforms[k] = trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
00784         }
00785       }
00786       if (!intersect_single.empty())
00787       {
00788         std::vector<std::string> &jnames = parts[i].joint_trajectory.joint_names;
00789         jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
00790         parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
00791         std::map<std::string, std::size_t> index;
00792         for (std::size_t j = 0 ; j < trajectory.joint_trajectory.joint_names.size() ; ++j)
00793           index[trajectory.joint_trajectory.joint_names[j]] = j;
00794         std::vector<std::size_t> bijection(jnames.size());
00795         for (std::size_t j = 0 ; j < jnames.size() ; ++j)
00796           bijection[j] = index[jnames[j]];
00797         parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
00798         for (std::size_t j = 0 ; j < trajectory.joint_trajectory.points.size() ; ++j)
00799         {
00800           parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
00801           if (!trajectory.joint_trajectory.points[j].positions.empty())
00802           {
00803             parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
00804             for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00805               parts[i].joint_trajectory.points[j].positions[k] = trajectory.joint_trajectory.points[j].positions[bijection[k]];
00806           }
00807           if (!trajectory.joint_trajectory.points[j].velocities.empty())
00808           {
00809             parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
00810             for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00811               parts[i].joint_trajectory.points[j].velocities[k] = trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
00812           }
00813           if (!trajectory.joint_trajectory.points[j].accelerations.empty())
00814           {
00815             parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
00816             for (std::size_t k = 0 ; k < bijection.size() ; ++k)
00817               parts[i].joint_trajectory.points[j].accelerations[k] = trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
00818           }
00819         }
00820       }
00821     }
00822   }
00823   return true;
00824 }
00825 
00826 bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
00827 {
00828   if (trajectory.multi_dof_joint_trajectory.points.empty() &&  trajectory.joint_trajectory.points.empty())
00829   {
00830     ROS_WARN("The trajectory to execute is empty");
00831     return false;
00832   }
00833   std::set<std::string> actuated_joints;
00834   actuated_joints.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
00835                          trajectory.multi_dof_joint_trajectory.joint_names.end());
00836   actuated_joints.insert(trajectory.joint_trajectory.joint_names.begin(),
00837                          trajectory.joint_trajectory.joint_names.end());
00838   if (actuated_joints.empty())
00839   {
00840     ROS_WARN("The trajectory to execute specifies no joints");
00841     return false;
00842   }
00843 
00844   if (controllers.empty())
00845   {
00846     bool retry = true;
00847     bool reloaded = false;
00848     while (retry)
00849     {
00850       retry = false;
00851       std::vector<std::string> all_controller_names;
00852       for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
00853         all_controller_names.push_back(it->first);
00854       if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
00855       {
00856         if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
00857           return true;
00858       }
00859       else
00860       {
00861         // maybe we failed because we did not have a complete list of controllers
00862         if (!reloaded)
00863         {
00864           reloadControllerInformation();
00865           reloaded = true;
00866           retry = true;
00867         }
00868       }
00869     }
00870   }
00871   else
00872   {
00873     // check if the specified controllers are valid names;
00874     // if they appear not to be, try to reload the controller information, just in case they are new in the system
00875     bool reloaded = false;
00876     for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00877       if (known_controllers_.find(controllers[i]) == known_controllers_.end())
00878       {
00879         reloadControllerInformation();
00880         reloaded = true;
00881         break;
00882       }
00883     if (reloaded)
00884       for (std::size_t i = 0 ; i < controllers.size() ; ++i)
00885         if (known_controllers_.find(controllers[i]) == known_controllers_.end())
00886         {
00887           ROS_ERROR("Controller '%s' is not known", controllers[i].c_str());
00888           return false;
00889         }
00890     if (selectControllers(actuated_joints, controllers, context.controllers_))
00891     {
00892       if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
00893         return true;
00894     }
00895   }
00896   std::stringstream ss;
00897   for (std::set<std::string>::const_iterator it = actuated_joints.begin() ; it != actuated_joints.end() ; ++it)
00898     ss << *it << " ";
00899   ROS_ERROR("Unable to identify any set of controllers that can actuate the specified joints: [ %s]", ss.str().c_str());
00900   return false;
00901 }
00902 
00903 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::executeAndWait(bool auto_clear)
00904 {
00905   execute(ExecutionCompleteCallback(), auto_clear);
00906   return waitForExecution();
00907 }
00908 
00909 void TrajectoryExecutionManager::stopExecutionInternal()
00910 {
00911   // execution_state_mutex_ needs to have been locked by the caller
00912   for (std::size_t i = 0 ; i < active_handles_.size() ; ++i)
00913     try
00914     {
00915       active_handles_[i]->cancelExecution();
00916     }
00917     catch(...)
00918     {
00919       ROS_ERROR("Exception caught when canceling execution.");
00920     }
00921 }
00922 
00923 void TrajectoryExecutionManager::stopExecution(bool auto_clear)
00924 {
00925   stop_continuous_execution_ = true;
00926   continuous_execution_condition_.notify_all();
00927 
00928   if (!execution_complete_)
00929   {
00930     execution_state_mutex_.lock();
00931     if (!execution_complete_)
00932     {
00933       // we call cancel for all active handles; we know these are not being modified as we loop through them because of the lock
00934       // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external trigger to stop has been received
00935       execution_complete_ = true;
00936       stopExecutionInternal();
00937 
00938       // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
00939       last_execution_status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
00940       execution_state_mutex_.unlock();
00941       ROS_INFO("Stopped trajectory execution.");
00942 
00943       // wait for the execution thread to finish
00944       execution_thread_->join();
00945       execution_thread_.reset();
00946 
00947       if (auto_clear)
00948         clear();
00949     }
00950     else
00951       execution_state_mutex_.unlock();
00952   }
00953   else
00954     if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we join it now
00955     {
00956       execution_thread_->join();
00957       execution_thread_.reset();
00958     }
00959 }
00960 
00961 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback &callback, bool auto_clear)
00962 {
00963   execute(callback, PathSegmentCompleteCallback(), auto_clear);
00964 }
00965 
00966 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
00967 {
00968   stopExecution(false);
00969   execution_complete_ = false;
00970   // start the execution thread
00971   execution_thread_.reset(new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
00972 }
00973 
00974 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::waitForExecution()
00975 {
00976   {
00977     boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
00978     while (!execution_complete_)
00979       execution_complete_condition_.wait(ulock);
00980   }
00981   {
00982     boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
00983     while (!continuous_execution_queue_.empty())
00984       continuous_execution_condition_.wait(ulock);
00985   }
00986 
00987   // this will join the thread for executing sequences of trajectories
00988   stopExecution(false);
00989 
00990   return last_execution_status_;
00991 }
00992 
00993 void TrajectoryExecutionManager::clear()
00994 {
00995   if (execution_complete_)
00996   {
00997     for (std::size_t i = 0 ; i < trajectories_.size() ; ++i)
00998       delete trajectories_[i];
00999     trajectories_.clear();
01000     {
01001       boost::mutex::scoped_lock slock(continuous_execution_mutex_);
01002       while (!continuous_execution_queue_.empty())
01003       {
01004         delete continuous_execution_queue_.front();
01005         continuous_execution_queue_.pop_front();
01006       }
01007     }
01008   }
01009   else
01010     ROS_ERROR("Cannot push a new trajectory while another is being executed");
01011 }
01012 
01013 void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
01014 {
01015   // if we already got a stop request before we even started anything, we abort
01016   if (execution_complete_)
01017   {
01018     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01019     if (callback)
01020       callback(last_execution_status_);
01021     return;
01022   }
01023 
01024   ROS_DEBUG("Starting trajectory execution ...");
01025   // assume everything will be OK
01026   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
01027 
01028   // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
01029   // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
01030   for (std::size_t i = 0 ; i < trajectories_.size() ; ++i)
01031   {
01032     bool epart = executePart(i);
01033     if (epart && part_callback)
01034       part_callback(i);
01035     if (!epart || execution_complete_)
01036       break;
01037   }
01038 
01039   ROS_DEBUG("Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());
01040 
01041   // notify whoever is waiting for the event of trajectory completion
01042   execution_state_mutex_.lock();
01043   execution_complete_ = true;
01044   execution_state_mutex_.unlock();
01045   execution_complete_condition_.notify_all();
01046 
01047   // clear the paths just executed, if needed
01048   if (auto_clear)
01049     clear();
01050 
01051   // call user-specified callback
01052   if (callback)
01053     callback(last_execution_status_);
01054 }
01055 
01056 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
01057 {
01058   TrajectoryExecutionContext &context = *trajectories_[part_index];
01059 
01060   // first make sure desired controllers are active
01061   if (ensureActiveControllers(context.controllers_))
01062   {
01063     // stop if we are already asked to do so
01064     if (execution_complete_)
01065       return false;
01066 
01067     std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
01068     {
01069       boost::mutex::scoped_lock slock(execution_state_mutex_);
01070       if (!execution_complete_)
01071       {
01072         // time indexing uses this member too, so we lock this mutex as well
01073         time_index_mutex_.lock();
01074         current_context_ = part_index;
01075         time_index_mutex_.unlock();
01076         active_handles_.resize(context.controllers_.size());
01077         for (std::size_t i = 0 ; i < context.controllers_.size() ; ++i)
01078         {
01079           moveit_controller_manager::MoveItControllerHandlePtr h;
01080           try
01081           {
01082             h = controller_manager_->getControllerHandle(context.controllers_[i]);
01083           }
01084           catch(...)
01085           {
01086             ROS_ERROR("Exception caught when retrieving controller handle");
01087           }
01088           if (!h)
01089           {
01090             active_handles_.clear();
01091             current_context_ = -1;
01092             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01093             ROS_ERROR("No controller handle for controller '%s'. Aborting.", context.controllers_[i].c_str());
01094             return false;
01095           }
01096           active_handles_[i] = h;
01097         }
01098         handles = active_handles_; // keep a copy for later, to avoid thread safety issues
01099         for (std::size_t i = 0 ; i < context.trajectory_parts_.size() ; ++i)
01100         {
01101           bool ok = false;
01102           try
01103           {
01104             ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
01105           }
01106           catch(...)
01107           {
01108             ROS_ERROR("Exception caught when sending trajectory to controller");
01109           }
01110           if (!ok)
01111           {
01112             for (std::size_t j = 0 ; j < i ; ++j)
01113               try
01114               {
01115                 active_handles_[j]->cancelExecution();
01116               }
01117               catch(...)
01118               {
01119                 ROS_ERROR("Exception caught when canceling execution");
01120               }
01121             ROS_ERROR("Failed to send trajectory part %lu of %lu to controller %s", i + 1, context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
01122             if (i > 0)
01123               ROS_ERROR("Cancelling previously sent trajectory parts");
01124             active_handles_.clear();
01125             current_context_ = -1;
01126             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01127             return false;
01128           }
01129         }
01130       }
01131     }
01132 
01133     // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
01134     ros::Time current_time = ros::Time::now();
01135     ros::Duration expected_trajectory_duration(0.0);
01136     int longest_part = -1;
01137     for (std::size_t i = 0 ; i < context.trajectory_parts_.size() ; ++i)
01138     {
01139       ros::Duration d(0.0);
01140       if (!context.trajectory_parts_[i].joint_trajectory.points.empty())
01141       {
01142         if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
01143           d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
01144         if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
01145           d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
01146         d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ? ros::Duration(0.0) :
01147                       context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
01148                       context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ? ros::Duration(0.0) :
01149                       context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
01150 
01151         if (longest_part < 0 ||
01152             std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
01153                      context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
01154             std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
01155                      context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
01156           longest_part = i;
01157       }
01158       expected_trajectory_duration = std::max(d, expected_trajectory_duration);
01159     }
01160     // add 10% + 0.5s to the expected duration; this is just to allow things to finish propery
01161 
01162     expected_trajectory_duration = expected_trajectory_duration * allowed_execution_duration_scaling_ + DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
01163 
01164     if (longest_part >= 0)
01165     {
01166       boost::mutex::scoped_lock slock(time_index_mutex_);
01167 
01168       // construct a map from expected time to state index, for easy access to expected state location
01169       if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >= context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
01170       {
01171         ros::Duration d(0.0);
01172         if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
01173           d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
01174         for (std::size_t j = 0 ; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size() ; ++j)
01175           time_index_.push_back(current_time + d + context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
01176       }
01177       else
01178       {
01179         ros::Duration d(0.0);
01180         if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
01181           d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
01182         for (std::size_t j = 0 ; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size() ; ++j)
01183           time_index_.push_back(current_time + d + context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
01184       }
01185     }
01186 
01187     bool result = true;
01188     for (std::size_t i = 0 ; i < handles.size() ; ++i)
01189     {
01190       if (execution_duration_monitoring_)
01191       {
01192         if (!handles[i]->waitForExecution(expected_trajectory_duration))
01193           if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
01194           {
01195             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());
01196             {
01197               boost::mutex::scoped_lock slock(execution_state_mutex_);
01198               stopExecutionInternal(); // this is trally tricky. we can't call stopExecution() here, so we call the internal function only
01199             }
01200             last_execution_status_ = moveit_controller_manager::ExecutionStatus::TIMED_OUT;
01201             result = false;
01202             break;
01203           }
01204       }
01205       else
01206         handles[i]->waitForExecution();
01207 
01208       // if something made the trajectory stop, we stop this thread too
01209       if (execution_complete_)
01210       {
01211         result = false;
01212         break;
01213       }
01214       else
01215         if (handles[i]->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::SUCCEEDED)
01216         {
01217           ROS_WARN("Controller handle reports status %s", handles[i]->getLastExecutionStatus().asString().c_str());
01218           last_execution_status_ = handles[i]->getLastExecutionStatus();
01219           result = false;
01220         }
01221     }
01222 
01223     // clear the active handles
01224     execution_state_mutex_.lock();
01225     active_handles_.clear();
01226 
01227     // clear the time index
01228     time_index_mutex_.lock();
01229     time_index_.clear();
01230     current_context_ = -1;
01231     time_index_mutex_.unlock();
01232 
01233     execution_state_mutex_.unlock();
01234     return result;
01235   }
01236   else
01237   {
01238     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01239     return false;
01240   }
01241 }
01242 
01243 std::pair<int, int> TrajectoryExecutionManager::getCurrentExpectedTrajectoryIndex() const
01244 {
01245   boost::mutex::scoped_lock slock(time_index_mutex_);
01246   if (current_context_ < 0)
01247     return std::make_pair(-1, -1);
01248   if (time_index_.empty())
01249     return std::make_pair((int)current_context_, -1);
01250   std::vector<ros::Time>::const_iterator it = std::lower_bound(time_index_.begin(), time_index_.end(), ros::Time::now());
01251   int pos = it - time_index_.begin();
01252   return std::make_pair((int)current_context_, pos);
01253 }
01254 
01255 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>& TrajectoryExecutionManager::getTrajectories() const
01256 {
01257   return trajectories_;
01258 }
01259 
01260 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::getLastExecutionStatus() const
01261 {
01262   return last_execution_status_;
01263 }
01264 
01265 bool TrajectoryExecutionManager::ensureActiveControllersForGroup(const std::string &group)
01266 {
01267   const robot_model::JointModelGroup *joint_model_group = kinematic_model_->getJointModelGroup(group);
01268   if (joint_model_group)
01269     return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
01270   else
01271     return false;
01272 }
01273 
01274 bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string> &joints)
01275 {
01276   std::vector<std::string> all_controller_names;
01277   for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
01278     all_controller_names.push_back(it->first);
01279   std::vector<std::string> selected_controllers;
01280   std::set<std::string> jset;
01281   jset.insert(joints.begin(), joints.end());
01282   if (selectControllers(jset, all_controller_names, selected_controllers))
01283     return ensureActiveControllers(selected_controllers);
01284   else
01285     return false;
01286 }
01287 
01288 bool TrajectoryExecutionManager::ensureActiveController(const std::string &controller)
01289 {
01290   return ensureActiveControllers(std::vector<std::string>(1, controller));
01291 }
01292 
01293 bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string> &controllers)
01294 {
01295   updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
01296 
01297   if (manage_controllers_)
01298   {
01299     std::vector<std::string> controllers_to_activate;
01300     std::vector<std::string> controllers_to_deactivate;
01301     std::set<std::string> joints_to_be_activated;
01302     std::set<std::string> joints_to_be_deactivated;
01303     for (std::size_t i = 0 ; i < controllers.size() ; ++i)
01304     {
01305       std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controllers[i]);
01306       if (it == known_controllers_.end())
01307       {
01308         ROS_ERROR_STREAM("Controller " << controllers[i] << " is not known");
01309         return false;
01310       }
01311       if (!it->second.state_.active_)
01312       {
01313         ROS_DEBUG_STREAM("Need to activate " << controllers[i]);
01314         controllers_to_activate.push_back(controllers[i]);
01315         joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
01316         for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin() ;
01317              kt != it->second.overlapping_controllers_.end() ; ++kt)
01318         {
01319           const ControllerInformation &ci = known_controllers_[*kt];
01320           if (ci.state_.active_)
01321           {
01322             controllers_to_deactivate.push_back(*kt);
01323             joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
01324           }
01325         }
01326       }
01327       else
01328         ROS_DEBUG_STREAM("Controller " << controllers[i] << " is already active");
01329     }
01330     std::set<std::string> diff;
01331     std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
01332                         joints_to_be_activated.begin(), joints_to_be_activated.end(),
01333                         std::inserter(diff, diff.end()));
01334     if (!diff.empty())
01335     {
01336       // find the set of controllers that do not overlap with the ones we want to activate so far
01337       std::vector<std::string> possible_additional_controllers;
01338       for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
01339       {
01340         bool ok = true;
01341         for (std::size_t k = 0 ; k < controllers_to_activate.size() ; ++k)
01342           if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) != it->second.overlapping_controllers_.end())
01343           {
01344             ok = false;
01345             break;
01346           }
01347         if (ok)
01348           possible_additional_controllers.push_back(it->first);
01349       }
01350 
01351       // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
01352       std::vector<std::string> additional_controllers;
01353       if (selectControllers(diff, possible_additional_controllers, additional_controllers))
01354         controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(), additional_controllers.end());
01355       else
01356         return false;
01357     }
01358     if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
01359     {
01360       if (controller_manager_)
01361       {
01362         // load controllers to be activated, if needed, and reset the state update cache
01363         for (std::size_t a = 0 ; a < controllers_to_activate.size() ; ++a)
01364         {
01365           ControllerInformation &ci = known_controllers_[controllers_to_activate[a]];
01366           ci.last_update_ = ros::Time();
01367         }
01368         // reset the state update cache
01369         for (std::size_t a = 0 ; a < controllers_to_deactivate.size() ; ++a)
01370           known_controllers_[controllers_to_deactivate[a]].last_update_ = ros::Time();
01371         return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
01372       }
01373       else
01374         return false;
01375     }
01376     else
01377       return true;
01378   }
01379   else
01380   {
01381     std::set<std::string> originally_active;
01382     for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin() ; it != known_controllers_.end() ; ++it)
01383       if (it->second.state_.active_)
01384         originally_active.insert(it->first);
01385     return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
01386   }
01387 }
01388 
01389 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 02:31:40