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 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 double 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   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   // load the controller manager plugin
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   // other configuration steps
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       // remove handles we no longer need
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       // now send stuff to controllers
00387 
00388       // first make sure desired controllers are active
00389       if (areControllersActive(context->controllers_))
00390       {
00391         // get the controller handles needed to execute the new trajectory
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         // push all trajectories to all controllers simultaneously
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         // remember which handles we used
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     // preference is given to controllers marked as default
00596     if (nrdefault[a] > nrdefault[b])
00597       return true;
00598     if (nrdefault[a] < nrdefault[b])
00599       return false;
00600 
00601     // and then to ones that operate on fewer joints
00602     if (nrjoints[a] < nrjoints[b])
00603       return true;
00604     if (nrjoints[a] > nrjoints[b])
00605       return false;
00606 
00607     // and then to active ones
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   // generate all combinations of controller_count controllers that operate on disjoint sets of joints
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   // if none was found, this is a problem
00643   if (selected_options.empty())
00644     return false;
00645 
00646   // if only one was found, return it
00647   if (selected_options.size() == 1)
00648   {
00649     selected_controllers.swap(selected_options[0]);
00650     return true;
00651   }
00652 
00653   // if more options were found, evaluate them all and return the best one
00654 
00655   // count how many default controllers are used in each reported option, and how many joints are actuated in total by the selected controllers,
00656   // to use that in the ranking of the options
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   // define a bijection to compute the raking of the found options
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   // sort the options
00681   std::sort(bijection.begin(), bijection.end(), order);
00682 
00683   // depending on whether we are allowed to load & unload controllers,
00684   // we have different preference on deciding between options
00685   if (!manage_controllers_)
00686   {
00687     // if we can't load different options at will, just choose one that is already loaded
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   // otherwise, just use the first valid option
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       // if we are not managing controllers, prefer to use active controllers even if there are more of them
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         // maybe we failed because we did not have a complete list of controllers
00883         if (!reloaded)
00884         {
00885           reloadControllerInformation();
00886           reloaded = true;
00887           retry = true;
00888         }
00889       }
00890     }
00891   }
00892   else
00893   {
00894     // check if the specified controllers are valid names;
00895     // if they appear not to be, try to reload the controller information, just in case they are new in the system
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   // execution_state_mutex_ needs to have been locked by the caller
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       // we call cancel for all active handles; we know these are not being modified as we loop through them because of the lock
00969       // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external trigger to stop has been received
00970       execution_complete_ = true;
00971       stopExecutionInternal();
00972 
00973       // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
00974       last_execution_status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
00975       execution_state_mutex_.unlock();
00976       ROS_INFO("Stopped trajectory execution.");
00977 
00978       // wait for the execution thread to finish
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_) // just in case we have some thread waiting to be joined from some point in the past, we join it now
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   // start the execution thread
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   // this will join the thread for executing sequences of trajectories
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   // if we already got a stop request before we even started anything, we abort
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   // assume everything will be OK
01061   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
01062 
01063   // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
01064   // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
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   // notify whoever is waiting for the event of trajectory completion
01077   execution_state_mutex_.lock();
01078   execution_complete_ = true;
01079   execution_state_mutex_.unlock();
01080   execution_complete_condition_.notify_all();
01081 
01082   // clear the paths just executed, if needed
01083   if (auto_clear)
01084     clear();
01085 
01086   // call user-specified callback
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   // first make sure desired controllers are active
01096   if (ensureActiveControllers(context.controllers_))
01097   {
01098     // stop if we are already asked to do so
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         // time indexing uses this member too, so we lock this mutex as well
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_; // keep a copy for later, to avoid thread safety issues
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     // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
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     // add 10% + 0.5s to the expected duration; this is just to allow things to finish propery
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       // construct a map from expected time to state index, for easy access to expected state location
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(); // this is trally tricky. we can't call stopExecution() here, so we call the internal function only
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       // if something made the trajectory stop, we stop this thread too
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     // clear the active handles
01260     execution_state_mutex_.lock();
01261     active_handles_.clear();
01262 
01263     // clear the time index
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       // find the set of controllers that do not overlap with the ones we want to activate so far
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       // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
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         // load controllers to be activated, if needed, and reset the state update cache
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         // reset the state update cache
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 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:31