omni_ethercat.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ingo Kresse <kresse@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 #include <unistd.h>
00020 #include <sys/time.h>
00021 #include <math.h>
00022 
00023 #include <ros/ros.h>
00024 #include <geometry_msgs/Twist.h>
00025 #include <std_msgs/Bool.h>
00026 #include <tf/transform_broadcaster.h>
00027 #include <diagnostic_updater/diagnostic_updater.h>
00028 #include <rosie_msgs/PowerState.h>
00029 
00030 
00031 extern "C" {
00032 #include "omnilib/omnilib.h"
00033 }
00034 
00035 #define LIMIT(x, l) ( (x>l) ? l : (x<-l) ? -l : x )
00036 
00037 class Omnidrive
00038 {
00039 private:
00040   ros::NodeHandle n_;
00041   diagnostic_updater::Updater diagnostic_;
00042   ros::Publisher power_pub_;
00043   ros::Subscriber power_sub_;
00044   ros::Time watchdog_time_;
00045   double drive_[3], drive_last_[3];
00046   bool runstop_;
00047   std::string frame_id_;
00048   std::string child_frame_id_;
00049   std::string power_name_;
00050   void cmdArrived(const geometry_msgs::Twist::ConstPtr& msg);
00051   void stateUpdate(diagnostic_updater::DiagnosticStatusWrapper &s);
00052   void powerCommand(const rosie_msgs::PowerState::ConstPtr& msg);
00053   void runstopReceiver(const std_msgs::BoolConstPtr &msg);
00054 public:
00055   Omnidrive();
00056   void main();
00057 };
00058 
00059 
00060 Omnidrive::Omnidrive() : n_("omnidrive"), diagnostic_(), runstop_(false)
00061 {
00062   diagnostic_.setHardwareID("omnidrive");
00063   diagnostic_.add("Base", this, &Omnidrive::stateUpdate);
00064   n_.param("frame_id", frame_id_, std::string("/odom"));
00065   n_.param("child_frame_id", child_frame_id_, std::string("/base_link"));
00066   n_.param("power_name", power_name_, std::string("Wheels"));
00067 
00068   power_pub_ = n_.advertise<rosie_msgs::PowerState>("/power_state", 1);
00069   power_sub_ = n_.subscribe<rosie_msgs::PowerState>("/power_command", 16, &Omnidrive::powerCommand, this);
00070 
00071 
00072   for(int i=0; i < 3; i++) {
00073     drive_last_[i] = 0;
00074     drive_[i] = 0;
00075   }
00076 
00077   watchdog_time_ = ros::Time::now();
00078 }
00079 
00080 void Omnidrive::cmdArrived(const geometry_msgs::Twist::ConstPtr& msg)
00081 {
00082   // NOTE: No need for synchronization since this is called inside spinOnce() in the main loop
00083 
00084   drive_[0] = msg->linear.x;
00085   drive_[1] = msg->linear.y;
00086   drive_[2] = msg->angular.z;
00087 
00088   if(msg->linear.z == -1) {
00089     // emergency brake!
00090     for(int i=0; i < 3; i++) {
00091       drive_last_[i] = 0;
00092       drive_[i] = 0;
00093     }
00094   }
00095 
00096   watchdog_time_ = ros::Time::now();
00097 }
00098 
00099 
00100 void Omnidrive::runstopReceiver(const std_msgs::BoolConstPtr &msg)
00101 {
00102   runstop_ = msg->data;
00103 }
00104 
00105 void Omnidrive::stateUpdate(diagnostic_updater::DiagnosticStatusWrapper &s)
00106 {
00107   int estop;
00108   char drive[4];
00109   omnidrive_status(&drive[0], &drive[1], &drive[2], &drive[3], &estop);
00110 
00111   bool operational = (drive[0] == '4' &&
00112                       drive[1] == '4' &&
00113                       drive[2] == '4' &&
00114                       drive[3] == '4' &&
00115                       estop == 0);
00116 
00117   if(operational)
00118     s.summary(0, "Operational");
00119   else
00120     s.summary(1, "Down");
00121   
00122   for(int i=0; i < 4; i++)
00123     s.add(std::string("status drive ")+(char) ('1' + i),
00124           std::string("")+drive[i]);
00125   s.add("Emergency Stop", (estop) ? "== pressed ==" : "released");
00126 
00127   commstatus_t comm = omnidrive_commstatus();
00128 
00129   for(int i=0; i < 4; i++)
00130     s.addf(std::string("comm status drive ")+(char) ('1' + i),
00131 //    s.addf("cstatus drive",
00132            "0x%02X, %s, %s operational",
00133              comm.slave_state[i],
00134              comm.slave_online[i] ? "online" : "offline",
00135              comm.slave_operational[i] ? "" : "not");
00136 
00137   s.addf("master state", "Link is %s, %d slaves, AL states: 0x%02X",
00138            comm.master_link ? "up" : "down",
00139            comm.master_slaves_responding,
00140            comm.master_al_states);
00141 
00142   s.addf("working counter", "%d, %s",
00143            comm.working_counter,
00144            comm.working_counter_state == 2 ? "complete" : "incomplete");
00145 
00146   rosie_msgs::PowerState power;
00147   power.name = power_name_;
00148   power.enabled = operational;
00149 
00150   power_pub_.publish(power);
00151 }
00152 
00153 void Omnidrive::powerCommand(const rosie_msgs::PowerState::ConstPtr& msg)
00154 {
00155   if(msg->name == power_name_)
00156   {
00157     int estop;
00158     char drive[4];
00159     omnidrive_status(&drive[0], &drive[1], &drive[2], &drive[3], &estop);
00160 
00161     bool power_state = (drive[0] == '4' &&
00162                         drive[1] == '4' &&
00163                         drive[2] == '4' &&
00164                         drive[3] == '4' &&
00165                         estop == 0);
00166 
00167     if(msg->enabled == true && power_state == false)
00168     {
00169       omnidrive_recover();
00170       omnidrive_poweron();
00171     }
00172 
00173     if(msg->enabled == false && power_state == true)
00174     {
00175       omnidrive_poweroff();
00176     }
00177     
00178   }
00179 }
00180 
00181 void Omnidrive::main()
00182 {
00183   double speed, acc_max, t, radius, drift;
00184   int tf_frequency, runstop_frequency;
00185   const int loop_frequency = 250; // 250Hz update frequency
00186 
00187   n_.param("speed", speed, 0.1);
00188   // default acc: brake from max. speed to 0 within 1.5cm
00189   n_.param("acceleration", acc_max, 0.5*speed*speed/0.015);
00190   // radius of the robot
00191   n_.param("radius", radius, 0.6);
00192   n_.param("tf_frequency", tf_frequency, 50);
00193   n_.param("runstop_frequency", runstop_frequency, 10);
00194   n_.param("watchdog_period", t, 0.5);
00195   ros::Duration watchdog_period(t);
00196   n_.param("odometry_correction", drift, 1.0);
00197 
00198   // set acceleration to correct scale
00199   acc_max /= loop_frequency;
00200 
00201   if(omnidrive_init() != 0) {
00202     ROS_ERROR("failed to initialize omnidrive");
00203     ROS_ERROR("check dmesg and try \"sudo /etc/init.d/ethercat restart\"");
00204     return;
00205   }
00206   omnidrive_set_correction(drift);
00207 
00208   tf::TransformBroadcaster transforms;
00209 
00210   ros::Subscriber sub = n_.subscribe("/cmd_vel", 10, &Omnidrive::cmdArrived, this);
00211   ros::Subscriber runstop_sub = n_.subscribe("/soft_runstop", 1, &Omnidrive::runstopReceiver, this);
00212   ros::Publisher hard_runstop_pub = n_.advertise<std_msgs::Bool>("/hard_runstop", 1);
00213 
00214   double x=0, y=0, a=0;
00215 
00216   int tf_publish_counter=0;
00217   int tf_send_rate = loop_frequency / tf_frequency;
00218 
00219   int runstop_publish_counter=0;
00220   int runstop_send_rate = loop_frequency / runstop_frequency;
00221 
00222   ros::Rate r(loop_frequency);
00223 
00224   while(n_.ok()) {
00225 
00226     omnidrive_odometry(&x, &y, &a);
00227 
00228     if(ros::Time::now() - watchdog_time_ > watchdog_period || runstop_) {
00229       // emergency brake!
00230       omnidrive_drive(0, 0, 0);
00231       
00232       if((drive_[0] != 0 || drive_[1] != 0 || drive_[2] !=0)
00233          && !runstop_)
00234         ROS_WARN("engaged watchdog!");
00235 
00236       for(int i=0; i < 3; i++) {
00237         drive_last_[i] = 0;
00238         drive_[i] = 0;
00239       }
00240 
00241       watchdog_time_ = ros::Time::now();
00242     }
00243 
00244     for(int i=0; i < 3; i++) {
00245       // acceleration limiting
00246       double acc = drive_[i] - drive_last_[i];
00247       double fac_rot = (i == 2) ? 1.0/radius : 1.0;
00248       acc = LIMIT(acc, acc_max*fac_rot*fac_rot);
00249       drive_[i] = drive_last_[i] + acc;
00250 
00251       // velocity limiting
00252       drive_[i] = LIMIT(drive_[i], speed*fac_rot);
00253 
00254       drive_last_[i] = drive_[i];
00255     }
00256 
00257     omnidrive_drive(drive_[0], drive_[1], drive_[2]);
00258 
00259     // publish odometry readings
00260     if(++tf_publish_counter == tf_send_rate) {
00261       tf::Quaternion q;
00262       q.setRPY(0, 0, a);
00263       tf::Transform pose(q, tf::Point(x, y, 0.0));
00264       transforms.sendTransform(tf::StampedTransform(pose, ros::Time::now(), frame_id_, child_frame_id_));
00265       tf_publish_counter = 0;
00266     }
00267 
00268     // publish hard runstop state
00269     if(++runstop_publish_counter == runstop_send_rate) {
00270       int runstop=0;
00271       omnidrive_status(0,0,0,0,&runstop);
00272       std_msgs::Bool msg;
00273       msg.data = (runstop != 0);
00274       hard_runstop_pub.publish(msg);
00275       runstop_publish_counter = 0;
00276     }
00277 
00278     // process incoming messages
00279     ros::spinOnce();
00280 
00281     diagnostic_.update();
00282     r.sleep();
00283   }
00284 
00285   omnidrive_shutdown();
00286 }
00287 
00288 
00289 int main(int argc, char *argv[])
00290 {
00291   ros::init(argc, argv, "omni_ethercat");
00292 
00293   Omnidrive drive;
00294   drive.main();
00295 
00296   return 0;
00297 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


omni_ethercat
Author(s): Ingo Kresse
autogenerated on Thu May 23 2013 12:56:36