steerbot.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics, Inc. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00028 // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
00029 
00030 // ROS
00031 #include <ros/ros.h>
00032 #include <std_msgs/Float64.h>
00033 #include <std_srvs/Empty.h>
00034 
00035 // ros_control
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 // NaN
00043 #include <limits>
00044 
00045 // ostringstream
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     // Intialize raw data
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     // directly get from controller
00080     os << rear_wheel_jnt_vel_cmd_ << ", ";
00081     os << front_steer_jnt_pos_cmd_ << ", ";
00082 
00083     // convert to each joint velocity
00084     //-- differential drive
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     //-- ackerman link
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       // wheels
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         // Note that pos_[i] will be NaN for one more cycle after we start(),
00118         // but that is consistent with the knowledge we have about the state
00119         // of the robot.
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       // steers
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       // directly get from controller
00132       os << rear_wheel_jnt_vel_cmd_ << ", ";
00133       os << front_steer_jnt_pos_cmd_ << ", ";
00134 
00135       // convert to each joint velocity
00136       //-- differential drive
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       //-- ackerman link
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       // wheels
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       // steers
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       // wheels
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       // steers
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& /*req*/, std_srvs::Empty::Response& /*res*/)
00181   {
00182     ROS_INFO_STREAM("running_ = " << running_ << ".");
00183     running_ = true;
00184     return true;
00185   }
00186 
00187   bool stopCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
00188   {
00189     ROS_INFO_STREAM("running_ = " << running_ << ".");
00190     running_ = false;
00191     return true;
00192   }
00193 
00194 private:
00195   
00196   void cleanUp()
00197   {
00198     // wheel
00199     //-- wheel joint names
00200     rear_wheel_jnt_name_.empty();
00201     virtual_rear_wheel_jnt_names_.clear();
00202     //-- actual rear wheel joint
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     //-- virtual rear wheel joint
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     //-- virtual front wheel joint
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     // steer
00219     //-- steer joint names
00220     front_steer_jnt_name_.empty();
00221     virtual_front_steer_jnt_names_.clear();
00222     //-- front steer joint
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     //-- virtual wheel joint
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     // wheel joint to get linear command
00243     _nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_);
00244 
00245     // virtual wheel joint for gazebo con_rol
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     // steer joint to get angular command
00264     _nh.getParam(ns_ + "front_steer", front_steer_jnt_name_);
00265 
00266     // virtual steer joint for gazebo control
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     // register mapped interface to ros_control
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     // actual wheel joints
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     // virtual rear wheel joints
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     // virtual front wheel joints
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     // actual steer joints
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     // virtual steer joints
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     // register joint (both JointState and CommandJoint)
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     // joint command
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   // common
00366   hardware_interface::JointStateInterface jnt_state_interface_;// rear wheel
00367   //-- actual joint(single actuator)
00368   //---- joint name
00369   std::string rear_wheel_jnt_name_;
00370   //---- joint interface parameters
00371   double rear_wheel_jnt_pos_;
00372   double rear_wheel_jnt_vel_;
00373   double rear_wheel_jnt_eff_;
00374   //---- joint interface command
00375   double rear_wheel_jnt_vel_cmd_;
00376   //---- Hardware interface: joint
00377   hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_;
00378   //hardware_interface::JointStateInterface wheel_jnt_state_interface_;
00379   //
00380   //-- virtual joints(two rear wheels)
00381   //---- joint name
00382   std::vector<std::string> virtual_rear_wheel_jnt_names_;
00383   //---- joint interface parameters
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   //---- joint interface command
00388   std::vector<double> virtual_rear_wheel_jnt_vel_cmd_;
00389   //-- virtual joints(two front wheels)
00390   //---- joint name
00391   std::vector<std::string> virtual_front_wheel_jnt_names_;
00392   //---- joint interface parameters
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   //---- joint interface command
00397   std::vector<double> virtual_front_wheel_jnt_vel_cmd_;
00398 
00399   // front steer
00400   //-- actual joint(single actuator)
00401   //---- joint name
00402   std::string front_steer_jnt_name_;
00403   //---- joint interface parameters
00404   double front_steer_jnt_pos_;
00405   double front_steer_jnt_vel_;
00406   double front_steer_jnt_eff_;
00407   //---- joint interface command
00408   double front_steer_jnt_pos_cmd_;
00409   //---- Hardware interface: joint
00410   hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_;
00411   //hardware_interface::JointStateInterface steer_jnt_state_interface_;
00412   //
00413   //-- virtual joints(two steers)
00414   //---- joint name
00415   std::vector<std::string> virtual_front_steer_jnt_names_;
00416   //---- joint interface parameters
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   //---- joint interface command
00421   std::vector<double> virtual_front_steer_jnt_pos_cmd_;
00422 
00423   // Wheel separation, wrt the midpoint of the wheel width:
00424   double wheel_separation_w_;
00425   // Wheel separation, wrt the midpoint of the wheel width:
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 };


steer_drive_controller
Author(s): CIR-KIT , Masaru Morita
autogenerated on Sat Jun 8 2019 19:20:25