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             parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
00874                 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
00875         }
00876       }
00877       if (!intersect_single.empty())
00878       {
00879         std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
00880         jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
00881         parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
00882         std::map<std::string, std::size_t> index;
00883         for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
00884           index[trajectory.joint_trajectory.joint_names[j]] = j;
00885         std::vector<std::size_t> bijection(jnames.size());
00886         for (std::size_t j = 0; j < jnames.size(); ++j)
00887           bijection[j] = index[jnames[j]];
00888         parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
00889         for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
00890         {
00891           parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
00892           if (!trajectory.joint_trajectory.points[j].positions.empty())
00893           {
00894             parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
00895             for (std::size_t k = 0; k < bijection.size(); ++k)
00896               parts[i].joint_trajectory.points[j].positions[k] =
00897                   trajectory.joint_trajectory.points[j].positions[bijection[k]];
00898           }
00899           if (!trajectory.joint_trajectory.points[j].velocities.empty())
00900           {
00901             parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
00902             for (std::size_t k = 0; k < bijection.size(); ++k)
00903               parts[i].joint_trajectory.points[j].velocities[k] =
00904                   trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
00905           }
00906           if (!trajectory.joint_trajectory.points[j].accelerations.empty())
00907           {
00908             parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
00909             for (std::size_t k = 0; k < bijection.size(); ++k)
00910               parts[i].joint_trajectory.points[j].accelerations[k] =
00911                   trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
00912           }
00913           if (!trajectory.joint_trajectory.points[j].effort.empty())
00914           {
00915             parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
00916             for (std::size_t k = 0; k < bijection.size(); ++k)
00917               parts[i].joint_trajectory.points[j].effort[k] =
00918                   trajectory.joint_trajectory.points[j].effort[bijection[k]];
00919           }
00920         }
00921       }
00922     }
00923   }
00924   return true;
00925 }
00926 
00927 bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const
00928 {
00929   if (!csm_ || allowed_start_tolerance_ == 0)  // skip validation if csm is nil or on this magic number
00930     return true;
00931 
00932   ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
00933 
00934   robot_state::RobotStatePtr current_state;
00935   if (!csm_->waitForCurrentState(1.0) || !(current_state = csm_->getCurrentState()))
00936   {
00937     ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within "
00938                                      "1s");
00939     return false;
00940   }
00941 
00942   for (std::vector<moveit_msgs::RobotTrajectory>::const_iterator traj_it = context.trajectory_parts_.begin();
00943        traj_it != context.trajectory_parts_.end(); ++traj_it)
00944   {
00945     const std::vector<double>& positions = traj_it->joint_trajectory.points.front().positions;
00946     const std::vector<std::string>& joint_names = traj_it->joint_trajectory.joint_names;
00947     const std::size_t n = joint_names.size();
00948     if (positions.size() != n)
00949     {
00950       ROS_ERROR_NAMED("traj_execution", "Wrong trajectory: #joints: %zu != #positions: %zu", n, positions.size());
00951       return false;
00952     }
00953 
00954     for (std::size_t i = 0; i < n; ++i)
00955     {
00956       const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
00957       if (!jm)
00958       {
00959         ROS_ERROR_STREAM_NAMED("traj_execution", "Unknown joint in trajectory: " << joint_names[i]);
00960         return false;
00961       }
00962 
00963       // TODO: check multi-DoF joints ?
00964       double cur_position = current_state->getJointPositions(jm)[0];
00965       double traj_position = positions[i];
00966       // normalize positions and compare
00967       jm->enforcePositionBounds(&cur_position);
00968       jm->enforcePositionBounds(&traj_position);
00969       if (fabs(cur_position - traj_position) > allowed_start_tolerance_)
00970       {
00971         ROS_ERROR_NAMED("traj_execution",
00972                         "\nInvalid Trajectory: start point deviates from current robot state more than %g"
00973                         "\njoint '%s': expected: %g, current: %g",
00974                         allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
00975         return false;
00976       }
00977     }
00978   }
00979   return true;
00980 }
00981 
00982 bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
00983                                            const moveit_msgs::RobotTrajectory& trajectory,
00984                                            const std::vector<std::string>& controllers)
00985 {
00986   if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
00987   {
00988     ROS_WARN_NAMED("traj_execution", "The trajectory to execute is empty");
00989     return false;
00990   }
00991   std::set<std::string> actuated_joints;
00992   actuated_joints.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
00993                          trajectory.multi_dof_joint_trajectory.joint_names.end());
00994   actuated_joints.insert(trajectory.joint_trajectory.joint_names.begin(),
00995                          trajectory.joint_trajectory.joint_names.end());
00996   if (actuated_joints.empty())
00997   {
00998     ROS_WARN_NAMED("traj_execution", "The trajectory to execute specifies no joints");
00999     return false;
01000   }
01001 
01002   if (controllers.empty())
01003   {
01004     bool retry = true;
01005     bool reloaded = false;
01006     while (retry)
01007     {
01008       retry = false;
01009       std::vector<std::string> all_controller_names;
01010       for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01011            it != known_controllers_.end(); ++it)
01012         all_controller_names.push_back(it->first);
01013       if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
01014       {
01015         if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
01016           return true;
01017       }
01018       else
01019       {
01020         // maybe we failed because we did not have a complete list of controllers
01021         if (!reloaded)
01022         {
01023           reloadControllerInformation();
01024           reloaded = true;
01025           retry = true;
01026         }
01027       }
01028     }
01029   }
01030   else
01031   {
01032     // check if the specified controllers are valid names;
01033     // if they appear not to be, try to reload the controller information, just in case they are new in the system
01034     bool reloaded = false;
01035     for (std::size_t i = 0; i < controllers.size(); ++i)
01036       if (known_controllers_.find(controllers[i]) == known_controllers_.end())
01037       {
01038         reloadControllerInformation();
01039         reloaded = true;
01040         break;
01041       }
01042     if (reloaded)
01043       for (std::size_t i = 0; i < controllers.size(); ++i)
01044         if (known_controllers_.find(controllers[i]) == known_controllers_.end())
01045         {
01046           ROS_ERROR_NAMED("traj_execution", "Controller '%s' is not known", controllers[i].c_str());
01047           return false;
01048         }
01049     if (selectControllers(actuated_joints, controllers, context.controllers_))
01050     {
01051       if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
01052         return true;
01053     }
01054   }
01055   std::stringstream ss;
01056   for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
01057     ss << *it << " ";
01058   ROS_ERROR_NAMED("traj_execution",
01059                   "Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
01060                   ss.str().c_str());
01061 
01062   std::stringstream ss2;
01063   std::map<std::string, ControllerInformation>::const_iterator mi;
01064   for (mi = known_controllers_.begin(); mi != known_controllers_.end(); mi++)
01065   {
01066     ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
01067 
01068     std::set<std::string>::const_iterator ji;
01069     for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
01070     {
01071       ss2 << "  " << *ji << std::endl;
01072     }
01073   }
01074   ROS_ERROR_NAMED("traj_execution", "Known controllers and their joints:\n%s", ss2.str().c_str());
01075   return false;
01076 }
01077 
01078 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::executeAndWait(bool auto_clear)
01079 {
01080   execute(ExecutionCompleteCallback(), auto_clear);
01081   return waitForExecution();
01082 }
01083 
01084 void TrajectoryExecutionManager::stopExecutionInternal()
01085 {
01086   // execution_state_mutex_ needs to have been locked by the caller
01087   for (std::size_t i = 0; i < active_handles_.size(); ++i)
01088     try
01089     {
01090       active_handles_[i]->cancelExecution();
01091     }
01092     catch (...)
01093     {
01094       ROS_ERROR_NAMED("traj_execution", "Exception caught when canceling execution.");
01095     }
01096 }
01097 
01098 void TrajectoryExecutionManager::stopExecution(bool auto_clear)
01099 {
01100   stop_continuous_execution_ = true;
01101   continuous_execution_condition_.notify_all();
01102 
01103   if (!execution_complete_)
01104   {
01105     execution_state_mutex_.lock();
01106     if (!execution_complete_)
01107     {
01108       // we call cancel for all active handles; we know these are not being modified as we loop through them because of
01109       // the lock
01110       // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external
01111       // trigger to stop has been received
01112       execution_complete_ = true;
01113       stopExecutionInternal();
01114 
01115       // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
01116       last_execution_status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
01117       execution_state_mutex_.unlock();
01118       ROS_INFO_NAMED("traj_execution", "Stopped trajectory execution.");
01119 
01120       // wait for the execution thread to finish
01121       execution_thread_->join();
01122       execution_thread_.reset();
01123 
01124       if (auto_clear)
01125         clear();
01126     }
01127     else
01128       execution_state_mutex_.unlock();
01129   }
01130   else if (execution_thread_)  // just in case we have some thread waiting to be joined from some point in the past, we
01131                                // join it now
01132   {
01133     execution_thread_->join();
01134     execution_thread_.reset();
01135   }
01136 }
01137 
01138 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback, bool auto_clear)
01139 {
01140   execute(callback, PathSegmentCompleteCallback(), auto_clear);
01141 }
01142 
01143 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback,
01144                                          const PathSegmentCompleteCallback& part_callback, bool auto_clear)
01145 {
01146   stopExecution(false);
01147 
01148   // check whether first trajectory starts at current robot state
01149   if (trajectories_.size() && !validate(*trajectories_.front()))
01150   {
01151     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01152     if (auto_clear)
01153       clear();
01154     if (callback)
01155       callback(last_execution_status_);
01156     return;
01157   }
01158 
01159   // start the execution thread
01160   execution_complete_ = false;
01161   execution_thread_.reset(
01162       new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
01163 }
01164 
01165 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::waitForExecution()
01166 {
01167   {
01168     boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
01169     while (!execution_complete_)
01170       execution_complete_condition_.wait(ulock);
01171   }
01172   {
01173     boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
01174     while (!continuous_execution_queue_.empty())
01175       continuous_execution_condition_.wait(ulock);
01176   }
01177 
01178   // this will join the thread for executing sequences of trajectories
01179   stopExecution(false);
01180 
01181   return last_execution_status_;
01182 }
01183 
01184 void TrajectoryExecutionManager::clear()
01185 {
01186   if (execution_complete_)
01187   {
01188     for (std::size_t i = 0; i < trajectories_.size(); ++i)
01189       delete trajectories_[i];
01190     trajectories_.clear();
01191     {
01192       boost::mutex::scoped_lock slock(continuous_execution_mutex_);
01193       while (!continuous_execution_queue_.empty())
01194       {
01195         delete continuous_execution_queue_.front();
01196         continuous_execution_queue_.pop_front();
01197       }
01198     }
01199   }
01200   else
01201     ROS_ERROR_NAMED("traj_execution", "Cannot push a new trajectory while another is being executed");
01202 }
01203 
01204 void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback,
01205                                                const PathSegmentCompleteCallback& part_callback, bool auto_clear)
01206 {
01207   // if we already got a stop request before we even started anything, we abort
01208   if (execution_complete_)
01209   {
01210     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01211     if (callback)
01212       callback(last_execution_status_);
01213     return;
01214   }
01215 
01216   ROS_DEBUG_NAMED("traj_execution", "Starting trajectory execution ...");
01217   // assume everything will be OK
01218   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
01219 
01220   // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
01221   // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
01222   for (std::size_t i = 0; i < trajectories_.size(); ++i)
01223   {
01224     bool epart = executePart(i);
01225     if (epart && part_callback)
01226       part_callback(i);
01227     if (!epart || execution_complete_)
01228       break;
01229   }
01230 
01231   ROS_DEBUG_NAMED("traj_execution", "Completed trajectory execution with status %s ...",
01232                   last_execution_status_.asString().c_str());
01233 
01234   // notify whoever is waiting for the event of trajectory completion
01235   execution_state_mutex_.lock();
01236   execution_complete_ = true;
01237   execution_state_mutex_.unlock();
01238   execution_complete_condition_.notify_all();
01239 
01240   // clear the paths just executed, if needed
01241   if (auto_clear)
01242     clear();
01243 
01244   // call user-specified callback
01245   if (callback)
01246     callback(last_execution_status_);
01247 }
01248 
01249 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
01250 {
01251   TrajectoryExecutionContext& context = *trajectories_[part_index];
01252 
01253   // first make sure desired controllers are active
01254   if (ensureActiveControllers(context.controllers_))
01255   {
01256     // stop if we are already asked to do so
01257     if (execution_complete_)
01258       return false;
01259 
01260     std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
01261     {
01262       boost::mutex::scoped_lock slock(execution_state_mutex_);
01263       if (!execution_complete_)
01264       {
01265         // time indexing uses this member too, so we lock this mutex as well
01266         time_index_mutex_.lock();
01267         current_context_ = part_index;
01268         time_index_mutex_.unlock();
01269         active_handles_.resize(context.controllers_.size());
01270         for (std::size_t i = 0; i < context.controllers_.size(); ++i)
01271         {
01272           moveit_controller_manager::MoveItControllerHandlePtr h;
01273           try
01274           {
01275             h = controller_manager_->getControllerHandle(context.controllers_[i]);
01276           }
01277           catch (...)
01278           {
01279             ROS_ERROR_NAMED("traj_execution", "Exception caught when retrieving controller handle");
01280           }
01281           if (!h)
01282           {
01283             active_handles_.clear();
01284             current_context_ = -1;
01285             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01286             ROS_ERROR_NAMED("traj_execution", "No controller handle for controller '%s'. Aborting.",
01287                             context.controllers_[i].c_str());
01288             return false;
01289           }
01290           active_handles_[i] = h;
01291         }
01292         handles = active_handles_;  // keep a copy for later, to avoid thread safety issues
01293         for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
01294         {
01295           bool ok = false;
01296           try
01297           {
01298             ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
01299           }
01300           catch (...)
01301           {
01302             ROS_ERROR_NAMED("traj_execution", "Exception caught when sending trajectory to controller");
01303           }
01304           if (!ok)
01305           {
01306             for (std::size_t j = 0; j < i; ++j)
01307               try
01308               {
01309                 active_handles_[j]->cancelExecution();
01310               }
01311               catch (...)
01312               {
01313                 ROS_ERROR_NAMED("traj_execution", "Exception caught when canceling execution");
01314               }
01315             ROS_ERROR_NAMED("traj_execution", "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
01316                             context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
01317             if (i > 0)
01318               ROS_ERROR_NAMED("traj_execution", "Cancelling previously sent trajectory parts");
01319             active_handles_.clear();
01320             current_context_ = -1;
01321             last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01322             return false;
01323           }
01324         }
01325       }
01326     }
01327 
01328     // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
01329     ros::Time current_time = ros::Time::now();
01330     ros::Duration expected_trajectory_duration(0.0);
01331     int longest_part = -1;
01332     for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
01333     {
01334       ros::Duration d(0.0);
01335       if (!context.trajectory_parts_[i].joint_trajectory.points.empty())
01336       {
01337         if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
01338           d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
01339         if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
01340           d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
01341         d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
01342                           ros::Duration(0.0) :
01343                           context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
01344                       context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
01345                           ros::Duration(0.0) :
01346                           context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
01347 
01348         if (longest_part < 0 ||
01349             std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
01350                      context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
01351                 std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
01352                          context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
01353           longest_part = i;
01354       }
01355       expected_trajectory_duration = std::max(d, expected_trajectory_duration);
01356     }
01357     // add 10% + 0.5s to the expected duration; this is just to allow things to finish propery
01358 
01359     expected_trajectory_duration = expected_trajectory_duration * allowed_execution_duration_scaling_ +
01360                                    ros::Duration(allowed_goal_duration_margin_);
01361 
01362     if (longest_part >= 0)
01363     {
01364       boost::mutex::scoped_lock slock(time_index_mutex_);
01365 
01366       // construct a map from expected time to state index, for easy access to expected state location
01367       if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
01368           context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
01369       {
01370         ros::Duration d(0.0);
01371         if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
01372           d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
01373         for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size(); ++j)
01374           time_index_.push_back(current_time + d +
01375                                 context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
01376       }
01377       else
01378       {
01379         ros::Duration d(0.0);
01380         if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
01381           d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
01382         for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size();
01383              ++j)
01384           time_index_.push_back(
01385               current_time + d +
01386               context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
01387       }
01388     }
01389 
01390     bool result = true;
01391     for (std::size_t i = 0; i < handles.size(); ++i)
01392     {
01393       if (execution_duration_monitoring_)
01394       {
01395         if (!handles[i]->waitForExecution(expected_trajectory_duration))
01396           if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
01397           {
01398             ROS_ERROR_NAMED("traj_execution", "Controller is taking too long to execute trajectory (the expected upper "
01399                                               "bound for the trajectory execution was %lf seconds). Stopping "
01400                                               "trajectory.",
01401                             expected_trajectory_duration.toSec());
01402             {
01403               boost::mutex::scoped_lock slock(execution_state_mutex_);
01404               stopExecutionInternal();  // this is trally tricky. we can't call stopExecution() here, so we call the
01405                                         // internal function only
01406             }
01407             last_execution_status_ = moveit_controller_manager::ExecutionStatus::TIMED_OUT;
01408             result = false;
01409             break;
01410           }
01411       }
01412       else
01413         handles[i]->waitForExecution();
01414 
01415       // if something made the trajectory stop, we stop this thread too
01416       if (execution_complete_)
01417       {
01418         result = false;
01419         break;
01420       }
01421       else if (handles[i]->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::SUCCEEDED)
01422       {
01423         ROS_WARN_STREAM_NAMED("traj_execution", "Controller handle "
01424                                                     << handles[i]->getName() << " reports status "
01425                                                     << handles[i]->getLastExecutionStatus().asString());
01426         last_execution_status_ = handles[i]->getLastExecutionStatus();
01427         result = false;
01428       }
01429     }
01430 
01431     // clear the active handles
01432     execution_state_mutex_.lock();
01433     active_handles_.clear();
01434 
01435     // clear the time index
01436     time_index_mutex_.lock();
01437     time_index_.clear();
01438     current_context_ = -1;
01439     time_index_mutex_.unlock();
01440 
01441     execution_state_mutex_.unlock();
01442     return result;
01443   }
01444   else
01445   {
01446     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
01447     return false;
01448   }
01449 }
01450 
01451 std::pair<int, int> TrajectoryExecutionManager::getCurrentExpectedTrajectoryIndex() const
01452 {
01453   boost::mutex::scoped_lock slock(time_index_mutex_);
01454   if (current_context_ < 0)
01455     return std::make_pair(-1, -1);
01456   if (time_index_.empty())
01457     return std::make_pair((int)current_context_, -1);
01458   std::vector<ros::Time>::const_iterator it =
01459       std::lower_bound(time_index_.begin(), time_index_.end(), ros::Time::now());
01460   int pos = it - time_index_.begin();
01461   return std::make_pair((int)current_context_, pos);
01462 }
01463 
01464 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
01465 TrajectoryExecutionManager::getTrajectories() const
01466 {
01467   return trajectories_;
01468 }
01469 
01470 moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::getLastExecutionStatus() const
01471 {
01472   return last_execution_status_;
01473 }
01474 
01475 bool TrajectoryExecutionManager::ensureActiveControllersForGroup(const std::string& group)
01476 {
01477   const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group);
01478   if (joint_model_group)
01479     return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
01480   else
01481     return false;
01482 }
01483 
01484 bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string>& joints)
01485 {
01486   std::vector<std::string> all_controller_names;
01487   for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01488        it != known_controllers_.end(); ++it)
01489     all_controller_names.push_back(it->first);
01490   std::vector<std::string> selected_controllers;
01491   std::set<std::string> jset;
01492   for (std::size_t i = 0; i < joints.size(); ++i)
01493   {
01494     const robot_model::JointModel* jm = robot_model_->getJointModel(joints[i]);
01495     if (jm)
01496     {
01497       if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
01498         continue;
01499       jset.insert(joints[i]);
01500     }
01501   }
01502 
01503   if (selectControllers(jset, all_controller_names, selected_controllers))
01504     return ensureActiveControllers(selected_controllers);
01505   else
01506     return false;
01507 }
01508 
01509 bool TrajectoryExecutionManager::ensureActiveController(const std::string& controller)
01510 {
01511   return ensureActiveControllers(std::vector<std::string>(1, controller));
01512 }
01513 
01514 bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string>& controllers)
01515 {
01516   updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
01517 
01518   if (manage_controllers_)
01519   {
01520     std::vector<std::string> controllers_to_activate;
01521     std::vector<std::string> controllers_to_deactivate;
01522     std::set<std::string> joints_to_be_activated;
01523     std::set<std::string> joints_to_be_deactivated;
01524     for (std::size_t i = 0; i < controllers.size(); ++i)
01525     {
01526       std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controllers[i]);
01527       if (it == known_controllers_.end())
01528       {
01529         ROS_ERROR_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " is not known");
01530         return false;
01531       }
01532       if (!it->second.state_.active_)
01533       {
01534         ROS_DEBUG_STREAM_NAMED("traj_execution", "Need to activate " << controllers[i]);
01535         controllers_to_activate.push_back(controllers[i]);
01536         joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
01537         for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin();
01538              kt != it->second.overlapping_controllers_.end(); ++kt)
01539         {
01540           const ControllerInformation& ci = known_controllers_[*kt];
01541           if (ci.state_.active_)
01542           {
01543             controllers_to_deactivate.push_back(*kt);
01544             joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
01545           }
01546         }
01547       }
01548       else
01549         ROS_DEBUG_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " is already active");
01550     }
01551     std::set<std::string> diff;
01552     std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
01553                         joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
01554     if (!diff.empty())
01555     {
01556       // find the set of controllers that do not overlap with the ones we want to activate so far
01557       std::vector<std::string> possible_additional_controllers;
01558       for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01559            it != known_controllers_.end(); ++it)
01560       {
01561         bool ok = true;
01562         for (std::size_t k = 0; k < controllers_to_activate.size(); ++k)
01563           if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) !=
01564               it->second.overlapping_controllers_.end())
01565           {
01566             ok = false;
01567             break;
01568           }
01569         if (ok)
01570           possible_additional_controllers.push_back(it->first);
01571       }
01572 
01573       // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
01574       std::vector<std::string> additional_controllers;
01575       if (selectControllers(diff, possible_additional_controllers, additional_controllers))
01576         controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
01577                                        additional_controllers.end());
01578       else
01579         return false;
01580     }
01581     if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
01582     {
01583       if (controller_manager_)
01584       {
01585         // load controllers to be activated, if needed, and reset the state update cache
01586         for (std::size_t a = 0; a < controllers_to_activate.size(); ++a)
01587         {
01588           ControllerInformation& ci = known_controllers_[controllers_to_activate[a]];
01589           ci.last_update_ = ros::Time();
01590         }
01591         // reset the state update cache
01592         for (std::size_t a = 0; a < controllers_to_deactivate.size(); ++a)
01593           known_controllers_[controllers_to_deactivate[a]].last_update_ = ros::Time();
01594         return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
01595       }
01596       else
01597         return false;
01598     }
01599     else
01600       return true;
01601   }
01602   else
01603   {
01604     std::set<std::string> originally_active;
01605     for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
01606          it != known_controllers_.end(); ++it)
01607       if (it->second.state_.active_)
01608         originally_active.insert(it->first);
01609     return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
01610   }
01611 }
01612 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:19