Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039 #include <moveit_simple_controller_manager/action_based_controller_handle.h>
00040 #include <moveit_simple_controller_manager/gripper_controller_handle.h>
00041 #include <moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h>
00042 #include <pluginlib/class_list_macros.h>
00043 #include <algorithm>
00044 #include <map>
00045
00046 namespace moveit_simple_controller_manager
00047 {
00048
00049
00050 class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItControllerManager
00051 {
00052 public:
00053
00054 MoveItSimpleControllerManager() : node_handle_("~")
00055 {
00056 if (!node_handle_.hasParam("controller_list"))
00057 {
00058 ROS_ERROR_STREAM("MoveitSimpleControllerManager: 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("MoveitSimpleControllerManager: controller_list should be specified as an array");
00067 return;
00068 }
00069
00070
00071 for (int i = 0 ; i < controller_list.size() ; ++i)
00072 {
00073 if (!controller_list[i].hasMember("name") || !controller_list[i].hasMember("joints"))
00074 {
00075 ROS_ERROR("MoveitSimpleControllerManager: Name and joints must be specifed for each controller");
00076 continue;
00077 }
00078
00079 try
00080 {
00081 std::string name = std::string(controller_list[i]["name"]);
00082
00083 std::string action_ns;
00084 if (controller_list[i].hasMember("ns"))
00085 {
00086
00087 action_ns = std::string(controller_list[i]["ns"]);
00088 ROS_WARN("MoveitSimpleControllerManager: use of 'ns' is deprecated, use 'action_ns' instead.");
00089 }
00090 else if (controller_list[i].hasMember("action_ns"))
00091 action_ns = std::string(controller_list[i]["action_ns"]);
00092 else
00093 ROS_WARN("MoveitSimpleControllerManager: please note that 'action_ns' no longer has a default value.");
00094
00095 if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray)
00096 {
00097 ROS_ERROR_STREAM("MoveitSimpleControllerManager: The list of joints for controller " << name << " is not specified as an array");
00098 continue;
00099 }
00100
00101 if (!controller_list[i].hasMember("type"))
00102 {
00103 ROS_ERROR_STREAM("MoveitSimpleControllerManager: No type specified for controller " << name);
00104 continue;
00105 }
00106
00107 std::string type = std::string(controller_list[i]["type"]);
00108
00109 ActionBasedControllerHandleBasePtr new_handle;
00110 if ( type == "GripperCommand" )
00111 {
00112 new_handle.reset(new GripperControllerHandle(name, action_ns));
00113 if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected())
00114 {
00115 if (controller_list[i].hasMember("command_joint"))
00116 static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(controller_list[i]["command_joint"]);
00117 else
00118 static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(controller_list[i]["joints"][0]);
00119
00120 if (controller_list[i].hasMember("allow_failure"))
00121 static_cast<GripperControllerHandle*>(new_handle.get())->allowFailure(true);
00122
00123 ROS_INFO_STREAM("MoveitSimpleControllerManager: Added GripperCommand controller for " << name );
00124 controllers_[name] = new_handle;
00125 }
00126 }
00127 else if ( type == "FollowJointTrajectory" )
00128 {
00129 new_handle.reset(new FollowJointTrajectoryControllerHandle(name, action_ns));
00130 if (static_cast<FollowJointTrajectoryControllerHandle*>(new_handle.get())->isConnected())
00131 {
00132 ROS_INFO_STREAM("MoveitSimpleControllerManager: Added FollowJointTrajectory controller for " << name );
00133 controllers_[name] = new_handle;
00134 }
00135 }
00136
00137
00138 for (int j = 0 ; j < controller_list[i]["joints"].size() ; ++j)
00139 controllers_[name]->addJoint(std::string(controller_list[i]["joints"][j]));
00140 }
00141 catch (...)
00142 {
00143 ROS_ERROR("MoveitSimpleControllerManager: Unable to parse controller information");
00144 }
00145 }
00146 }
00147
00148 virtual ~MoveItSimpleControllerManager()
00149 {
00150 }
00151
00152
00153
00154
00155 virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string &name)
00156 {
00157 std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.find(name);
00158 if (it != controllers_.end())
00159 return static_cast<moveit_controller_manager::MoveItControllerHandlePtr>(it->second);
00160 else
00161 ROS_FATAL_STREAM("No such controller: " << name);
00162 }
00163
00164
00165
00166
00167 virtual void getControllersList(std::vector<std::string> &names)
00168 {
00169 for (std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it)
00170 names.push_back(it->first);
00171 ROS_INFO_STREAM("Returned " << names.size() << " controllers in list");
00172 }
00173
00174
00175
00176
00177 virtual void getActiveControllers(std::vector<std::string> &names)
00178 {
00179 getControllersList(names);
00180 }
00181
00182
00183
00184
00185 virtual void getLoadedControllers(std::vector<std::string> &names)
00186 {
00187 getControllersList(names);
00188 }
00189
00190
00191
00192
00193 virtual void getControllerJoints(const std::string &name, std::vector<std::string> &joints)
00194 {
00195 std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.find(name);
00196 if (it != controllers_.end())
00197 {
00198 it->second->getJoints(joints);
00199 }
00200 else
00201 {
00202 ROS_WARN("The joints for controller '%s' are not known. Perhaps the controller configuration is not loaded on the param server?", name.c_str());
00203 joints.clear();
00204 }
00205 }
00206
00207
00208
00209
00210 virtual moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string &name)
00211 {
00212 moveit_controller_manager::MoveItControllerManager::ControllerState state;
00213 state.active_ = true;
00214 state.default_ = true;
00215 return state;
00216 }
00217
00218
00219 virtual bool switchControllers(const std::vector<std::string> &activate, const std::vector<std::string> &deactivate) { return false; }
00220
00221 protected:
00222
00223 ros::NodeHandle node_handle_;
00224 std::map<std::string, ActionBasedControllerHandleBasePtr> controllers_;
00225 };
00226
00227 }
00228
00229 PLUGINLIB_EXPORT_CLASS(moveit_simple_controller_manager::MoveItSimpleControllerManager,
00230 moveit_controller_manager::MoveItControllerManager);