moveit_fake_controller_manager.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
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 Ioan A. Sucan 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, Dave Coleman, Robert Haschke */
00036 
00037 #include "moveit_fake_controllers.h"
00038 #include <ros/ros.h>
00039 #include <moveit/robot_state/robot_state.h>
00040 #include <moveit/robot_model_loader/robot_model_loader.h>
00041 #include <sensor_msgs/JointState.h>
00042 #include <pluginlib/class_list_macros.h>
00043 #include <map>
00044 #include <iterator>
00045 
00046 namespace moveit_fake_controller_manager
00047 {
00048 static const std::string DEFAULT_TYPE = "interpolate";
00049 static const std::string ROBOT_DESCRIPTION = "robot_description";
00050 
00051 class MoveItFakeControllerManager : public moveit_controller_manager::MoveItControllerManager
00052 {
00053 public:
00054   MoveItFakeControllerManager() : node_handle_("~")
00055   {
00056     if (!node_handle_.hasParam("controller_list"))
00057     {
00058       ROS_ERROR_STREAM("MoveItFakeControllerManager: No controller_list specified.");
00059       return;
00060     }
00061 
00062     XmlRpc::XmlRpcValue controller_list;
00063     node_handle_.getParam("controller_list", controller_list);
00064     if (controller_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00065     {
00066       ROS_ERROR("MoveItFakeControllerManager: controller_list should be specified as an array");
00067       return;
00068     }
00069 
00070     pub_ = node_handle_.advertise<sensor_msgs::JointState>("fake_controller_joint_states", 100, false);
00071 
00072     /* publish initial pose */
00073     XmlRpc::XmlRpcValue initial;
00074     if (node_handle_.getParam("initial", initial))
00075     {
00076       sensor_msgs::JointState js = loadInitialJointValues(initial);
00077       js.header.stamp = ros::Time::now();
00078       pub_.publish(js);
00079     }
00080 
00081     /* actually create each controller */
00082     for (int i = 0; i < controller_list.size(); ++i)
00083     {
00084       if (!controller_list[i].hasMember("name") || !controller_list[i].hasMember("joints"))
00085       {
00086         ROS_ERROR("MoveItFakeControllerManager: Name and joints must be specifed for each controller");
00087         continue;
00088       }
00089 
00090       try
00091       {
00092         std::string name = std::string(controller_list[i]["name"]);
00093 
00094         if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray)
00095         {
00096           ROS_ERROR_STREAM("MoveItFakeControllerManager: The list of joints for controller "
00097                            << name << " is not specified as an array");
00098           continue;
00099         }
00100         std::vector<std::string> joints;
00101         for (int j = 0; j < controller_list[i]["joints"].size(); ++j)
00102           joints.push_back(std::string(controller_list[i]["joints"][j]));
00103 
00104         const std::string& type =
00105             controller_list[i].hasMember("type") ? std::string(controller_list[i]["type"]) : DEFAULT_TYPE;
00106         if (type == "last point")
00107           controllers_[name].reset(new LastPointController(name, joints, pub_));
00108         else if (type == "via points")
00109           controllers_[name].reset(new ViaPointController(name, joints, pub_));
00110         else if (type == "interpolate")
00111           controllers_[name].reset(new InterpolatingController(name, joints, pub_));
00112         else
00113           ROS_ERROR_STREAM("Unknown fake controller type: " << type);
00114       }
00115       catch (...)
00116       {
00117         ROS_ERROR("MoveItFakeControllerManager: Unable to parse controller information");
00118       }
00119     }
00120   }
00121 
00122   sensor_msgs::JointState loadInitialJointValues(XmlRpc::XmlRpcValue& param) const
00123   {
00124     sensor_msgs::JointState js;
00125 
00126     if (param.getType() != XmlRpc::XmlRpcValue::TypeArray || param.size() == 0)
00127     {
00128       ROS_ERROR_ONCE_NAMED("loadInitialJointValues", "Parameter 'initial' should be an array of (group, pose) "
00129                                                      "structs.");
00130       return js;
00131     }
00132 
00133     robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION);
00134     robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
00135     typedef std::map<std::string, double> JointPoseMap;
00136     JointPoseMap joints;
00137 
00138     for (int i = 0, end = param.size(); i != end; ++i)
00139     {
00140       try
00141       {
00142         std::string group_name = std::string(param[i]["group"]);
00143         std::string pose_name = std::string(param[i]["pose"]);
00144         if (!robot_model->hasJointModelGroup(group_name))
00145         {
00146           ROS_WARN_STREAM_NAMED("loadInitialJointValues", "Unknown joint model group: " << group_name);
00147           continue;
00148         }
00149         moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name);
00150         moveit::core::RobotState robot_state(robot_model);
00151         const std::vector<std::string>& joint_names = jmg->getActiveJointModelNames();
00152 
00153         if (!robot_state.setToDefaultValues(jmg, pose_name))
00154         {
00155           ROS_WARN_NAMED("loadInitialJointValues", "Unknown pose '%s' for group '%s'.", pose_name.c_str(),
00156                          group_name.c_str());
00157           continue;
00158         }
00159         ROS_INFO_NAMED("loadInitialJointValues", "Set joints of group '%s' to pose '%s'.", group_name.c_str(),
00160                        pose_name.c_str());
00161 
00162         for (std::vector<std::string>::const_iterator jit = joint_names.begin(), end = joint_names.end(); jit != end;
00163              ++jit)
00164         {
00165           const moveit::core::JointModel* jm = robot_state.getJointModel(*jit);
00166           if (!jm)
00167           {
00168             ROS_WARN_STREAM_NAMED("loadInitialJointValues", "Unknown joint: " << *jit);
00169             continue;
00170           }
00171           if (jm->getVariableCount() != 1)
00172           {
00173             ROS_WARN_STREAM_NAMED("loadInitialJointValues", "Cannot handle multi-variable joint: " << *jit);
00174             continue;
00175           }
00176 
00177           joints[*jit] = robot_state.getJointPositions(jm)[0];
00178         }
00179       }
00180       catch (...)
00181       {
00182         ROS_ERROR_ONCE_NAMED("loadInitialJointValues", "Unable to parse initial pose information.");
00183       }
00184     }
00185 
00186     // fill the joint state
00187     for (JointPoseMap::const_iterator it = joints.begin(), end = joints.end(); it != end; ++it)
00188     {
00189       js.name.push_back(it->first);
00190       js.position.push_back(it->second);
00191     }
00192     return js;
00193   }
00194 
00195   virtual ~MoveItFakeControllerManager()
00196   {
00197   }
00198 
00199   /*
00200    * Get a controller, by controller name (which was specified in the controllers.yaml
00201    */
00202   virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name)
00203   {
00204     std::map<std::string, BaseFakeControllerPtr>::const_iterator it = controllers_.find(name);
00205     if (it != controllers_.end())
00206       return it->second;
00207     else
00208       ROS_FATAL_STREAM("No such controller: " << name);
00209     return moveit_controller_manager::MoveItControllerHandlePtr();
00210   }
00211 
00212   /*
00213    * Get the list of controller names.
00214    */
00215   virtual void getControllersList(std::vector<std::string>& names)
00216   {
00217     for (std::map<std::string, BaseFakeControllerPtr>::const_iterator it = controllers_.begin();
00218          it != controllers_.end(); ++it)
00219       names.push_back(it->first);
00220     ROS_INFO_STREAM("Returned " << names.size() << " controllers in list");
00221   }
00222 
00223   /*
00224    * Fake controllers are always active
00225    */
00226   virtual void getActiveControllers(std::vector<std::string>& names)
00227   {
00228     getControllersList(names);
00229   }
00230 
00231   /*
00232    * Fake controllers are always loaded
00233    */
00234   virtual void getLoadedControllers(std::vector<std::string>& names)
00235   {
00236     getControllersList(names);
00237   }
00238 
00239   /*
00240    * Get the list of joints that a controller can control.
00241    */
00242   virtual void getControllerJoints(const std::string& name, std::vector<std::string>& joints)
00243   {
00244     std::map<std::string, BaseFakeControllerPtr>::const_iterator it = controllers_.find(name);
00245     if (it != controllers_.end())
00246     {
00247       it->second->getJoints(joints);
00248     }
00249     else
00250     {
00251       ROS_WARN("The joints for controller '%s' are not known. Perhaps the controller configuration is not loaded on "
00252                "the param server?",
00253                name.c_str());
00254       joints.clear();
00255     }
00256   }
00257 
00258   /*
00259    * Controllers are all active and default.
00260    */
00261   virtual moveit_controller_manager::MoveItControllerManager::ControllerState
00262   getControllerState(const std::string& name)
00263   {
00264     moveit_controller_manager::MoveItControllerManager::ControllerState state;
00265     state.active_ = true;
00266     state.default_ = true;
00267     return state;
00268   }
00269 
00270   /* Cannot switch our controllers */
00271   virtual bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate)
00272   {
00273     return false;
00274   }
00275 
00276 protected:
00277   ros::NodeHandle node_handle_;
00278   ros::Publisher pub_;
00279   std::map<std::string, BaseFakeControllerPtr> controllers_;
00280 };
00281 
00282 }  // end namespace moveit_fake_controller_manager
00283 
00284 PLUGINLIB_EXPORT_CLASS(moveit_fake_controller_manager::MoveItFakeControllerManager,
00285                        moveit_controller_manager::MoveItControllerManager);


moveit_fake_controller_manager
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:26