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
00027
00028
00029
00030
00031 #include <ros/ros.h>
00032 #include <std_msgs/Float64.h>
00033 #include <std_srvs/Empty.h>
00034
00035
00036 #include <controller_manager/controller_manager.h>
00037 #include <hardware_interface/joint_command_interface.h>
00038 #include <hardware_interface/joint_state_interface.h>
00039 #include <hardware_interface/robot_hw.h>
00040 #include <realtime_tools/realtime_buffer.h>
00041
00042
00043 #include <limits>
00044
00045
00046 #include <sstream>
00047
00048 enum INDEX_WHEEL {
00049 INDEX_RIGHT = 0,
00050 INDEX_LEFT = 1
00051 };
00052
00053 class Steerbot : public hardware_interface::RobotHW
00054 {
00055 public:
00056 Steerbot()
00057 : running_(true)
00058 , start_srv_(nh_.advertiseService("start", &Steerbot::startCallback, this))
00059 , stop_srv_(nh_.advertiseService("stop", &Steerbot::stopCallback, this))
00060 , ns_("steerbot_hw_sim/")
00061 {
00062
00063 this->cleanUp();
00064 this->getJointNames(nh_);
00065 this->registerHardwareInterfaces();
00066
00067 nh_.getParam(ns_ + "wheel_separation_w", wheel_separation_w_);
00068 nh_.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
00069 ROS_INFO_STREAM("wheel_separation_w in test steerbot= " << wheel_separation_w_);
00070 ROS_INFO_STREAM("wheel_separation_h in test steerbot= " << wheel_separation_h_);
00071 }
00072
00073 ros::Time getTime() const {return ros::Time::now();}
00074 ros::Duration getPeriod() const {return ros::Duration(0.01);}
00075
00076 void read()
00077 {
00078 std::ostringstream os;
00079
00080 os << rear_wheel_jnt_vel_cmd_ << ", ";
00081 os << front_steer_jnt_pos_cmd_ << ", ";
00082
00083
00084
00085 for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
00086 {
00087 virtual_rear_wheel_jnt_vel_cmd_[i] = rear_wheel_jnt_vel_cmd_;
00088 os << virtual_rear_wheel_jnt_vel_cmd_[i] << ", ";
00089 }
00090
00091
00092 const double h = wheel_separation_h_;
00093 const double w = wheel_separation_w_;
00094 virtual_front_steer_jnt_pos_cmd_[INDEX_RIGHT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h + w/2.0*tan(front_steer_jnt_pos_cmd_));
00095 virtual_front_steer_jnt_pos_cmd_[INDEX_LEFT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h - w/2.0*tan(front_steer_jnt_pos_cmd_));
00096
00097 for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
00098 {
00099 os << virtual_front_steer_jnt_pos_cmd_[i] << ", ";
00100 }
00101
00102 if (rear_wheel_jnt_vel_cmd_ != 0.0 || front_steer_jnt_pos_cmd_ != 0.0)
00103 ROS_INFO_STREAM("Commands for joints: " << os.str());
00104
00105 }
00106
00107 void write()
00108 {
00109 std::ostringstream os;
00110 if (running_)
00111 {
00112
00113 rear_wheel_jnt_pos_ += rear_wheel_jnt_vel_*getPeriod().toSec();
00114 rear_wheel_jnt_vel_ = rear_wheel_jnt_vel_cmd_;
00115 for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
00116 {
00117
00118
00119
00120 virtual_rear_wheel_jnt_pos_[i] += virtual_rear_wheel_jnt_vel_[i]*getPeriod().toSec();
00121 virtual_rear_wheel_jnt_vel_[i] = virtual_rear_wheel_jnt_vel_cmd_[i];
00122 }
00123
00124
00125 front_steer_jnt_pos_ = front_steer_jnt_pos_cmd_;
00126 for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
00127 {
00128 virtual_front_steer_jnt_pos_[i] = virtual_front_steer_jnt_pos_cmd_[i];
00129 }
00130
00131
00132 os << rear_wheel_jnt_vel_cmd_ << ", ";
00133 os << front_steer_jnt_pos_cmd_ << ", ";
00134
00135
00136
00137 for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
00138 {
00139 os << virtual_rear_wheel_jnt_pos_[i] << ", ";
00140 }
00141
00142
00143 for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
00144 {
00145 os << virtual_front_steer_jnt_pos_[i] << ", ";
00146 }
00147 }
00148 else
00149 {
00150
00151 rear_wheel_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
00152 rear_wheel_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
00153 std::fill_n(virtual_rear_wheel_jnt_pos_.begin(), virtual_rear_wheel_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
00154 std::fill_n(virtual_rear_wheel_jnt_vel_.begin(), virtual_rear_wheel_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
00155
00156 front_steer_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
00157 front_steer_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
00158 std::fill_n(virtual_front_steer_jnt_pos_.begin(), virtual_front_steer_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
00159 std::fill_n(virtual_front_steer_jnt_vel_.begin(), virtual_front_steer_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
00160
00161
00162 os << rear_wheel_jnt_pos_ << ", ";
00163 os << rear_wheel_jnt_vel_ << ", ";
00164 for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
00165 {
00166 os << virtual_rear_wheel_jnt_pos_[i] << ", ";
00167 }
00168
00169
00170 os << front_steer_jnt_pos_ << ", ";
00171 os << front_steer_jnt_vel_ << ", ";
00172 for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
00173 {
00174 os << virtual_front_steer_jnt_pos_[i] << ", ";
00175 }
00176 }
00177 ROS_INFO_STREAM("running_ = " << running_ << ". commands are " << os.str());
00178 }
00179
00180 bool startCallback(std_srvs::Empty::Request& , std_srvs::Empty::Response& )
00181 {
00182 ROS_INFO_STREAM("running_ = " << running_ << ".");
00183 running_ = true;
00184 return true;
00185 }
00186
00187 bool stopCallback(std_srvs::Empty::Request& , std_srvs::Empty::Response& )
00188 {
00189 ROS_INFO_STREAM("running_ = " << running_ << ".");
00190 running_ = false;
00191 return true;
00192 }
00193
00194 private:
00195
00196 void cleanUp()
00197 {
00198
00199
00200 rear_wheel_jnt_name_.empty();
00201 virtual_rear_wheel_jnt_names_.clear();
00202
00203 rear_wheel_jnt_pos_ = 0;
00204 rear_wheel_jnt_vel_ = 0;
00205 rear_wheel_jnt_eff_ = 0;
00206 rear_wheel_jnt_vel_cmd_ = 0;
00207
00208 virtual_rear_wheel_jnt_pos_.clear();
00209 virtual_rear_wheel_jnt_vel_.clear();
00210 virtual_rear_wheel_jnt_eff_.clear();
00211 virtual_rear_wheel_jnt_vel_cmd_.clear();
00212
00213 virtual_front_wheel_jnt_pos_.clear();
00214 virtual_front_wheel_jnt_vel_.clear();
00215 virtual_front_wheel_jnt_eff_.clear();
00216 virtual_front_wheel_jnt_vel_cmd_.clear();
00217
00218
00219
00220 front_steer_jnt_name_.empty();
00221 virtual_front_steer_jnt_names_.clear();
00222
00223 front_steer_jnt_pos_ = 0;
00224 front_steer_jnt_vel_ = 0;
00225 front_steer_jnt_eff_ = 0;
00226 front_steer_jnt_pos_cmd_ = 0;
00227
00228 virtual_front_steer_jnt_pos_.clear();
00229 virtual_front_steer_jnt_vel_.clear();
00230 virtual_front_steer_jnt_eff_.clear();
00231 virtual_front_steer_jnt_pos_cmd_.clear();
00232 }
00233
00234 void getJointNames(ros::NodeHandle &_nh)
00235 {
00236 this->getWheelJointNames(_nh);
00237 this->getSteerJointNames(_nh);
00238 }
00239
00240 void getWheelJointNames(ros::NodeHandle &_nh)
00241 {
00242
00243 _nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_);
00244
00245
00246 _nh.getParam(ns_ + "rear_wheels", virtual_rear_wheel_jnt_names_);
00247 int dof = virtual_rear_wheel_jnt_names_.size();
00248 virtual_rear_wheel_jnt_pos_.resize(dof);
00249 virtual_rear_wheel_jnt_vel_.resize(dof);
00250 virtual_rear_wheel_jnt_eff_.resize(dof);
00251 virtual_rear_wheel_jnt_vel_cmd_.resize(dof);
00252
00253 _nh.getParam(ns_ + "front_wheels", virtual_front_wheel_jnt_names_);
00254 dof = virtual_front_wheel_jnt_names_.size();
00255 virtual_front_wheel_jnt_pos_.resize(dof);
00256 virtual_front_wheel_jnt_vel_.resize(dof);
00257 virtual_front_wheel_jnt_eff_.resize(dof);
00258 virtual_front_wheel_jnt_vel_cmd_.resize(dof);
00259 }
00260
00261 void getSteerJointNames(ros::NodeHandle &_nh)
00262 {
00263
00264 _nh.getParam(ns_ + "front_steer", front_steer_jnt_name_);
00265
00266
00267 _nh.getParam(ns_ + "front_steers", virtual_front_steer_jnt_names_);
00268
00269 const int dof = virtual_front_steer_jnt_names_.size();
00270 virtual_front_steer_jnt_pos_.resize(dof);
00271 virtual_front_steer_jnt_vel_.resize(dof);
00272 virtual_front_steer_jnt_eff_.resize(dof);
00273 virtual_front_steer_jnt_pos_cmd_.resize(dof);
00274 }
00275
00276 void registerHardwareInterfaces()
00277 {
00278 this->registerSteerInterface();
00279 this->registerWheelInterface();
00280
00281
00282 registerInterface(&jnt_state_interface_);
00283 registerInterface(&rear_wheel_jnt_vel_cmd_interface_);
00284 registerInterface(&front_steer_jnt_pos_cmd_interface_);
00285 }
00286
00287 void registerWheelInterface()
00288 {
00289
00290 this->registerInterfaceHandles(
00291 jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
00292 rear_wheel_jnt_name_, rear_wheel_jnt_pos_, rear_wheel_jnt_vel_, rear_wheel_jnt_eff_, rear_wheel_jnt_vel_cmd_);
00293
00294
00295 for (int i = 0; i < virtual_rear_wheel_jnt_names_.size(); i++)
00296 {
00297 this->registerInterfaceHandles(
00298 jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
00299 virtual_rear_wheel_jnt_names_[i], virtual_rear_wheel_jnt_pos_[i], virtual_rear_wheel_jnt_vel_[i], virtual_rear_wheel_jnt_eff_[i], virtual_rear_wheel_jnt_vel_cmd_[i]);
00300 }
00301
00302 for (int i = 0; i < virtual_front_wheel_jnt_names_.size(); i++)
00303 {
00304 this->registerInterfaceHandles(
00305 jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
00306 virtual_front_wheel_jnt_names_[i], virtual_front_wheel_jnt_pos_[i], virtual_front_wheel_jnt_vel_[i], virtual_front_wheel_jnt_eff_[i], virtual_front_wheel_jnt_vel_cmd_[i]);
00307 }
00308 }
00309
00310 void registerSteerInterface()
00311 {
00312
00313 this->registerInterfaceHandles(
00314 jnt_state_interface_, front_steer_jnt_pos_cmd_interface_,
00315 front_steer_jnt_name_, front_steer_jnt_pos_, front_steer_jnt_vel_, front_steer_jnt_eff_, front_steer_jnt_pos_cmd_);
00316
00317
00318 for (int i = 0; i < virtual_front_steer_jnt_names_.size(); i++)
00319 {
00320 this->registerInterfaceHandles(
00321 jnt_state_interface_, front_steer_jnt_pos_cmd_interface_,
00322 virtual_front_steer_jnt_names_[i], virtual_front_steer_jnt_pos_[i], virtual_front_steer_jnt_vel_[i], virtual_front_steer_jnt_eff_[i], virtual_front_steer_jnt_pos_cmd_[i]);
00323 }
00324 }
00325
00326 void registerInterfaceHandles(
00327 hardware_interface::JointStateInterface& _jnt_state_interface,
00328 hardware_interface::JointCommandInterface& _jnt_cmd_interface,
00329 const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff, double& _jnt_cmd)
00330 {
00331
00332 this->registerJointStateInterfaceHandle(_jnt_state_interface, _jnt_name,
00333 _jnt_pos, _jnt_vel, _jnt_eff);
00334 this->registerCommandJointInterfaceHandle(_jnt_state_interface, _jnt_cmd_interface,
00335 _jnt_name, _jnt_cmd);
00336 }
00337
00338 void registerJointStateInterfaceHandle(
00339 hardware_interface::JointStateInterface& _jnt_state_interface,
00340 const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff)
00341 {
00342 hardware_interface::JointStateHandle state_handle(_jnt_name,
00343 &_jnt_pos,
00344 &_jnt_vel,
00345 &_jnt_eff);
00346 _jnt_state_interface.registerHandle(state_handle);
00347
00348 ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the JointStateInterface");
00349 }
00350
00351 void registerCommandJointInterfaceHandle(
00352 hardware_interface::JointStateInterface& _jnt_state_interface,
00353 hardware_interface::JointCommandInterface& _jnt_cmd_interface,
00354 const std::string _jnt_name, double& _jnt_cmd)
00355 {
00356
00357 hardware_interface::JointHandle _handle(_jnt_state_interface.getHandle(_jnt_name),
00358 &_jnt_cmd);
00359 _jnt_cmd_interface.registerHandle(_handle);
00360
00361 ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the CommandJointInterface");
00362 }
00363
00364 private:
00365
00366 hardware_interface::JointStateInterface jnt_state_interface_;
00367
00368
00369 std::string rear_wheel_jnt_name_;
00370
00371 double rear_wheel_jnt_pos_;
00372 double rear_wheel_jnt_vel_;
00373 double rear_wheel_jnt_eff_;
00374
00375 double rear_wheel_jnt_vel_cmd_;
00376
00377 hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_;
00378
00379
00380
00381
00382 std::vector<std::string> virtual_rear_wheel_jnt_names_;
00383
00384 std::vector<double> virtual_rear_wheel_jnt_pos_;
00385 std::vector<double> virtual_rear_wheel_jnt_vel_;
00386 std::vector<double> virtual_rear_wheel_jnt_eff_;
00387
00388 std::vector<double> virtual_rear_wheel_jnt_vel_cmd_;
00389
00390
00391 std::vector<std::string> virtual_front_wheel_jnt_names_;
00392
00393 std::vector<double> virtual_front_wheel_jnt_pos_;
00394 std::vector<double> virtual_front_wheel_jnt_vel_;
00395 std::vector<double> virtual_front_wheel_jnt_eff_;
00396
00397 std::vector<double> virtual_front_wheel_jnt_vel_cmd_;
00398
00399
00400
00401
00402 std::string front_steer_jnt_name_;
00403
00404 double front_steer_jnt_pos_;
00405 double front_steer_jnt_vel_;
00406 double front_steer_jnt_eff_;
00407
00408 double front_steer_jnt_pos_cmd_;
00409
00410 hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_;
00411
00412
00413
00414
00415 std::vector<std::string> virtual_front_steer_jnt_names_;
00416
00417 std::vector<double> virtual_front_steer_jnt_pos_;
00418 std::vector<double> virtual_front_steer_jnt_vel_;
00419 std::vector<double> virtual_front_steer_jnt_eff_;
00420
00421 std::vector<double> virtual_front_steer_jnt_pos_cmd_;
00422
00423
00424 double wheel_separation_w_;
00425
00426 double wheel_separation_h_;
00427
00428 std::string ns_;
00429 bool running_;
00430
00431 ros::NodeHandle nh_;
00432 ros::ServiceServer start_srv_;
00433 ros::ServiceServer stop_srv_;
00434 };