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/robot_state/robot_state.h>
00039 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h>
00040 #include <dynamic_reconfigure/server.h>
00041 
00042 namespace trajectory_execution_manager
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
00048                                                                     // before triggering a trajectory cancel (applied
00049                                                                     // after scaling)
00050 static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =
00051     1.1;  // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
00052 
00053 using namespace moveit_ros_planning;
00054 
00055 class TrajectoryExecutionManager::DynamicReconfigureImpl
00056 {
00057 public:
00058   DynamicReconfigureImpl(TrajectoryExecutionManager* owner)
00059     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))
00060   {
00061     dynamic_reconfigure_server_.setCallback(
00062         boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00063   }
00064 
00065 private:
00066   void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level)
00067   {
00068     owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
00069     owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
00070     owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
00071     owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
00072     owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
00073   }
00074 
00075   TrajectoryExecutionManager* owner_;
00076   dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
00077 };
00078 
00079 TrajectoryExecutionManager::TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& kmodel)
00080   : TrajectoryExecutionManager(kmodel, planning_scene_monitor::CurrentStateMonitorPtr())
00081 {
00082 }
00083 
00084 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
00085                                                        const planning_scene_monitor::CurrentStateMonitorPtr& csm)
00086   : robot_model_(kmodel), csm_(csm), node_handle_("~")
00087 {
00088   if (!node_handle_.getParam("moveit_manage_controllers", manage_controllers_))
00089     manage_controllers_ = false;
00090 
00091   initialize();
00092 }
00093 
00094 TrajectoryExecutionManager::TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& kmodel,
00095                                                        bool manage_controllers)
00096   : TrajectoryExecutionManager(kmodel, planning_scene_monitor::CurrentStateMonitorPtr(), manage_controllers)
00097 {
00098 }
00099 
00100 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
00101                                                        const planning_scene_monitor::CurrentStateMonitorPtr& csm,
00102                                                        bool manage_controllers)
00103   : robot_model_(kmodel), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers)
00104 {
00105   initialize();
00106 }
00107 
00108 TrajectoryExecutionManager::~TrajectoryExecutionManager()
00109 {
00110   run_continuous_execution_thread_ = false;
00111   stopExecution(true);
00112   delete reconfigure_impl_;
00113 }
00114 
00115 static const char* DEPRECATION_WARNING =
00116     "\nDeprecation warning: parameter '%s' moved into namespace 'trajectory_execution'."
00117     "\nPlease, adjust file trajectory_execution.launch.xml!";
00118 void TrajectoryExecutionManager::initialize()
00119 {
00120   reconfigure_impl_ = NULL;
00121   verbose_ = false;
00122   execution_complete_ = true;
00123   stop_continuous_execution_ = false;
00124   current_context_ = -1;
00125   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00126   run_continuous_execution_thread_ = true;
00127   execution_duration_monitoring_ = true;
00128   execution_velocity_scaling_ = 1.0;
00129   allowed_start_tolerance_ = 0.01;
00130 
00131   // TODO: Reading from old param location should be removed in L-turtle. Handled by DynamicReconfigure.
00132   if (node_handle_.getParam("allowed_execution_duration_scaling", allowed_execution_duration_scaling_))
00133     ROS_WARN_NAMED("trajectory_execution_manager", DEPRECATION_WARNING, "allowed_execution_duration_scaling");
00134   else
00135     allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
00136 
00137   if (node_handle_.getParam("allowed_goal_duration_margin", allowed_goal_duration_margin_))
00138     ROS_WARN_NAMED("trajectory_execution_manager", DEPRECATION_WARNING, "allowed_goal_duration_margin");
00139   else
00140     allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
00141 
00142   // load the controller manager plugin
00143   try
00144   {
00145     controller_manager_loader_.reset(new pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>(
00146         "moveit_core", "moveit_controller_manager::MoveItControllerManager"));
00147   }
00148   catch (pluginlib::PluginlibException& ex)
00149   {
00150     ROS_FATAL_STREAM_NAMED("traj_execution",
00151                            "Exception while creating controller manager plugin loader: " << ex.what());
00152     return;
00153   }
00154 
00155   if (controller_manager_loader_)
00156   {
00157     std::string controller;
00158     if (!node_handle_.getParam("moveit_controller_manager", controller))
00159     {
00160       const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
00161       if (classes.size() == 1)
00162       {
00163         controller = classes[0];
00164         ROS_WARN_NAMED("traj_execution", "Parameter '~moveit_controller_manager' is not specified but only one "
00165                                          "matching plugin was found: '%s'. Using that one.",
00166                        controller.c_str());
00167       }
00168       else
00169         ROS_FATAL_NAMED("traj_execution", "Parameter '~moveit_controller_manager' not specified. This is needed to "
00170                                           "identify the plugin to use for interacting with controllers. No paths can "
00171                                           "be executed.");
00172     }
00173 
00174     if (!controller.empty())
00175       try
00176       {
00177         controller_manager_.reset(controller_manager_loader_->createUnmanagedInstance(controller));
00178       }
00179       catch (pluginlib::PluginlibException& ex)
00180       {
00181         ROS_FATAL_STREAM_NAMED("traj_execution", "Exception while loading controller manager '" << controller
00182                                                                                                 << "': " << ex.what());
00183       }
00184   }
00185 
00186   // other configuration steps
00187   reloadControllerInformation();
00188 
00189   event_topic_subscriber_ =
00190       root_node_handle_.subscribe(EXECUTION_EVENT_TOPIC, 100, &TrajectoryExecutionManager::receiveEvent, this);
00191 
00192   reconfigure_impl_ = new DynamicReconfigureImpl(this);
00193 
00194   if (!csm_)
00195     ROS_WARN_NAMED("traj_execution", "Trajectory validation is disabled, because no CurrentStateMonitor was provided "
00196                                      "in constructor.");
00197 
00198   if (manage_controllers_)
00199     ROS_INFO_NAMED("traj_execution", "Trajectory execution is managing controllers");
00200   else
00201     ROS_INFO_NAMED("traj_execution", "Trajectory execution is not managing controllers");
00202 }
00203 
00204 void TrajectoryExecutionManager::enableExecutionDurationMonitoring(bool flag)
00205 {
00206   execution_duration_monitoring_ = flag;
00207 }
00208 
00209 void TrajectoryExecutionManager::setAllowedExecutionDurationScaling(double scaling)
00210 {
00211   allowed_execution_duration_scaling_ = scaling;
00212 }
00213 
00214 void TrajectoryExecutionManager::setAllowedGoalDurationMargin(double margin)
00215 {
00216   allowed_goal_duration_margin_ = margin;
00217 }
00218 
00219 void TrajectoryExecutionManager::setExecutionVelocityScaling(double scaling)
00220 {
00221   execution_velocity_scaling_ = scaling;
00222 }
00223 
00224 void TrajectoryExecutionManager::setAllowedStartTolerance(double tolerance)
00225 {
00226   allowed_start_tolerance_ = tolerance;
00227 }
00228 
00229 bool TrajectoryExecutionManager::isManagingControllers() const
00230 {
00231   return manage_controllers_;
00232 }
00233 
00234 const moveit_controller_manager::MoveItControllerManagerPtr& TrajectoryExecutionManager::getControllerManager() const
00235 {
00236   return controller_manager_;
00237 }
00238 
00239 void TrajectoryExecutionManager::processEvent(const std::string& event)
00240 {
00241   if (event == "stop")
00242     stopExecution(true);
00243   else
00244     ROS_WARN_STREAM_NAMED("traj_execution", "Unknown event type: '" << event << "'");
00245 }
00246 
00247 void TrajectoryExecutionManager::receiveEvent(const std_msgs::StringConstPtr& event)
00248 {
00249   ROS_INFO_STREAM_NAMED("traj_execution", "Received event '" << event->data << "'");
00250   processEvent(event->data);
00251 }
00252 
00253 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller)
00254 {
00255   if (controller.empty())
00256     return push(trajectory, std::vector<std::string>());
00257   else
00258     return push(trajectory, std::vector<std::string>(1, controller));
00259 }
00260 
00261 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller)
00262 {
00263   if (controller.empty())
00264     return push(trajectory, std::vector<std::string>());
00265   else
00266     return push(trajectory, std::vector<std::string>(1, controller));
00267 }
00268 
00269 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory& trajectory,
00270                                       const std::vector<std::string>& controllers)
00271 {
00272   moveit_msgs::RobotTrajectory traj;
00273   traj.joint_trajectory = trajectory;
00274   return push(traj, controllers);
00275 }
00276 
00277 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory,
00278                                       const std::vector<std::string>& controllers)
00279 {
00280   if (!execution_complete_)
00281   {
00282     ROS_ERROR_NAMED("traj_execution", "Cannot push a new trajectory while another is being executed");
00283     return false;
00284   }
00285 
00286   TrajectoryExecutionContext* context = new TrajectoryExecutionContext();
00287   if (configure(*context, trajectory, controllers))
00288   {
00289     if (verbose_)
00290     {
00291       std::stringstream ss;
00292       ss << "Pushed trajectory for execution using controllers [ ";
00293       for (std::size_t i = 0; i < context->controllers_.size(); ++i)
00294         ss << context->controllers_[i] << " ";
00295       ss << "]:" << std::endl;
00296       for (std::size_t i = 0; i < context->trajectory_parts_.size(); ++i)
00297         ss << context->trajectory_parts_[i] << std::endl;
00298       ROS_INFO_NAMED("traj_execution", "%s", ss.str().c_str());
00299     }
00300     trajectories_.push_back(context);
00301     return true;
00302   }
00303   else
00304   {
00305     delete context;
00306     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00307   }
00308 
00309   return false;
00310 }
00311 
00312 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory,
00313                                                 const std::string& controller)
00314 {
00315   if (controller.empty())
00316     return pushAndExecute(trajectory, std::vector<std::string>());
00317   else
00318     return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
00319 }
00320 
00321 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory,
00322                                                 const std::string& controller)
00323 {
00324   if (controller.empty())
00325     return pushAndExecute(trajectory, std::vector<std::string>());
00326   else
00327     return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
00328 }
00329 
00330 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState& state, const std::string& controller)
00331 {
00332   if (controller.empty())
00333     return pushAndExecute(state, std::vector<std::string>());
00334   else
00335     return pushAndExecute(state, std::vector<std::string>(1, controller));
00336 }
00337 
00338 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory,
00339                                                 const std::vector<std::string>& controllers)
00340 {
00341   moveit_msgs::RobotTrajectory traj;
00342   traj.joint_trajectory = trajectory;
00343   return pushAndExecute(traj, controllers);
00344 }
00345 
00346 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState& state,
00347                                                 const std::vector<std::string>& controllers)
00348 {
00349   moveit_msgs::RobotTrajectory traj;
00350   traj.joint_trajectory.header = state.header;
00351   traj.joint_trajectory.joint_names = state.name;
00352   traj.joint_trajectory.points.resize(1);
00353   traj.joint_trajectory.points[0].positions = state.position;
00354   traj.joint_trajectory.points[0].velocities = state.velocity;
00355   traj.joint_trajectory.points[0].effort = state.effort;
00356   traj.joint_trajectory.points[0].time_from_start = ros::Duration(0, 0);
00357   return pushAndExecute(traj, controllers);
00358 }
00359 
00360 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory,
00361                                                 const std::vector<std::string>& controllers)
00362 {
00363   if (!execution_complete_)
00364   {
00365     ROS_ERROR_NAMED("traj_execution", "Cannot push & execute a new trajectory while another is being executed");
00366     return false;
00367   }
00368 
00369   TrajectoryExecutionContext* context = new TrajectoryExecutionContext();
00370   if (configure(*context, trajectory, controllers))
00371   {
00372     {
00373       boost::mutex::scoped_lock slock(continuous_execution_mutex_);
00374       continuous_execution_queue_.push_back(context);
00375       if (!continuous_execution_thread_)
00376         continuous_execution_thread_.reset(
00377             new boost::thread(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)));
00378     }
00379     last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00380     continuous_execution_condition_.notify_all();
00381     return true;
00382   }
00383   else
00384   {
00385     delete context;
00386     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00387     return false;
00388   }
00389 }
00390 
00391 void TrajectoryExecutionManager::continuousExecutionThread()
00392 {
00393   std::set<moveit_controller_manager::MoveItControllerHandlePtr> used_handles;
00394   while (run_continuous_execution_thread_)
00395   {
00396     if (!stop_continuous_execution_)
00397     {
00398       boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
00399       while (continuous_execution_queue_.empty() && run_continuous_execution_thread_ && !stop_continuous_execution_)
00400         continuous_execution_condition_.wait(ulock);
00401     }
00402 
00403     if (stop_continuous_execution_ || !run_continuous_execution_thread_)
00404     {
00405       for (std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
00406            uit != used_handles.end(); ++uit)
00407         if ((*uit)->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::RUNNING)
00408           (*uit)->cancelExecution();
00409       used_handles.clear();
00410       while (!continuous_execution_queue_.empty())
00411       {
00412         TrajectoryExecutionContext* context = continuous_execution_queue_.front();
00413         continuous_execution_queue_.pop_front();
00414         delete context;
00415       }
00416       stop_continuous_execution_ = false;
00417       continue;
00418     }
00419 
00420     while (!continuous_execution_queue_.empty())
00421     {
00422       TrajectoryExecutionContext* context = NULL;
00423       {
00424         boost::mutex::scoped_lock slock(continuous_execution_mutex_);
00425         if (continuous_execution_queue_.empty())
00426           break;
00427         context = continuous_execution_queue_.front();
00428         continuous_execution_queue_.pop_front();
00429         if (continuous_execution_queue_.empty())
00430           continuous_execution_condition_.notify_all();
00431       }
00432 
00433       // remove handles we no longer need
00434       std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
00435       while (uit != used_handles.end())
00436         if ((*uit)->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::RUNNING)
00437         {
00438           std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator toErase = uit;
00439           ++uit;
00440           used_handles.erase(toErase);
00441         }
00442         else
00443           ++uit;
00444 
00445       // now send stuff to controllers
00446 
00447       // first make sure desired controllers are active
00448       if (areControllersActive(context->controllers_))
00449       {
00450         // get the controller handles needed to execute the new trajectory
00451         std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles(context->controllers_.size());
00452         for (std::size_t i = 0; i < context->controllers_.size(); ++i)
00453         {
00454           moveit_controller_manager::MoveItControllerHandlePtr h;
00455           try
00456           {
00457             h = controller_manager_->getControllerHandle(context->controllers_[i]);
00458           }
00459           catch (...)
00460           {
00461             ROS_ERROR_NAMED("traj_execution", "Exception caught when retrieving controller handle");
00462           }
00463           if (!h)
00464           {
00465             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00466             ROS_ERROR_NAMED("traj_execution", "No controller handle for controller '%s'. Aborting.",
00467                             context->controllers_[i].c_str());
00468             handles.clear();
00469             break;
00470           }
00471           handles[i] = h;
00472         }
00473 
00474         if (stop_continuous_execution_ || !run_continuous_execution_thread_)
00475         {
00476           delete context;
00477           break;
00478         }
00479 
00480         // push all trajectories to all controllers simultaneously
00481         if (!handles.empty())
00482           for (std::size_t i = 0; i < context->trajectory_parts_.size(); ++i)
00483           {
00484             bool ok = false;
00485             try
00486             {
00487               ok = handles[i]->sendTrajectory(context->trajectory_parts_[i]);
00488             }
00489             catch (...)
00490             {
00491               ROS_ERROR_NAMED("traj_execution", "Exception caught when sending trajectory to controller");
00492             }
00493             if (!ok)
00494             {
00495               for (std::size_t j = 0; j < i; ++j)
00496                 try
00497                 {
00498                   handles[j]->cancelExecution();
00499                 }
00500                 catch (...)
00501                 {
00502                   ROS_ERROR_NAMED("traj_execution", "Exception caught when canceling execution");
00503                 }
00504               ROS_ERROR_NAMED("traj_execution", "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
00505                               context->trajectory_parts_.size(), handles[i]->getName().c_str());
00506               if (i > 0)
00507                 ROS_ERROR_NAMED("traj_execution", "Cancelling previously sent trajectory parts");
00508               last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00509               handles.clear();
00510               break;
00511             }
00512           }
00513         delete context;
00514 
00515         // remember which handles we used
00516         for (std::size_t i = 0; i < handles.size(); ++i)
00517           used_handles.insert(handles[i]);
00518       }
00519       else
00520       {
00521         ROS_ERROR_NAMED("traj_execution", "Not all needed controllers are active. Cannot push and execute. You can try "
00522                                           "calling ensureActiveControllers() before pushAndExecute()");
00523         last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00524         delete context;
00525       }
00526     }
00527   }
00528 }
00529 
00530 void TrajectoryExecutionManager::reloadControllerInformation()
00531 {
00532   known_controllers_.clear();
00533   if (controller_manager_)
00534   {
00535     std::vector<std::string> names;
00536     controller_manager_->getControllersList(names);
00537     for (std::size_t i = 0; i < names.size(); ++i)
00538     {
00539       std::vector<std::string> joints;
00540       controller_manager_->getControllerJoints(names[i], joints);
00541       ControllerInformation ci;
00542       ci.name_ = names[i];
00543       ci.joints_.insert(joints.begin(), joints.end());
00544       known_controllers_[ci.name_] = ci;
00545     }
00546 
00547     for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
00548          it != known_controllers_.end(); ++it)
00549       for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
00550            jt != known_controllers_.end(); ++jt)
00551         if (it != jt)
00552         {
00553           std::vector<std::string> intersect;
00554           std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
00555                                 jt->second.joints_.end(), std::back_inserter(intersect));
00556           if (!intersect.empty())
00557           {
00558             it->second.overlapping_controllers_.insert(jt->first);
00559             jt->second.overlapping_controllers_.insert(it->first);
00560           }
00561         }
00562   }
00563 }
00564 
00565 void TrajectoryExecutionManager::updateControllerState(const std::string& controller, const ros::Duration& age)
00566 {
00567   std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
00568   if (it != known_controllers_.end())
00569     updateControllerState(it->second, age);
00570   else
00571     ROS_ERROR_NAMED("traj_execution", "Controller '%s' is not known.", controller.c_str());
00572 }
00573 
00574 void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci, const ros::Duration& age)
00575 {
00576   if (ros::Time::now() - ci.last_update_ >= age)
00577   {
00578     if (controller_manager_)
00579     {
00580       if (verbose_)
00581         ROS_INFO_NAMED("traj_execution", "Updating information for controller '%s'.", ci.name_.c_str());
00582       ci.state_ = controller_manager_->getControllerState(ci.name_);
00583       ci.last_update_ = ros::Time::now();
00584     }
00585   }
00586   else if (verbose_)
00587     ROS_INFO_NAMED("traj_execution", "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
00588 }
00589 
00590 void TrajectoryExecutionManager::updateControllersState(const ros::Duration& age)
00591 {
00592   for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
00593        it != known_controllers_.end(); ++it)
00594     updateControllerState(it->second, age);
00595 }
00596 
00597 bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
00598                                                             const std::set<std::string>& actuated_joints)
00599 {
00600   std::set<std::string> combined_joints;
00601   for (std::size_t i = 0; i < selected.size(); ++i)
00602   {
00603     const ControllerInformation& ci = known_controllers_[selected[i]];
00604     combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
00605   }
00606 
00607   if (verbose_)
00608   {
00609     std::stringstream ss, saj, sac;
00610     for (std::size_t i = 0; i < selected.size(); ++i)
00611       ss << selected[i] << " ";
00612     for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
00613       saj << *it << " ";
00614     for (std::set<std::string>::const_iterator it = combined_joints.begin(); it != combined_joints.end(); ++it)
00615       sac << *it << " ";
00616     ROS_INFO_NAMED("traj_execution", "Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]",
00617                    ss.str().c_str(), sac.str().c_str(), saj.str().c_str());
00618   }
00619 
00620   return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
00621 }
00622 
00623 void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
00624                                                                const std::vector<std::string>& available_controllers,
00625                                                                std::vector<std::string>& selected_controllers,
00626                                                                std::vector<std::vector<std::string> >& selected_options,
00627                                                                const std::set<std::string>& actuated_joints)
00628 {
00629   if (selected_controllers.size() == controller_count)
00630   {
00631     if (checkControllerCombination(selected_controllers, actuated_joints))
00632       selected_options.push_back(selected_controllers);
00633     return;
00634   }
00635 
00636   for (std::size_t i = start_index; i < available_controllers.size(); ++i)
00637   {
00638     bool overlap = false;
00639     const ControllerInformation& ci = known_controllers_[available_controllers[i]];
00640     for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
00641     {
00642       if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
00643         overlap = true;
00644     }
00645     if (overlap)
00646       continue;
00647     selected_controllers.push_back(available_controllers[i]);
00648     generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
00649                                   selected_options, actuated_joints);
00650     selected_controllers.pop_back();
00651   }
00652 }
00653 
00654 namespace
00655 {
00656 struct OrderPotentialControllerCombination
00657 {
00658   bool operator()(const std::size_t a, const std::size_t b) const
00659   {
00660     // preference is given to controllers marked as default
00661     if (nrdefault[a] > nrdefault[b])
00662       return true;
00663     if (nrdefault[a] < nrdefault[b])
00664       return false;
00665 
00666     // and then to ones that operate on fewer joints
00667     if (nrjoints[a] < nrjoints[b])
00668       return true;
00669     if (nrjoints[a] > nrjoints[b])
00670       return false;
00671 
00672     // and then to active ones
00673     if (nractive[a] < nractive[b])
00674       return true;
00675     if (nractive[a] > nractive[b])
00676       return false;
00677 
00678     return false;
00679   }
00680 
00681   std::vector<std::vector<std::string> > selected_options;
00682   std::vector<std::size_t> nrdefault;
00683   std::vector<std::size_t> nrjoints;
00684   std::vector<std::size_t> nractive;
00685 };
00686 }
00687 
00688 bool TrajectoryExecutionManager::findControllers(const std::set<std::string>& actuated_joints,
00689                                                  std::size_t controller_count,
00690                                                  const std::vector<std::string>& available_controllers,
00691                                                  std::vector<std::string>& selected_controllers)
00692 {
00693   // generate all combinations of controller_count controllers that operate on disjoint sets of joints
00694   std::vector<std::string> work_area;
00695   OrderPotentialControllerCombination order;
00696   std::vector<std::vector<std::string> >& selected_options = order.selected_options;
00697   generateControllerCombination(0, controller_count, available_controllers, work_area, selected_options,
00698                                 actuated_joints);
00699 
00700   if (verbose_)
00701   {
00702     std::stringstream saj;
00703     std::stringstream sac;
00704     for (std::size_t i = 0; i < available_controllers.size(); ++i)
00705       sac << available_controllers[i] << " ";
00706     for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
00707       saj << *it << " ";
00708     ROS_INFO_NAMED("traj_execution",
00709                    "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
00710                    controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
00711   }
00712 
00713   // if none was found, this is a problem
00714   if (selected_options.empty())
00715     return false;
00716 
00717   // if only one was found, return it
00718   if (selected_options.size() == 1)
00719   {
00720     selected_controllers.swap(selected_options[0]);
00721     return true;
00722   }
00723 
00724   // if more options were found, evaluate them all and return the best one
00725 
00726   // count how many default controllers are used in each reported option, and how many joints are actuated in total by
00727   // the selected controllers,
00728   // to use that in the ranking of the options
00729   order.nrdefault.resize(selected_options.size(), 0);
00730   order.nrjoints.resize(selected_options.size(), 0);
00731   order.nractive.resize(selected_options.size(), 0);
00732   for (std::size_t i = 0; i < selected_options.size(); ++i)
00733   {
00734     for (std::size_t k = 0; k < selected_options[i].size(); ++k)
00735     {
00736       updateControllerState(selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
00737       const ControllerInformation& ci = known_controllers_[selected_options[i][k]];
00738 
00739       if (ci.state_.default_)
00740         order.nrdefault[i]++;
00741       if (ci.state_.active_)
00742         order.nractive[i]++;
00743       order.nrjoints[i] += ci.joints_.size();
00744     }
00745   }
00746 
00747   // define a bijection to compute the raking of the found options
00748   std::vector<std::size_t> bijection(selected_options.size(), 0);
00749   for (std::size_t i = 0; i < selected_options.size(); ++i)
00750     bijection[i] = i;
00751 
00752   // sort the options
00753   std::sort(bijection.begin(), bijection.end(), order);
00754 
00755   // depending on whether we are allowed to load & unload controllers,
00756   // we have different preference on deciding between options
00757   if (!manage_controllers_)
00758   {
00759     // if we can't load different options at will, just choose one that is already loaded
00760     for (std::size_t i = 0; i < selected_options.size(); ++i)
00761       if (areControllersActive(selected_options[bijection[i]]))
00762       {
00763         selected_controllers.swap(selected_options[bijection[i]]);
00764         return true;
00765       }
00766   }
00767 
00768   // otherwise, just use the first valid option
00769   selected_controllers.swap(selected_options[bijection[0]]);
00770   return true;
00771 }
00772 
00773 bool TrajectoryExecutionManager::isControllerActive(const std::string& controller)
00774 {
00775   return areControllersActive(std::vector<std::string>(1, controller));
00776 }
00777 
00778 bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string>& controllers)
00779 {
00780   for (std::size_t i = 0; i < controllers.size(); ++i)
00781   {
00782     updateControllerState(controllers[i], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
00783     std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
00784     if (it == known_controllers_.end() || !it->second.state_.active_)
00785       return false;
00786   }
00787   return true;
00788 }
00789 
00790 bool TrajectoryExecutionManager::selectControllers(const std::set<std::string>& actuated_joints,
00791                                                    const std::vector<std::string>& available_controllers,
00792                                                    std::vector<std::string>& selected_controllers)
00793 {
00794   for (std::size_t i = 1; i <= available_controllers.size(); ++i)
00795     if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
00796     {
00797       // if we are not managing controllers, prefer to use active controllers even if there are more of them
00798       if (!manage_controllers_ && !areControllersActive(selected_controllers))
00799       {
00800         std::vector<std::string> other_option;
00801         for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
00802           if (findControllers(actuated_joints, j, available_controllers, other_option))
00803           {
00804             if (areControllersActive(other_option))
00805             {
00806               selected_controllers = other_option;
00807               break;
00808             }
00809           }
00810       }
00811       return true;
00812     }
00813   return false;
00814 }
00815 
00816 bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTrajectory& trajectory,
00817                                                       const std::vector<std::string>& controllers,
00818                                                       std::vector<moveit_msgs::RobotTrajectory>& parts)
00819 {
00820   parts.clear();
00821   parts.resize(controllers.size());
00822 
00823   std::set<std::string> actuated_joints_mdof;
00824   actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
00825                               trajectory.multi_dof_joint_trajectory.joint_names.end());
00826   std::set<std::string> actuated_joints_single;
00827   for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
00828   {
00829     const robot_model::JointModel* jm = robot_model_->getJointModel(trajectory.joint_trajectory.joint_names[i]);
00830     if (jm)
00831     {
00832       if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
00833         continue;
00834       actuated_joints_single.insert(jm->getName());
00835     }
00836   }
00837 
00838   for (std::size_t i = 0; i < controllers.size(); ++i)
00839   {
00840     std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
00841     if (it == known_controllers_.end())
00842     {
00843       ROS_ERROR_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " not found.");
00844       return false;
00845     }
00846     std::vector<std::string> intersect_mdof;
00847     std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
00848                           actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
00849     std::vector<std::string> intersect_single;
00850     std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
00851                           actuated_joints_single.end(), std::back_inserter(intersect_single));
00852     if (intersect_mdof.empty() && intersect_single.empty())
00853       ROS_WARN_STREAM_NAMED("traj_execution", "No joints to be distributed for controller " << controllers[i]);
00854     {
00855       if (!intersect_mdof.empty())
00856       {
00857         std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
00858         jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
00859         std::map<std::string, std::size_t> index;
00860         for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
00861           index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
00862         std::vector<std::size_t> bijection(jnames.size());
00863         for (std::size_t j = 0; j < jnames.size(); ++j)
00864           bijection[j] = index[jnames[j]];
00865 
00866         parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
00867         for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
00868         {
00869           parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
00870               trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
00871           parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
00872           for (std::size_t k = 0; k < bijection.size(); ++k)
00873           {
00874             parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
00875                 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
00876 
00877             if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
00878             {
00879               parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
00880 
00881               parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
00882                   trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
00883 
00884               parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
00885                   trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
00886 
00887               parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
00888                   trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
00889 
00890               parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
00891                   trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
00892 
00893               parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
00894                   trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
00895 
00896               parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
00897                   trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
00898             }
00899           }
00900         }
00901       }
00902       if (!intersect_single.empty())
00903       {
00904         std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
00905         jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
00906         parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
00907         std::map<std::string, std::size_t> index;
00908         for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
00909           index[trajectory.joint_trajectory.joint_names[j]] = j;
00910         std::vector<std::size_t> bijection(jnames.size());
00911         for (std::size_t j = 0; j < jnames.size(); ++j)
00912           bijection[j] = index[jnames[j]];
00913         parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
00914         for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
00915         {
00916           parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
00917           if (!trajectory.joint_trajectory.points[j].positions.empty())
00918           {
00919             parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
00920             for (std::size_t k = 0; k < bijection.size(); ++k)
00921               parts[i].joint_trajectory.points[j].positions[k] =
00922                   trajectory.joint_trajectory.points[j].positions[bijection[k]];
00923           }
00924           if (!trajectory.joint_trajectory.points[j].velocities.empty())
00925           {
00926             parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
00927             for (std::size_t k = 0; k < bijection.size(); ++k)
00928               parts[i].joint_trajectory.points[j].velocities[k] =
00929                   trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
00930           }
00931           if (!trajectory.joint_trajectory.points[j].accelerations.empty())
00932           {
00933             parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
00934             for (std::size_t k = 0; k < bijection.size(); ++k)
00935               parts[i].joint_trajectory.points[j].accelerations[k] =
00936                   trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
00937           }
00938           if (!trajectory.joint_trajectory.points[j].effort.empty())
00939           {
00940             parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
00941             for (std::size_t k = 0; k < bijection.size(); ++k)
00942               parts[i].joint_trajectory.points[j].effort[k] =
00943                   trajectory.joint_trajectory.points[j].effort[bijection[k]];
00944           }
00945         }
00946       }
00947     }
00948   }
00949   return true;
00950 }
00951 
00952 bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const
00953 {
00954   if (!csm_ || allowed_start_tolerance_ == 0)  // skip validation if csm is nil or on this magic number
00955     return true;
00956 
00957   ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
00958 
00959   robot_state::RobotStatePtr current_state;
00960   if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState()))
00961   {
00962     ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within "
00963                                      "1s");
00964     return false;
00965   }
00966 
00967   for (std::vector<moveit_msgs::RobotTrajectory>::const_iterator traj_it = context.trajectory_parts_.begin();
00968        traj_it != context.trajectory_parts_.end(); ++traj_it)
00969   {
00970     if (!traj_it->multi_dof_joint_trajectory.points.empty())
00971     {
00972       ROS_WARN_NAMED("traj_execution", "Validation of MultiDOFJointTrajectory is not implemented.");
00973       // go on to check joint_trajectory component though
00974     }
00975     if (traj_it->joint_trajectory.points.empty())
00976     {
00977       // There is nothing to check
00978       continue;
00979     }
00980     const std::vector<double>& positions = traj_it->joint_trajectory.points.front().positions;
00981     const std::vector<std::string>& joint_names = traj_it->joint_trajectory.joint_names;
00982     const std::size_t n = joint_names.size();
00983     if (positions.size() != n)
00984     {
00985       ROS_ERROR_NAMED("traj_execution", "Wrong trajectory: #joints: %zu != #positions: %zu", n, positions.size());
00986       return false;
00987     }
00988 
00989     for (std::size_t i = 0; i < n; ++i)
00990     {
00991       const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
00992       if (!jm)
00993       {
00994         ROS_ERROR_STREAM_NAMED("traj_execution", "Unknown joint in trajectory: " << joint_names[i]);
00995         return false;
00996       }
00997 
00998       double cur_position = current_state->getJointPositions(jm)[0];
00999       double traj_position = positions[i];
01000       // normalize positions and compare
01001       jm->enforcePositionBounds(&cur_position);
01002       jm->enforcePositionBounds(&traj_position);
01003       if (fabs(cur_position - traj_position) > allowed_start_tolerance_)
01004       {
01005         ROS_ERROR_NAMED("traj_execution",
01006                         "\nInvalid Trajectory: start point deviates from current robot state more than %g"
01007                         "\njoint '%s': expected: %g, current: %g",
01008                         allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
01009         return false;
01010       }
01011     }
01012   }
01013   return true;
01014 }
01015 
01016 bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
01017                                            const moveit_msgs::RobotTrajectory& trajectory,
01018                                            const std::vector<std::string>& controllers)
01019 {
01020   if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
01021   {
01022     ROS_WARN_NAMED("traj_execution", "The trajectory to execute is empty");
01023     return false;
01024   }
01025   std::set<std::string> actuated_joints;
01026   actuated_joints.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
01027                          trajectory.multi_dof_joint_trajectory.joint_names.end());
01028   actuated_joints.insert(trajectory.joint_trajectory.joint_names.begin(),
01029                          trajectory.joint_trajectory.joint_names.end());
01030   if (actuated_joints.empty())
01031   {
01032     ROS_WARN_NAMED("traj_execution", "The trajectory to execute specifies no joints");
01033     return false;
01034   }
01035 
01036   if (controllers.empty())
01037   {
01038     bool retry = true;
01039     bool reloaded = false;
01040     while (retry)
01041     {
01042       retry = false;
01043       std::vector<std::string> all_controller_names;
01044       for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01045            it != known_controllers_.end(); ++it)
01046         all_controller_names.push_back(it->first);
01047       if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
01048       {
01049         if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
01050           return true;
01051       }
01052       else
01053       {
01054         // maybe we failed because we did not have a complete list of controllers
01055         if (!reloaded)
01056         {
01057           reloadControllerInformation();
01058           reloaded = true;
01059           retry = true;
01060         }
01061       }
01062     }
01063   }
01064   else
01065   {
01066     // check if the specified controllers are valid names;
01067     // if they appear not to be, try to reload the controller information, just in case they are new in the system
01068     bool reloaded = false;
01069     for (std::size_t i = 0; i < controllers.size(); ++i)
01070       if (known_controllers_.find(controllers[i]) == known_controllers_.end())
01071       {
01072         reloadControllerInformation();
01073         reloaded = true;
01074         break;
01075       }
01076     if (reloaded)
01077       for (std::size_t i = 0; i < controllers.size(); ++i)
01078         if (known_controllers_.find(controllers[i]) == known_controllers_.end())
01079         {
01080           ROS_ERROR_NAMED("traj_execution", "Controller '%s' is not known", controllers[i].c_str());
01081           return false;
01082         }
01083     if (selectControllers(actuated_joints, controllers, context.controllers_))
01084     {
01085       if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
01086         return true;
01087     }
01088   }
01089   std::stringstream ss;
01090   for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
01091     ss << *it << " ";
01092   ROS_ERROR_NAMED("traj_execution",
01093                   "Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
01094                   ss.str().c_str());
01095 
01096   std::stringstream ss2;
01097   std::map<std::string, ControllerInformation>::const_iterator mi;
01098   for (mi = known_controllers_.begin(); mi != known_controllers_.end(); mi++)
01099   {
01100     ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
01101 
01102     std::set<std::string>::const_iterator ji;
01103     for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
01104     {
01105       ss2 << "  " << *ji << std::endl;
01106     }
01107   }
01108   ROS_ERROR_NAMED("traj_execution", "Known controllers and their joints:\n%s", ss2.str().c_str());
01109   return false;
01110 }
01111 
01112 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::executeAndWait(bool auto_clear)
01113 {
01114   execute(ExecutionCompleteCallback(), auto_clear);
01115   return waitForExecution();
01116 }
01117 
01118 void TrajectoryExecutionManager::stopExecutionInternal()
01119 {
01120   // execution_state_mutex_ needs to have been locked by the caller
01121   for (std::size_t i = 0; i < active_handles_.size(); ++i)
01122     try
01123     {
01124       active_handles_[i]->cancelExecution();
01125     }
01126     catch (...)
01127     {
01128       ROS_ERROR_NAMED("traj_execution", "Exception caught when canceling execution.");
01129     }
01130 }
01131 
01132 void TrajectoryExecutionManager::stopExecution(bool auto_clear)
01133 {
01134   stop_continuous_execution_ = true;
01135   continuous_execution_condition_.notify_all();
01136 
01137   if (!execution_complete_)
01138   {
01139     execution_state_mutex_.lock();
01140     if (!execution_complete_)
01141     {
01142       // we call cancel for all active handles; we know these are not being modified as we loop through them because of
01143       // the lock
01144       // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external
01145       // trigger to stop has been received
01146       execution_complete_ = true;
01147       stopExecutionInternal();
01148 
01149       // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
01150       last_execution_status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
01151       execution_state_mutex_.unlock();
01152       ROS_INFO_NAMED("traj_execution", "Stopped trajectory execution.");
01153 
01154       // wait for the execution thread to finish
01155       execution_thread_->join();
01156       execution_thread_.reset();
01157 
01158       if (auto_clear)
01159         clear();
01160     }
01161     else
01162       execution_state_mutex_.unlock();
01163   }
01164   else if (execution_thread_)  // just in case we have some thread waiting to be joined from some point in the past, we
01165                                // join it now
01166   {
01167     execution_thread_->join();
01168     execution_thread_.reset();
01169   }
01170 }
01171 
01172 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback, bool auto_clear)
01173 {
01174   execute(callback, PathSegmentCompleteCallback(), auto_clear);
01175 }
01176 
01177 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback,
01178                                          const PathSegmentCompleteCallback& part_callback, bool auto_clear)
01179 {
01180   stopExecution(false);
01181 
01182   // check whether first trajectory starts at current robot state
01183   if (trajectories_.size() && !validate(*trajectories_.front()))
01184   {
01185     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01186     if (auto_clear)
01187       clear();
01188     if (callback)
01189       callback(last_execution_status_);
01190     return;
01191   }
01192 
01193   // start the execution thread
01194   execution_complete_ = false;
01195   execution_thread_.reset(
01196       new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
01197 }
01198 
01199 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::waitForExecution()
01200 {
01201   {
01202     boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
01203     while (!execution_complete_)
01204       execution_complete_condition_.wait(ulock);
01205   }
01206   {
01207     boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
01208     while (!continuous_execution_queue_.empty())
01209       continuous_execution_condition_.wait(ulock);
01210   }
01211 
01212   // this will join the thread for executing sequences of trajectories
01213   stopExecution(false);
01214 
01215   return last_execution_status_;
01216 }
01217 
01218 void TrajectoryExecutionManager::clear()
01219 {
01220   if (execution_complete_)
01221   {
01222     for (std::size_t i = 0; i < trajectories_.size(); ++i)
01223       delete trajectories_[i];
01224     trajectories_.clear();
01225     {
01226       boost::mutex::scoped_lock slock(continuous_execution_mutex_);
01227       while (!continuous_execution_queue_.empty())
01228       {
01229         delete continuous_execution_queue_.front();
01230         continuous_execution_queue_.pop_front();
01231       }
01232     }
01233   }
01234   else
01235     ROS_ERROR_NAMED("traj_execution", "Cannot push a new trajectory while another is being executed");
01236 }
01237 
01238 void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback,
01239                                                const PathSegmentCompleteCallback& part_callback, bool auto_clear)
01240 {
01241   // if we already got a stop request before we even started anything, we abort
01242   if (execution_complete_)
01243   {
01244     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01245     if (callback)
01246       callback(last_execution_status_);
01247     return;
01248   }
01249 
01250   ROS_DEBUG_NAMED("traj_execution", "Starting trajectory execution ...");
01251   // assume everything will be OK
01252   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
01253 
01254   // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
01255   // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
01256   for (std::size_t i = 0; i < trajectories_.size(); ++i)
01257   {
01258     bool epart = executePart(i);
01259     if (epart && part_callback)
01260       part_callback(i);
01261     if (!epart || execution_complete_)
01262       break;
01263   }
01264 
01265   ROS_DEBUG_NAMED("traj_execution", "Completed trajectory execution with status %s ...",
01266                   last_execution_status_.asString().c_str());
01267 
01268   // notify whoever is waiting for the event of trajectory completion
01269   execution_state_mutex_.lock();
01270   execution_complete_ = true;
01271   execution_state_mutex_.unlock();
01272   execution_complete_condition_.notify_all();
01273 
01274   // clear the paths just executed, if needed
01275   if (auto_clear)
01276     clear();
01277 
01278   // call user-specified callback
01279   if (callback)
01280     callback(last_execution_status_);
01281 }
01282 
01283 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
01284 {
01285   TrajectoryExecutionContext& context = *trajectories_[part_index];
01286 
01287   // first make sure desired controllers are active
01288   if (ensureActiveControllers(context.controllers_))
01289   {
01290     // stop if we are already asked to do so
01291     if (execution_complete_)
01292       return false;
01293 
01294     std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
01295     {
01296       boost::mutex::scoped_lock slock(execution_state_mutex_);
01297       if (!execution_complete_)
01298       {
01299         // time indexing uses this member too, so we lock this mutex as well
01300         time_index_mutex_.lock();
01301         current_context_ = part_index;
01302         time_index_mutex_.unlock();
01303         active_handles_.resize(context.controllers_.size());
01304         for (std::size_t i = 0; i < context.controllers_.size(); ++i)
01305         {
01306           moveit_controller_manager::MoveItControllerHandlePtr h;
01307           try
01308           {
01309             h = controller_manager_->getControllerHandle(context.controllers_[i]);
01310           }
01311           catch (...)
01312           {
01313             ROS_ERROR_NAMED("traj_execution", "Exception caught when retrieving controller handle");
01314           }
01315           if (!h)
01316           {
01317             active_handles_.clear();
01318             current_context_ = -1;
01319             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01320             ROS_ERROR_NAMED("traj_execution", "No controller handle for controller '%s'. Aborting.",
01321                             context.controllers_[i].c_str());
01322             return false;
01323           }
01324           active_handles_[i] = h;
01325         }
01326         handles = active_handles_;  // keep a copy for later, to avoid thread safety issues
01327         for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
01328         {
01329           bool ok = false;
01330           try
01331           {
01332             ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
01333           }
01334           catch (...)
01335           {
01336             ROS_ERROR_NAMED("traj_execution", "Exception caught when sending trajectory to controller");
01337           }
01338           if (!ok)
01339           {
01340             for (std::size_t j = 0; j < i; ++j)
01341               try
01342               {
01343                 active_handles_[j]->cancelExecution();
01344               }
01345               catch (...)
01346               {
01347                 ROS_ERROR_NAMED("traj_execution", "Exception caught when canceling execution");
01348               }
01349             ROS_ERROR_NAMED("traj_execution", "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
01350                             context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
01351             if (i > 0)
01352               ROS_ERROR_NAMED("traj_execution", "Cancelling previously sent trajectory parts");
01353             active_handles_.clear();
01354             current_context_ = -1;
01355             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01356             return false;
01357           }
01358         }
01359       }
01360     }
01361 
01362     // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
01363     ros::Time current_time = ros::Time::now();
01364     ros::Duration expected_trajectory_duration(0.0);
01365     int longest_part = -1;
01366     for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
01367     {
01368       ros::Duration d(0.0);
01369       if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
01370             context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
01371       {
01372         if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
01373           d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
01374         if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
01375           d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
01376         d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
01377                           ros::Duration(0.0) :
01378                           context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
01379                       context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
01380                           ros::Duration(0.0) :
01381                           context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
01382 
01383         if (longest_part < 0 ||
01384             std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
01385                      context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
01386                 std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
01387                          context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
01388           longest_part = i;
01389       }
01390       expected_trajectory_duration = std::max(d, expected_trajectory_duration);
01391     }
01392     // add 10% + 0.5s to the expected duration; this is just to allow things to finish propery
01393 
01394     expected_trajectory_duration = expected_trajectory_duration * allowed_execution_duration_scaling_ +
01395                                    ros::Duration(allowed_goal_duration_margin_);
01396 
01397     if (longest_part >= 0)
01398     {
01399       boost::mutex::scoped_lock slock(time_index_mutex_);
01400 
01401       // construct a map from expected time to state index, for easy access to expected state location
01402       if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
01403           context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
01404       {
01405         ros::Duration d(0.0);
01406         if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
01407           d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
01408         for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size(); ++j)
01409           time_index_.push_back(current_time + d +
01410                                 context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
01411       }
01412       else
01413       {
01414         ros::Duration d(0.0);
01415         if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
01416           d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
01417         for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size();
01418              ++j)
01419           time_index_.push_back(
01420               current_time + d +
01421               context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
01422       }
01423     }
01424 
01425     bool result = true;
01426     for (std::size_t i = 0; i < handles.size(); ++i)
01427     {
01428       if (execution_duration_monitoring_)
01429       {
01430         if (!handles[i]->waitForExecution(expected_trajectory_duration))
01431           if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
01432           {
01433             ROS_ERROR_NAMED("traj_execution", "Controller is taking too long to execute trajectory (the expected upper "
01434                                               "bound for the trajectory execution was %lf seconds). Stopping "
01435                                               "trajectory.",
01436                             expected_trajectory_duration.toSec());
01437             {
01438               boost::mutex::scoped_lock slock(execution_state_mutex_);
01439               stopExecutionInternal();  // this is trally tricky. we can't call stopExecution() here, so we call the
01440                                         // internal function only
01441             }
01442             last_execution_status_ = moveit_controller_manager::ExecutionStatus::TIMED_OUT;
01443             result = false;
01444             break;
01445           }
01446       }
01447       else
01448         handles[i]->waitForExecution();
01449 
01450       // if something made the trajectory stop, we stop this thread too
01451       if (execution_complete_)
01452       {
01453         result = false;
01454         break;
01455       }
01456       else if (handles[i]->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::SUCCEEDED)
01457       {
01458         ROS_WARN_STREAM_NAMED("traj_execution", "Controller handle "
01459                                                     << handles[i]->getName() << " reports status "
01460                                                     << handles[i]->getLastExecutionStatus().asString());
01461         last_execution_status_ = handles[i]->getLastExecutionStatus();
01462         result = false;
01463       }
01464     }
01465 
01466     // clear the active handles
01467     execution_state_mutex_.lock();
01468     active_handles_.clear();
01469 
01470     // clear the time index
01471     time_index_mutex_.lock();
01472     time_index_.clear();
01473     current_context_ = -1;
01474     time_index_mutex_.unlock();
01475 
01476     execution_state_mutex_.unlock();
01477     return result;
01478   }
01479   else
01480   {
01481     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01482     return false;
01483   }
01484 }
01485 
01486 std::pair<int, int> TrajectoryExecutionManager::getCurrentExpectedTrajectoryIndex() const
01487 {
01488   boost::mutex::scoped_lock slock(time_index_mutex_);
01489   if (current_context_ < 0)
01490     return std::make_pair(-1, -1);
01491   if (time_index_.empty())
01492     return std::make_pair((int)current_context_, -1);
01493   std::vector<ros::Time>::const_iterator it =
01494       std::lower_bound(time_index_.begin(), time_index_.end(), ros::Time::now());
01495   int pos = it - time_index_.begin();
01496   return std::make_pair((int)current_context_, pos);
01497 }
01498 
01499 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
01500 TrajectoryExecutionManager::getTrajectories() const
01501 {
01502   return trajectories_;
01503 }
01504 
01505 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::getLastExecutionStatus() const
01506 {
01507   return last_execution_status_;
01508 }
01509 
01510 bool TrajectoryExecutionManager::ensureActiveControllersForGroup(const std::string& group)
01511 {
01512   const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group);
01513   if (joint_model_group)
01514     return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
01515   else
01516     return false;
01517 }
01518 
01519 bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string>& joints)
01520 {
01521   std::vector<std::string> all_controller_names;
01522   for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01523        it != known_controllers_.end(); ++it)
01524     all_controller_names.push_back(it->first);
01525   std::vector<std::string> selected_controllers;
01526   std::set<std::string> jset;
01527   for (std::size_t i = 0; i < joints.size(); ++i)
01528   {
01529     const robot_model::JointModel* jm = robot_model_->getJointModel(joints[i]);
01530     if (jm)
01531     {
01532       if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
01533         continue;
01534       jset.insert(joints[i]);
01535     }
01536   }
01537 
01538   if (selectControllers(jset, all_controller_names, selected_controllers))
01539     return ensureActiveControllers(selected_controllers);
01540   else
01541     return false;
01542 }
01543 
01544 bool TrajectoryExecutionManager::ensureActiveController(const std::string& controller)
01545 {
01546   return ensureActiveControllers(std::vector<std::string>(1, controller));
01547 }
01548 
01549 bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string>& controllers)
01550 {
01551   updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
01552 
01553   if (manage_controllers_)
01554   {
01555     std::vector<std::string> controllers_to_activate;
01556     std::vector<std::string> controllers_to_deactivate;
01557     std::set<std::string> joints_to_be_activated;
01558     std::set<std::string> joints_to_be_deactivated;
01559     for (std::size_t i = 0; i < controllers.size(); ++i)
01560     {
01561       std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controllers[i]);
01562       if (it == known_controllers_.end())
01563       {
01564         ROS_ERROR_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " is not known");
01565         return false;
01566       }
01567       if (!it->second.state_.active_)
01568       {
01569         ROS_DEBUG_STREAM_NAMED("traj_execution", "Need to activate " << controllers[i]);
01570         controllers_to_activate.push_back(controllers[i]);
01571         joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
01572         for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin();
01573              kt != it->second.overlapping_controllers_.end(); ++kt)
01574         {
01575           const ControllerInformation& ci = known_controllers_[*kt];
01576           if (ci.state_.active_)
01577           {
01578             controllers_to_deactivate.push_back(*kt);
01579             joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
01580           }
01581         }
01582       }
01583       else
01584         ROS_DEBUG_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " is already active");
01585     }
01586     std::set<std::string> diff;
01587     std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
01588                         joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
01589     if (!diff.empty())
01590     {
01591       // find the set of controllers that do not overlap with the ones we want to activate so far
01592       std::vector<std::string> possible_additional_controllers;
01593       for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01594            it != known_controllers_.end(); ++it)
01595       {
01596         bool ok = true;
01597         for (std::size_t k = 0; k < controllers_to_activate.size(); ++k)
01598           if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) !=
01599               it->second.overlapping_controllers_.end())
01600           {
01601             ok = false;
01602             break;
01603           }
01604         if (ok)
01605           possible_additional_controllers.push_back(it->first);
01606       }
01607 
01608       // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
01609       std::vector<std::string> additional_controllers;
01610       if (selectControllers(diff, possible_additional_controllers, additional_controllers))
01611         controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
01612                                        additional_controllers.end());
01613       else
01614         return false;
01615     }
01616     if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
01617     {
01618       if (controller_manager_)
01619       {
01620         // load controllers to be activated, if needed, and reset the state update cache
01621         for (std::size_t a = 0; a < controllers_to_activate.size(); ++a)
01622         {
01623           ControllerInformation& ci = known_controllers_[controllers_to_activate[a]];
01624           ci.last_update_ = ros::Time();
01625         }
01626         // reset the state update cache
01627         for (std::size_t a = 0; a < controllers_to_deactivate.size(); ++a)
01628           known_controllers_[controllers_to_deactivate[a]].last_update_ = ros::Time();
01629         return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
01630       }
01631       else
01632         return false;
01633     }
01634     else
01635       return true;
01636   }
01637   else
01638   {
01639     std::set<std::string> originally_active;
01640     for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01641          it != known_controllers_.end(); ++it)
01642       if (it->second.state_.active_)
01643         originally_active.insert(it->first);
01644     return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
01645   }
01646 }
01647 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:07