pr2_moveit_controller_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, Sachin Chitta, E. Gil Jones */
00036 
00037 #include <ros/ros.h>
00038 #include <moveit/controller_manager/controller_manager.h>
00039 #include <control_msgs/FollowJointTrajectoryAction.h>
00040 #include <actionlib/client/simple_action_client.h>
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 #include <pr2_mechanism_msgs/ListControllers.h>
00044 #include <pr2_mechanism_msgs/SwitchController.h>
00045 #include <pr2_mechanism_msgs/LoadController.h>
00046 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00047 #include <algorithm>
00048 #include <map>
00049 
00050 namespace pr2_moveit_controller_manager
00051 {
00052 
00054 static const double DEFAULT_MAX_GRIPPER_EFFORT = 10000.0;
00055 
00057 static const double GRIPPER_OPEN = 0.086;
00058 
00060 static const double GRIPPER_CLOSED = 0.0;
00061 
00063 static const std::string R_GRIPPER_JOINT = "r_gripper_motor_screw_joint";
00064 
00066 static const std::string L_GRIPPER_JOINT = "l_gripper_motor_screw_joint";
00067 
00069 static const double GAP_CONVERSION_RATIO = 0.1714;
00070 
00071 template<typename T>
00072 class ActionBasedControllerHandle : public moveit_controller_manager::MoveItControllerHandle
00073 {
00074 public:
00075   ActionBasedControllerHandle(const std::string &name, const std::string &ns) :
00076     moveit_controller_manager::MoveItControllerHandle(name),
00077     namespace_(ns),
00078     done_(true)
00079   {
00080     controller_action_client_.reset(new actionlib::SimpleActionClient<T>(name_ +"/" + namespace_, true));
00081     unsigned int attempts = 0;
00082     while (ros::ok() && !controller_action_client_->waitForServer(ros::Duration(5.0)) && ++attempts < 3)
00083       ROS_INFO_STREAM("Waiting for " << name_ + "/" + namespace_ << " to come up");
00084 
00085     if (!controller_action_client_->isServerConnected())
00086     {
00087       ROS_ERROR_STREAM("Action client not connected: " << name_ + "/" + namespace_);
00088       controller_action_client_.reset();
00089     }
00090 
00091     last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00092   }
00093 
00094   bool isConnected() const
00095   {
00096     return controller_action_client_;
00097   }
00098 
00099   virtual bool cancelExecution()
00100   {
00101     if (!controller_action_client_)
00102       return false;
00103     if (!done_)
00104     {
00105       ROS_INFO_STREAM("Cancelling execution for " << name_);
00106       controller_action_client_->cancelGoal();
00107       last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
00108       done_ = true;
00109     }
00110     return true;
00111   }
00112 
00113   virtual bool waitForExecution(const ros::Duration &timeout = ros::Duration(0))
00114   {
00115     if (controller_action_client_ && !done_)
00116       return controller_action_client_->waitForResult(timeout);
00117     return true;
00118   }
00119 
00120   virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
00121   {
00122     return last_exec_;
00123   }
00124 
00125 protected:
00126 
00127   void finishControllerExecution(const actionlib::SimpleClientGoalState& state)
00128   {
00129     ROS_DEBUG_STREAM("Controller " << name_ << " is done with state " << state.toString() << ": " << state.getText());
00130     if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00131       last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00132     else
00133       if (state == actionlib::SimpleClientGoalState::ABORTED)
00134         last_exec_ = moveit_controller_manager::ExecutionStatus::ABORTED;
00135       else
00136         if (state == actionlib::SimpleClientGoalState::PREEMPTED)
00137           last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
00138         else
00139           last_exec_ = moveit_controller_manager::ExecutionStatus::FAILED;
00140     done_ = true;
00141   }
00142 
00143   moveit_controller_manager::ExecutionStatus last_exec_;
00144   std::string namespace_;
00145   bool done_;
00146   boost::shared_ptr<actionlib::SimpleActionClient<T> > controller_action_client_;
00147 };
00148 
00149 class Pr2GripperControllerHandle : public ActionBasedControllerHandle<pr2_controllers_msgs::Pr2GripperCommandAction>
00150 {
00151 public:
00152   Pr2GripperControllerHandle(const std::string &name, const std::string &ns  = "gripper_action") :
00153     ActionBasedControllerHandle<pr2_controllers_msgs::Pr2GripperCommandAction>(name, ns),
00154     closing_(false)
00155   {
00156   }
00157 
00158   virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
00159   {
00160     if (!controller_action_client_)
00161       return false;
00162     if (!trajectory.multi_dof_joint_trajectory.points.empty())
00163     {
00164       ROS_ERROR("The PR2 gripper controller cannot execute multi-dof trajectories.");
00165       return false;
00166     }
00167 
00168     if (trajectory.joint_trajectory.points.back().positions.empty())
00169     {
00170       ROS_ERROR("The PR2 gripper controller expects a joint trajectory with one point that specifies at least one position, but 0 positions provided)");
00171       return false;
00172     }
00173 
00174     if (trajectory.joint_trajectory.points.size() > 1)
00175       ROS_DEBUG("The PR2 gripper controller expects a joint trajectory with one point only, but %u provided. Using last point only.", (unsigned int)trajectory.joint_trajectory.points.size());
00176 
00177     int gripper_joint_index = -1;
00178     for (std::size_t i = 0 ; i < trajectory.joint_trajectory.joint_names.size() ; ++i)
00179       if (trajectory.joint_trajectory.joint_names[i] == R_GRIPPER_JOINT || trajectory.joint_trajectory.joint_names[i] == L_GRIPPER_JOINT)
00180       {
00181     gripper_joint_index = i;
00182     break;
00183       }
00184 
00185     if (!trajectory.joint_trajectory.joint_names.empty())
00186     {
00187       std::stringstream ss;
00188       ss << std::endl;
00189       for (std::size_t i = 0 ; i < trajectory.joint_trajectory.joint_names.size() ; ++i)
00190         ss << "PR2 gripper trajectory (" << i << "): " << trajectory.joint_trajectory.joint_names[i] << " " << trajectory.joint_trajectory.points[0].positions[i] << std::endl;
00191       ss << std::endl;
00192       ROS_DEBUG("%s", ss.str().c_str());
00193     }
00194 
00195     if (gripper_joint_index == -1)
00196     {
00197       ROS_ERROR("Could not find value for gripper virtual joint. Expected joint value for '%s' or '%s'.", L_GRIPPER_JOINT.c_str(), R_GRIPPER_JOINT.c_str());
00198       return false;
00199     }
00200 
00201     double gap_opening = trajectory.joint_trajectory.points.back().positions[gripper_joint_index] * GAP_CONVERSION_RATIO;
00202     ROS_DEBUG("PR2 gripper gap opening: %f", gap_opening);
00203     closing_ = false;
00204 
00205     if (gap_opening > GRIPPER_OPEN)
00206     {
00207       gap_opening = GRIPPER_OPEN;
00208       closing_ = false;
00209     }
00210     else
00211       if (gap_opening <= 0.0)
00212       {
00213         gap_opening = 0.0;
00214         closing_ = true;
00215       }
00216 
00217     pr2_controllers_msgs::Pr2GripperCommandGoal goal;
00218     goal.command.max_effort = DEFAULT_MAX_GRIPPER_EFFORT;
00219 
00220     goal.command.position = gap_opening;
00221     controller_action_client_->sendGoal(goal,
00222                                         boost::bind(&Pr2GripperControllerHandle::controllerDoneCallback, this, _1, _2),
00223                                         boost::bind(&Pr2GripperControllerHandle::controllerActiveCallback, this),
00224                                         boost::bind(&Pr2GripperControllerHandle::controllerFeedbackCallback, this, _1));
00225     done_ = false;
00226     last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
00227     return true;
00228   }
00229 
00230 private:
00231 
00232   void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
00233                               const pr2_controllers_msgs::Pr2GripperCommandResultConstPtr& result)
00234   {
00235     // the gripper action reports failure when closing the gripper and an object is inside
00236     if (state == actionlib::SimpleClientGoalState::ABORTED && closing_)
00237       finishControllerExecution(actionlib::SimpleClientGoalState::SUCCEEDED);
00238     else
00239       finishControllerExecution(state);
00240   }
00241 
00242   void controllerActiveCallback()
00243   {
00244     ROS_DEBUG_STREAM("Controller " << name_ << " started execution");
00245   }
00246 
00247   void controllerFeedbackCallback(const pr2_controllers_msgs::Pr2GripperCommandFeedbackConstPtr& feedback)
00248   {
00249   }
00250 
00251   bool closing_;
00252 };
00253 
00254 class Pr2FollowJointTrajectoryControllerHandle : public ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>
00255 {
00256 public:
00257 
00258   Pr2FollowJointTrajectoryControllerHandle(const std::string &name, const std::string &ns = "follow_joint_trajectory") :
00259     ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>(name, ns)
00260   {
00261   }
00262 
00263   virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
00264   {
00265     if (!controller_action_client_)
00266       return false;
00267     if (!trajectory.multi_dof_joint_trajectory.points.empty())
00268     {
00269       ROS_ERROR("The PR2 FollowJointTrajectory controller cannot execute multi-dof trajectories.");
00270       return false;
00271     }
00272     if (done_)
00273       ROS_DEBUG_STREAM("Sending trajectory to FollowJointTrajectory action for controller " << name_);
00274     else
00275       ROS_DEBUG_STREAM("Sending continuation for the currently executed trajectory to FollowJointTrajectory action for controller " << name_);
00276     control_msgs::FollowJointTrajectoryGoal goal;
00277     goal.trajectory = trajectory.joint_trajectory;
00278     controller_action_client_->sendGoal(goal,
00279                                         boost::bind(&Pr2FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
00280                                         boost::bind(&Pr2FollowJointTrajectoryControllerHandle::controllerActiveCallback, this),
00281                                         boost::bind(&Pr2FollowJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));
00282     done_ = false;
00283     last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
00284     return true;
00285   }
00286 
00287 protected:
00288 
00289   void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
00290                               const control_msgs::FollowJointTrajectoryResultConstPtr& result)
00291   {
00292     finishControllerExecution(state);
00293   }
00294 
00295   void controllerActiveCallback()
00296   {
00297     ROS_DEBUG_STREAM("Controller " << name_ << " started execution");
00298   }
00299 
00300   void controllerFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback)
00301   {
00302   }
00303 };
00304 
00305 class Pr2MoveItControllerManager : public moveit_controller_manager::MoveItControllerManager
00306 {
00307 public:
00308 
00309   Pr2MoveItControllerManager() : node_handle_("~")
00310   {
00311     if (node_handle_.hasParam("controller_manager_name"))
00312       node_handle_.getParam("controller_manager_name", controller_manager_name_);
00313     if (controller_manager_name_.empty())
00314       node_handle_.param("pr2_controller_manager_name", controller_manager_name_, std::string("pr2_controller_manager"));
00315     if (node_handle_.hasParam("use_controller_manager")) // 'use_controller_manager' is the old name
00316       node_handle_.param("use_controller_manager", use_controller_manager_, true);
00317     else
00318       node_handle_.param("use_pr2_controller_manager", use_controller_manager_, true);
00319 
00320     XmlRpc::XmlRpcValue controller_list;
00321     if (node_handle_.hasParam("controller_list"))
00322     {
00323       node_handle_.getParam("controller_list", controller_list);
00324       if (controller_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00325         ROS_WARN("Controller list should be specified as an array");
00326       else
00327         for (int i = 0 ; i < controller_list.size() ; ++i)
00328           if (!controller_list[i].hasMember("name") || !controller_list[i].hasMember("joints"))
00329             ROS_WARN("Name and joints must be specifed for each controller");
00330           else
00331           {
00332             try
00333             {
00334               ControllerInformation ci;
00335               std::string name = std::string(controller_list[i]["name"]);
00336               if (controller_list[i].hasMember("default"))
00337               {
00338                 try
00339                 {
00340                   if (controller_list[i]["default"].getType() == XmlRpc::XmlRpcValue::TypeBoolean)
00341                   {
00342                     ci.default_ = controller_list[i]["default"];
00343                   }
00344                   else
00345                   {
00346                     std::string def = std::string(controller_list[i]["default"]);
00347                     std::transform(def.begin(), def.end(), def.begin(), ::tolower);
00348                     if (def == "true" || def == "yes")
00349                       ci.default_ = true;
00350                   }
00351                 }
00352                 catch (...)
00353                 {
00354                 }
00355               }
00356               // 'ns' is the old name; use action_ns instead
00357               if (controller_list[i].hasMember("ns"))
00358               {
00359                 ci.ns_ = std::string(controller_list[i]["ns"]);
00360                 ROS_WARN("'ns' argument specified. Please use 'action_ns' instead.");
00361               }
00362               if (controller_list[i].hasMember("action_ns"))
00363                 ci.ns_ = std::string(controller_list[i]["action_ns"]);
00364               if (controller_list[i]["joints"].getType() == XmlRpc::XmlRpcValue::TypeArray)
00365               {
00366                 int nj = controller_list[i]["joints"].size();
00367                 for (int j = 0 ; j < nj ; ++j)
00368                   ci.joints_.push_back(std::string(controller_list[i]["joints"][j]));
00369               }
00370               else
00371                 ROS_WARN_STREAM("The list of joints for controller " << name << " is not specified as an array");
00372               if (!ci.joints_.empty())
00373                 possibly_unloaded_controllers_[name] = ci;
00374             }
00375             catch (...)
00376             {
00377               ROS_ERROR("Unable to parse controller information");
00378             }
00379           }
00380     }
00381     else
00382     {
00383       if (use_controller_manager_)
00384         ROS_DEBUG_STREAM("No controller list specified. Using list obtained from the " << controller_manager_name_);
00385       else
00386         ROS_ERROR_STREAM("Not using a controller manager and no controllers specified. There are no known controllers.");
00387     }
00388 
00389     if (use_controller_manager_)
00390     {
00391       static const unsigned int max_attempts = 5;
00392       unsigned int attempts = 0;
00393       while (ros::ok() && !ros::service::waitForService(controller_manager_name_ + "/list_controllers", ros::Duration(5.0)) && ++attempts < max_attempts)
00394         ROS_INFO_STREAM("Waiting for service " << controller_manager_name_ + "/list_controllers" << " to come up");
00395 
00396       if (attempts < max_attempts)
00397         while (ros::ok() && !ros::service::waitForService(controller_manager_name_ + "/switch_controller", ros::Duration(5.0)) && ++attempts < max_attempts)
00398           ROS_INFO_STREAM("Waiting for service " << controller_manager_name_ + "/switch_controller" << " to come up");
00399 
00400       if (attempts < max_attempts)
00401         while (ros::ok() && !ros::service::waitForService(controller_manager_name_ + "/load_controller", ros::Duration(5.0))  && ++attempts < max_attempts)
00402           ROS_INFO_STREAM("Waiting for service " << controller_manager_name_ + "/load_controller" << " to come up");
00403 
00404       if (attempts < max_attempts)
00405       {
00406         lister_service_ = root_node_handle_.serviceClient<pr2_mechanism_msgs::ListControllers>(controller_manager_name_ + "/list_controllers", true);
00407         switcher_service_ = root_node_handle_.serviceClient<pr2_mechanism_msgs::SwitchController>(controller_manager_name_ + "/switch_controller", true);
00408         loader_service_ = root_node_handle_.serviceClient<pr2_mechanism_msgs::LoadController>(controller_manager_name_ + "/load_controller", true);
00409       }
00410       else
00411         ROS_ERROR("Not using the PR2 controller manager");
00412     }
00413   }
00414 
00415   virtual ~Pr2MoveItControllerManager()
00416   {
00417   }
00418 
00419   virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string &name)
00420   {
00421     std::map<std::string, moveit_controller_manager::MoveItControllerHandlePtr>::const_iterator it = handle_cache_.find(name);
00422     if (it != handle_cache_.end())
00423       return it->second;
00424 
00425     moveit_controller_manager::MoveItControllerHandlePtr new_handle;
00426     if (possibly_unloaded_controllers_.find(name) != possibly_unloaded_controllers_.end())
00427     {
00428       const std::string &ns = possibly_unloaded_controllers_.at(name).ns_;
00429       new_handle = getControllerHandleHelper(name, ns);
00430     }
00431     else
00432       new_handle = getControllerHandleHelper(name, "");
00433 
00434     if (new_handle)
00435       handle_cache_[name] = new_handle;
00436     return new_handle;
00437   }
00438 
00439   virtual void getControllersList(std::vector<std::string> &names)
00440   {
00441     const pr2_mechanism_msgs::ListControllers::Response &res = getListControllerServiceResponse();
00442     std::set<std::string> names_set;
00443     names_set.insert(res.controllers.begin(), res.controllers.end());
00444 
00445     for (std::map<std::string, ControllerInformation>::const_iterator it = possibly_unloaded_controllers_.begin() ; it != possibly_unloaded_controllers_.end() ; ++it)
00446       names_set.insert(it->first);
00447 
00448     names.clear();
00449     names.insert(names.end(), names_set.begin(), names_set.end());
00450   }
00451 
00452   virtual void getActiveControllers(std::vector<std::string> &names)
00453   {
00454     names.clear();
00455     if (use_controller_manager_)
00456     {
00457       const pr2_mechanism_msgs::ListControllers::Response &res = getListControllerServiceResponse();
00458       for (std::size_t i = 0; i < res.controllers.size(); ++i)
00459         if (res.state[i] == "running")
00460           names.push_back(res.controllers[i]);
00461     }
00462     else
00463       // we assume best case scenario if we can't test whether the controller is active or not
00464       for (std::map<std::string, ControllerInformation>::const_iterator it = possibly_unloaded_controllers_.begin() ; it != possibly_unloaded_controllers_.end() ; ++it)
00465         names.push_back(it->first);
00466   }
00467 
00468   virtual void getControllerJoints(const std::string &name, std::vector<std::string> &joints)
00469   {
00470     std::map<std::string, ControllerInformation>::const_iterator it = possibly_unloaded_controllers_.find(name);
00471     if (it != possibly_unloaded_controllers_.end())
00472       joints = it->second.joints_;
00473     else
00474     {
00475       joints.clear();
00476       std::string param_name;
00477       if (node_handle_.searchParam(name + "/joints", param_name))
00478       {
00479         XmlRpc::XmlRpcValue joints_list;
00480         node_handle_.getParam(param_name, joints_list);
00481         if (joints_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00482           for (int i = 0 ; i < joints_list.size() ; ++i)
00483             joints.push_back((std::string)joints_list[i]);
00484       }
00485       else
00486         if (node_handle_.searchParam(name + "/joint", param_name))
00487         {
00488           std::string joint_name;
00489           if (node_handle_.getParam(param_name, joint_name))
00490             joints.push_back(joint_name);
00491         }
00492       if (joints.empty())
00493         ROS_INFO("The joints for controller '%s' are not known and were not found on the ROS param server under '%s/joints'or '%s/joint'. "
00494                  "Perhaps the controller configuration is not loaded on the param server?", name.c_str(), name.c_str(), name.c_str());
00495       ControllerInformation &ci = possibly_unloaded_controllers_[name];
00496       ci.joints_ = joints;
00497     }
00498   }
00499 
00500   virtual moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string &name)
00501   {
00502     moveit_controller_manager::MoveItControllerManager::ControllerState state;
00503     if (use_controller_manager_)
00504     {
00505       const pr2_mechanism_msgs::ListControllers::Response &res = getListControllerServiceResponse();
00506       for (std::size_t i = 0; i < res.controllers.size(); ++i)
00507       {
00508         if (res.controllers[i] == name)
00509         {
00510           if (res.state[i] == "running")
00511             state.active_ = true;
00512           break;
00513         }
00514       }
00515     }
00516     else
00517     {
00518       // if we cannot test, assume best case scenario.
00519       state.active_ = true;
00520     }
00521 
00522     std::map<std::string, ControllerInformation>::const_iterator it = possibly_unloaded_controllers_.find(name);
00523     if (it != possibly_unloaded_controllers_.end())
00524       if (it->second.default_)
00525         state.default_ = true;
00526     return state;
00527   }
00528 
00529   virtual bool switchControllers(const std::vector<std::string> &activate, const std::vector<std::string> &deactivate)
00530   {
00531     if (!use_controller_manager_)
00532     {
00533       ROS_WARN_STREAM("Cannot switch controllers without using the controller manager");
00534       return false;
00535     }
00536 
00537     if (!activate.empty())
00538     {
00539       // load controllers to be activated, if needed
00540       const std::vector<std::string> &loaded_c = getListControllerServiceResponse().controllers;
00541       std::set<std::string> loaded_c_set(loaded_c.begin(), loaded_c.end());
00542       for (std::size_t i = 0 ; i < activate.size() ; ++i)
00543         if (loaded_c_set.find(activate[i]) == loaded_c_set.end())
00544         {
00545           handle_cache_.erase(activate[i]);
00546           pr2_mechanism_msgs::LoadController::Request req;
00547           pr2_mechanism_msgs::LoadController::Response res;
00548           req.name = activate[i];
00549           if (!loader_service_.call(req, res))
00550           {
00551             ROS_WARN("Something went wrong with loader service while trying to load '%s'", req.name.c_str());
00552             return false;
00553           }
00554           if (!res.ok)
00555           {
00556             ROS_WARN("Loading controller '%s' failed", req.name.c_str());
00557             return false;
00558           }
00559         }
00560     }
00561 
00562     last_lister_response_ = ros::Time();
00563     pr2_mechanism_msgs::SwitchController::Request req;
00564     pr2_mechanism_msgs::SwitchController::Response res;
00565 
00566     req.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
00567     req.start_controllers = activate;
00568     req.stop_controllers = deactivate;
00569     if (!switcher_service_.call(req, res))
00570     {
00571       ROS_WARN_STREAM("Something went wrong with switcher service");
00572       return false;
00573     }
00574     if (!res.ok)
00575       ROS_WARN_STREAM("Switcher service failed");
00576     return res.ok;
00577   }
00578 
00579 protected:
00580 
00581   const pr2_mechanism_msgs::ListControllers::Response &getListControllerServiceResponse()
00582   {
00583     if (use_controller_manager_)
00584     {
00585       static const ros::Duration max_cache_age(1.0);
00586       if ((ros::Time::now() - last_lister_response_) > max_cache_age)
00587       {
00588         pr2_mechanism_msgs::ListControllers::Request req;
00589         if (!lister_service_.call(req, cached_lister_response_))
00590           ROS_WARN_STREAM("Something went wrong with lister service");
00591         last_lister_response_ = ros::Time::now();
00592       }
00593     }
00594     return cached_lister_response_;
00595   }
00596 
00597   moveit_controller_manager::MoveItControllerHandlePtr getControllerHandleHelper(const std::string &name, const std::string &ns)
00598   {
00599     moveit_controller_manager::MoveItControllerHandlePtr new_handle;
00600     if (name == "l_gripper_controller" || name == "r_gripper_controller")
00601     {
00602       new_handle.reset(ns.empty() ? new Pr2GripperControllerHandle(name) : new Pr2GripperControllerHandle(name, ns));
00603       if (!static_cast<Pr2GripperControllerHandle*>(new_handle.get())->isConnected())
00604         new_handle.reset();
00605     }
00606     else
00607     {
00608       new_handle.reset(ns.empty() ? new Pr2FollowJointTrajectoryControllerHandle(name) : new Pr2FollowJointTrajectoryControllerHandle(name, ns));
00609       if (!static_cast<Pr2FollowJointTrajectoryControllerHandle*>(new_handle.get())->isConnected())
00610         new_handle.reset();
00611     }
00612     return new_handle;
00613   }
00614 
00615   ros::NodeHandle node_handle_;
00616   ros::NodeHandle root_node_handle_;
00617 
00618   std::string controller_manager_name_;
00619   bool use_controller_manager_;
00620   ros::ServiceClient loader_service_;
00621   ros::ServiceClient switcher_service_;
00622   ros::ServiceClient lister_service_;
00623 
00624   ros::Time last_lister_response_;
00625   pr2_mechanism_msgs::ListControllers::Response cached_lister_response_;
00626 
00627   std::map<std::string, moveit_controller_manager::MoveItControllerHandlePtr> handle_cache_;
00628 
00629   struct ControllerInformation
00630   {
00631     ControllerInformation() : default_(false)
00632     {
00633     }
00634 
00635     bool default_;
00636     std::string ns_;
00637     std::vector<std::string> joints_;
00638   };
00639   std::map<std::string, ControllerInformation> possibly_unloaded_controllers_;
00640 };
00641 
00642 }
00643 
00644 PLUGINLIB_EXPORT_CLASS(pr2_moveit_controller_manager::Pr2MoveItControllerManager,
00645                        moveit_controller_manager::MoveItControllerManager);


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Sep 14 2015 14:17:49