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 #include <string>
00019 #include <vector>
00020 #include <ros/ros.h>
00021 #include <std_msgs/Float64.h>
00022 #include <std_msgs/Float64MultiArray.h>
00023 #include <controller_manager_msgs/LoadController.h>
00024 #include <controller_manager_msgs/SwitchController.h>
00025 #include <boost/thread/mutex.hpp>
00026
00027 class CobControlModeAdapter
00028 {
00029 public:
00030 bool initialize()
00031 {
00032 bool success = false;
00033
00034 joint_names_.clear();
00035 current_controller_names_.clear();
00036 traj_controller_names_.clear();
00037 pos_controller_names_.clear();
00038 vel_controller_names_.clear();
00039 last_pos_command_ = ros::Time();
00040 last_vel_command_ = ros::Time();
00041
00042 has_traj_controller_ = false;
00043 has_pos_controller_ = false;
00044 has_vel_controller_ = false;
00045
00046 current_control_mode_ = NONE;
00047
00048
00049 while (!ros::service::waitForService("controller_manager/load_controller", ros::Duration(5.0))) {;}
00050
00051 if (ros::service::exists("controller_manager/load_controller", false))
00052 {
00053 load_client_ = nh_.serviceClient<controller_manager_msgs::LoadController>("controller_manager/load_controller");
00054 }
00055 else
00056 {
00057 ROS_ERROR("...Load service not available!");
00058 return false;
00059 }
00060
00061 while (!ros::service::waitForService("controller_manager/switch_controller", ros::Duration(5.0))) {;}
00062
00063 if (ros::service::exists("controller_manager/switch_controller", false))
00064 {
00065 switch_client_ = nh_.serviceClient<controller_manager_msgs::SwitchController>("controller_manager/switch_controller");
00066 }
00067 else
00068 {
00069 ROS_ERROR("...Load service not available!");
00070 return false;
00071 }
00072
00073 std::string param = "max_command_silence";
00074 if (nh_.hasParam(param))
00075 {
00076 nh_.getParam(param, max_command_silence_);
00077 }
00078 else
00079 {
00080 ROS_ERROR("Parameter %s not set, using default 0.3s...", param.c_str());
00081 max_command_silence_ = 0.3;
00082 }
00083
00084
00085 param = "joint_names";
00086 if (!nh_.getParam(param, joint_names_))
00087 {
00088 ROS_ERROR("Parameter %s not set, shutting down node...", param.c_str());
00089 nh_.shutdown();
00090 }
00091
00092 std::string joint_trajectory_controller_name = nh_.param<std::string>("joint_trajectory_controller_name", "joint_trajectory_controller");
00093
00094 if (nh_.hasParam(joint_trajectory_controller_name))
00095 {
00096 has_traj_controller_ = true;
00097 traj_controller_names_.push_back(joint_trajectory_controller_name);
00098 ROS_DEBUG_STREAM(nh_.getNamespace() << " supports '" << joint_trajectory_controller_name << "'");
00099 }
00100
00101 std::string joint_group_position_controller_name = nh_.param<std::string>("joint_group_position_controller_name", "joint_group_position_controller");
00102
00103 if (nh_.hasParam(joint_group_position_controller_name))
00104 {
00105 has_pos_controller_ = true;
00106 pos_controller_names_.push_back(joint_group_position_controller_name);
00107 ROS_DEBUG_STREAM(nh_.getNamespace() << " supports '" << joint_group_position_controller_name << "'");
00108 }
00109
00110
00111 std::string joint_group_velocity_controller_name = nh_.param<std::string>("joint_group_velocity_controller_name", "joint_group_velocity_controller");
00112
00113 if (nh_.hasParam(joint_group_velocity_controller_name))
00114 {
00115 has_vel_controller_ = true;
00116 vel_controller_names_.push_back(joint_group_velocity_controller_name);
00117 ROS_DEBUG_STREAM(nh_.getNamespace() << " supports '" << joint_group_velocity_controller_name << "'");
00118 }
00119
00120
00121 if (has_traj_controller_)
00122 {
00123 for (unsigned int i = 0; i < traj_controller_names_.size(); i++)
00124 {
00125 success = loadController(traj_controller_names_[i]);
00126 }
00127 }
00128 if (has_pos_controller_)
00129 {
00130 for (unsigned int i = 0; i < pos_controller_names_.size(); i++)
00131 {
00132 success = loadController(pos_controller_names_[i]);
00133 }
00134 cmd_pos_sub_ = nh_.subscribe(joint_group_position_controller_name+"/command", 1, &CobControlModeAdapter::cmd_pos_cb, this);
00135 }
00136 if (has_vel_controller_)
00137 {
00138 for (unsigned int i = 0; i < vel_controller_names_.size(); i++)
00139 {
00140 success = loadController(vel_controller_names_[i]);
00141 }
00142 cmd_vel_sub_ = nh_.subscribe(joint_group_velocity_controller_name+"/command", 1, &CobControlModeAdapter::cmd_vel_cb, this);
00143 }
00144
00145
00146 if (has_traj_controller_)
00147 {
00148 success = switchController(traj_controller_names_, current_controller_names_);
00149 current_control_mode_ = TRAJECTORY;
00150 }
00151
00152 update_rate_ = 100;
00153 timer_ = nh_.createTimer(ros::Duration(1/update_rate_), &CobControlModeAdapter::update, this);
00154
00155 return true;
00156 }
00157
00158 bool loadController(std::string load_controller)
00159 {
00160 controller_manager_msgs::LoadController load_srv;
00161 load_srv.request.name = load_controller;
00162 if (load_client_.call(load_srv))
00163 {
00164 if (load_srv.response.ok)
00165 {
00166 ROS_INFO("%s loaded", load_controller.c_str());
00167 return true;
00168 }
00169 else
00170 {
00171 ROS_ERROR("Could not load controllers");
00172 }
00173 }
00174 else
00175 {
00176 ROS_ERROR("ServiceCall failed: load_controller");
00177 }
00178
00179 return false;
00180 }
00181
00182
00183 bool switchController(std::vector< std::string > start_controllers, std::vector< std::string > stop_controllers)
00184 {
00185 controller_manager_msgs::SwitchController switch_srv;
00186 switch_srv.request.start_controllers = start_controllers;
00187 switch_srv.request.stop_controllers = stop_controllers;
00188 switch_srv.request.strictness = 2;
00189
00190 if (switch_client_.call(switch_srv))
00191 {
00192 if (switch_srv.response.ok)
00193 {
00194 std::string str_start;
00195 std::string str_stop;
00196
00197 if (start_controllers.empty())
00198 {
00199 str_start = "no_start_controller_defined";
00200 }
00201 else
00202 {
00203 str_start = start_controllers.back();
00204 }
00205 if (stop_controllers.empty())
00206 {
00207 str_stop = "no_stop_controller_defined";
00208 }
00209 else
00210 {
00211 str_stop = stop_controllers.back();
00212 }
00213
00214 ROS_INFO("Switched Controllers. From %s to %s", str_stop.c_str(), str_start.c_str());
00215
00216 current_controller_names_ = start_controllers;
00217 return true;
00218 }
00219 else
00220 {
00221 ROS_ERROR("Could not switch controllers");
00222 }
00223 }
00224 else
00225 {
00226 ROS_ERROR("ServiceCall failed: switch_controller");
00227 }
00228
00229 return false;
00230 }
00231
00232 void cmd_pos_cb(const std_msgs::Float64MultiArray::ConstPtr& msg)
00233 {
00234 boost::mutex::scoped_lock lock(mutex_);
00235 last_pos_command_ = ros::Time::now();
00236 }
00237
00238 void cmd_vel_cb(const std_msgs::Float64MultiArray::ConstPtr& msg)
00239 {
00240 boost::mutex::scoped_lock lock(mutex_);
00241 last_vel_command_ = ros::Time::now();
00242 }
00243
00244 void update(const ros::TimerEvent& event)
00245 {
00246 if (!nh_.ok()) {return;}
00247
00248 boost::mutex::scoped_lock lock(mutex_);
00249
00250 ros::Duration period_vel = event.current_real - last_vel_command_;
00251 ros::Duration period_pos = event.current_real - last_pos_command_;
00252
00253 lock.unlock();
00254
00255 if (has_vel_controller_ && (period_vel.toSec() < max_command_silence_))
00256 {
00257 if (current_control_mode_ != VELOCITY)
00258 {
00259 bool success = switchController(vel_controller_names_, current_controller_names_);
00260 if (!success)
00261 {
00262 ROS_ERROR("Unable to switch to velocity_controllers. Not executing command...");
00263 }
00264 else
00265 {
00266 ROS_INFO("Successfully switched to velocity_controllers");
00267 current_control_mode_ = VELOCITY;
00268 }
00269 }
00270 }
00271 else if (has_pos_controller_ && (period_pos.toSec() < max_command_silence_))
00272 {
00273 if (current_control_mode_ != POSITION)
00274 {
00275 bool success = switchController(pos_controller_names_, current_controller_names_);
00276 if (!success)
00277 {
00278 ROS_ERROR("Unable to switch to position_controllers. Not executing command...");
00279 }
00280 else
00281 {
00282 ROS_INFO("Successfully switched to position_controllers");
00283 current_control_mode_ = POSITION;
00284 }
00285 }
00286 }
00287 else if (has_traj_controller_)
00288 {
00289 if (current_control_mode_ != TRAJECTORY)
00290 {
00291 if (current_control_mode_ == POSITION)
00292 {
00293 ROS_INFO("Have not heard a pos command for %f seconds, switched back to trajectory_controller", period_pos.toSec());
00294 }
00295 else
00296 {
00297 ROS_INFO("Have not heard a vel command for %f seconds, switched back to trajectory_controller", period_vel.toSec());
00298 }
00299 bool success = switchController(traj_controller_names_, current_controller_names_);
00300
00301 if (success)
00302 {
00303 current_control_mode_ = TRAJECTORY;
00304 }
00305 else
00306 {
00307 ROS_ERROR("Unable to switch to trajectory_controller. Not executing command...");
00308 }
00309 }
00310 }
00311 else
00312 {
00313 ROS_ERROR_STREAM(nh_.getNamespace() << " does not support 'joint_trajectory_controller' and no other controller was started yet");
00314 }
00315 }
00316
00317 private:
00318 ros::NodeHandle nh_;
00319
00320 ros::Timer timer_;
00321 double update_rate_;
00322
00323 std::vector< std::string > joint_names_;
00324
00325 enum
00326 {
00327 NONE, VELOCITY, POSITION, TRAJECTORY
00328 } current_control_mode_;
00329
00330 std::vector< std::string > current_controller_names_;
00331 std::vector< std::string > traj_controller_names_;
00332 std::vector< std::string > pos_controller_names_;
00333 std::vector< std::string > vel_controller_names_;
00334
00335 bool has_traj_controller_;
00336 bool has_pos_controller_;
00337 bool has_vel_controller_;
00338
00339 ros::Subscriber cmd_pos_sub_;
00340 ros::Subscriber cmd_vel_sub_;
00341
00342 ros::ServiceClient load_client_;
00343 ros::ServiceClient switch_client_;
00344
00345 double max_command_silence_;
00346 ros::Time last_pos_command_;
00347 ros::Time last_vel_command_;
00348 boost::mutex mutex_;
00349 };
00350
00351
00352 int main(int argc, char** argv)
00353 {
00354 ros::init(argc, argv, "cob_control_mode_adapter_node");
00355 ros::AsyncSpinner spinner(0);
00356 spinner.start();
00357
00358 CobControlModeAdapter* ccma = new CobControlModeAdapter();
00359
00360 if (!ccma->initialize())
00361 {
00362 ROS_ERROR("Failed to initialize CobControlModeAdapter");
00363 return -1;
00364 }
00365
00366 ros::waitForShutdown();
00367 return 0;
00368 }
00369