amtec_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 #include <boost/thread/mutex.hpp>
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <tf/transform_broadcaster.h>
00042 
00043 #include <amtec_base.h>
00044 #include <amtec_commands.h>
00045 #include <amtec_settings.h>
00046 #include <amtec_io.h>
00047 
00048 // messages
00049 #include <amtec/AmtecState.h>
00050 
00051 // services
00052 #include <amtec/GetStatus.h>
00053 #include <amtec/Halt.h>
00054 #include <amtec/Home.h>
00055 #include <amtec/Reset.h>
00056 #include <amtec/SetPosition.h>
00057 #include <amtec/SetVelocity.h>
00058 #include <amtec/TargetAcceleration.h>
00059 #include <amtec/TargetVelocity.h>
00060 #include <amtec/SweepPan.h>
00061 #include <amtec/SweepTilt.h>
00062 
00063 class AmtecNode
00064 {
00065 public:
00066   ros::NodeHandle node_;
00067   boost::mutex amtec_mutex_ ;
00068   amtec_powercube_p amtec_;
00069   amtec::AmtecState amtec_pan_state_message_;
00070   amtec::AmtecState amtec_tilt_state_message_;
00071 
00072   ros::Publisher pan_state_pub_;
00073   ros::Publisher tilt_state_pub_;
00074 
00075   ros::ServiceServer get_status_srv_;
00076   ros::ServiceServer halt_srv_;
00077   ros::ServiceServer home_srv_;
00078   ros::ServiceServer reset_srv_;
00079   ros::ServiceServer set_position_srv_;
00080   ros::ServiceServer set_velocity_srv_;
00081   ros::ServiceServer target_accel_srv_;
00082   ros::ServiceServer target_vel_srv_;
00083   ros::ServiceServer sweep_pan_srv_;
00084   ros::ServiceServer sweep_tilt_srv_;
00085 
00086   tf::TransformBroadcaster tf_;
00087   tf::Transform parent_to_amtec_;
00088   std::string parent_frame_;
00089   std::string amtec_frame_;
00090 
00091   // parameter
00092   AmtecNode() : node_(), amtec_(NULL), tf_()
00093   {
00094     std::string device, parity;
00095 
00096     // initialize amtec
00097     ROS_INFO("Initializing Amtec");
00098     amtec_ = amtecInitialize();
00099 
00100     parent_to_amtec_.setIdentity();
00101 
00102     ros::NodeHandle private_nh("~");
00103     // ***** Parameters *****
00104     private_nh.param("parent_frame", parent_frame_, std::string("head_link"));
00105     private_nh.param("amtec_frame", amtec_frame_, std::string("amtec_link"));
00106 
00107     private_nh.param("port", device, std::string("/dev/ttyUSB2"));
00108     private_nh.param("baud_rate", amtec_->dev.baud, 38400);
00109     private_nh.param("data_bits", amtec_->dev.databits, 8);
00110     private_nh.param("parity", parity, std::string("N"));
00111     private_nh.param("stop_bits", amtec_->dev.stopbits, 1);
00112     private_nh.param("hardware_flow_control", amtec_->dev.hwf, 0);
00113     private_nh.param("software_flow_control", amtec_->dev.swf, 0);
00114 
00115     private_nh.param("pan_id", amtec_->pan.id, 14);
00116     private_nh.param("pan_min_pos", amtec_->panset.MinPos, -3.5);
00117     private_nh.param("pan_max_pos", amtec_->panset.MaxPos, 3.5);
00118     private_nh.param("pan_max_vel", amtec_->panset.MaxVel, 2.0);
00119     private_nh.param("pan_max_acc", amtec_->panset.MaxAcc, 5.0);
00120     private_nh.param("pan_max_cur", amtec_->panset.MaxCur, 10.0);
00121     private_nh.param("pan_c0", amtec_->panset.C0, 12);
00122     private_nh.param("pan_damp", amtec_->panset.Damp, 4);
00123     private_nh.param("pan_a0", amtec_->panset.A0, 2);
00124 
00125     private_nh.param("tilt_id", amtec_->tilt.id, 13);
00126     private_nh.param("tilt_min_pos", amtec_->tiltset.MinPos, -1.57);
00127     private_nh.param("tilt_max_pos", amtec_->tiltset.MaxPos, 1.57);
00128     private_nh.param("tilt_max_vel", amtec_->tiltset.MaxVel, 1.0);
00129     private_nh.param("tilt_max_acc", amtec_->tiltset.MaxAcc, 2.5);
00130     private_nh.param("tilt_max_cur", amtec_->tiltset.MaxCur, 10.0);
00131     private_nh.param("tilt_use_break", amtec_->tiltset.UseBreak, 1);
00132     private_nh.param("tilt_c0", amtec_->tiltset.C0, 18);
00133     private_nh.param("tilt_damp", amtec_->tiltset.Damp, 3);
00134     private_nh.param("tilt_a0", amtec_->tiltset.A0, 1);
00135 
00136     strcpy(amtec_->dev.ttyport, device.c_str());
00137     if (parity != "") {
00138       if (parity == "N")
00139         amtec_->dev.parity = N;
00140       else if (parity == "E")
00141         amtec_->dev.parity = E;
00142       else if (parity == "O")
00143         amtec_->dev.parity = O;
00144       else {
00145         ROS_FATAL("ERROR: unknown PARITY_TYPE %s", parity.c_str());
00146         ROS_FATAL("       valid types: N, E, O");
00147         private_nh.shutdown();
00148         return;
00149       }
00150     }
00151 
00152     // connect amtec
00153     ROS_INFO("Connecting to amtec to %s", amtec_->dev.ttyport);
00154     if (amtecDeviceConnectPort(&amtec_->dev) < 0) {
00155       ROS_FATAL("Unable to connect amtec at %s", amtec_->dev.ttyport);
00156       private_nh.shutdown();
00157       return;
00158     }
00159 
00160     ROS_INFO("Resetting device");
00161     if(!amtecReset(&amtec_->dev, amtec_->pan.id)) {
00162       ROS_FATAL("Unable to connect to pan module");
00163       private_nh.shutdown();
00164       return;
00165     }
00166     if(!amtecReset(&amtec_->dev, amtec_->tilt.id)) {
00167       ROS_FATAL("Unable to connect tilt module");
00168       private_nh.shutdown();
00169       return;
00170     }
00171 
00172     ROS_INFO("Retrieving module state");
00173     unsigned int pan_serial = amtecGetDefCubeSerial(&amtec_->dev, amtec_->pan.id);
00174     unsigned int pan_state = amtecGetCubeState(&amtec_->dev, amtec_->pan.id);
00175     unsigned int tilt_serial = amtecGetDefCubeSerial(&amtec_->dev, amtec_->tilt.id);
00176     unsigned int tilt_state = amtecGetCubeState(&amtec_->dev, amtec_->pan.id);
00177 
00178     std::cout << "pan serial " << pan_serial << std::endl;
00179     std::cout << "pan state: " << std::endl;
00180     printModuleState(pan_state);
00181 
00182     std::cout << "tilt serial " << tilt_serial << std::endl;
00183     std::cout << "tilt state: " << std::endl;
00184     printModuleState(tilt_state);
00185 
00186     ROS_INFO("Homing device");
00187     amtecHome(&amtec_->dev, amtec_->pan.id);
00188     amtecHome(&amtec_->dev, amtec_->tilt.id);
00189     sleep(3);
00190 
00191     ROS_INFO("set PAN and TILT settings");
00192     amtecSetSettings(amtec_);
00193 
00194     ROS_INFO("Loading parameters");
00195     amtecGetParams(amtec_);
00196 
00197     // ***** Advertise Messages *****
00198     pan_state_pub_ = private_nh.advertise<amtec::AmtecState>("pan_state", 1);
00199     tilt_state_pub_ = private_nh.advertise<amtec::AmtecState>("tilt_state", 1);
00200 
00201     // ***** Start Services *****
00202     get_status_srv_ = private_nh.advertiseService("get_status", &AmtecNode::getStatus, this);
00203     halt_srv_ = private_nh.advertiseService("halt", &AmtecNode::halt, this);
00204     home_srv_ = private_nh.advertiseService("home", &AmtecNode::home, this);
00205     reset_srv_ = private_nh.advertiseService("reset", &AmtecNode::reset, this);
00206     set_position_srv_ = private_nh.advertiseService("set_position", &AmtecNode::setPosition, this);
00207     set_velocity_srv_ = private_nh.advertiseService("set_velocity", &AmtecNode::setVelocity, this);
00208     target_accel_srv_ = private_nh.advertiseService("target_acceleration", &AmtecNode::targetAcceleration, this);
00209     target_vel_srv_ = private_nh.advertiseService("target_velocity", &AmtecNode::targetVelocity, this);
00210     sweep_pan_srv_ = private_nh.advertiseService("sweep_pan", &AmtecNode::sweepPan, this);
00211     sweep_tilt_srv_ = private_nh.advertiseService("sweep_tilt", &AmtecNode::sweepTilt, this);
00212     ROS_INFO("Amtec is ready!");
00213   }
00214 
00215   virtual ~AmtecNode()
00216   {
00217     // shutdown amtec
00218     amtecClear(amtec_);
00219   }
00220 
00221   void printModuleState(unsigned int state)
00222   {
00223     if(state&STATE_HOME_OK) std::cout << "STATE_HOME_OK" << std::endl;
00224     if(state&STATE_HALTED) std::cout << "STATE_HALTED" << std::endl;
00225     if(state&STATE_SWR) std::cout << "STATE_SWR" << std::endl;
00226     if(state&STATE_SW1) std::cout << "STATE_SW1" << std::endl;
00227     if(state&STATE_SW2) std::cout << "STATE_SW2" << std::endl;
00228     if(state&STATE_BRAKEACTIVE) std::cout << "STATE_BRAKEACTIVE" << std::endl;
00229     if(state&STATE_CURLIMIT) std::cout << "STATE_CURLIMIT" << std::endl;
00230     if(state&STATE_MOTION) std::cout << "STATE_MOTION" << std::endl;
00231     if(state&STATE_RAMP_ACC) std::cout << "STATE_RAMP_ACC" << std::endl;
00232     if(state&STATE_RAMP_STEADY) std::cout << "STATE_RAMP_STEADY" << std::endl;
00233     if(state&STATE_RAMP_DEC) std::cout << "STATE_RAMP_DEC" << std::endl;
00234     if(state&STATE_RAMP_END) std::cout << "STATE_RAMP_END" << std::endl;
00235     if(state&STATE_INPROGRESS) std::cout << "STATE_INPROGRESS" << std::endl;
00236     if(state&STATE_FULLBUFFER) std::cout << "STATE_FULLBUFFER" << std::endl;
00237     if(state&STATE_ERROR) std::cout << "STATE_ERROR" << std::endl;
00238     if(state&STATE_POWERFAULT) std::cout << "STATE_POWERFAULT" << std::endl;
00239     if(state&STATE_TOW_ERROR) std::cout << "STATE_TOW_ERROR" << std::endl;
00240     if(state&STATE_COMM_ERROR) std::cout << "STATE_COMM_ERROR" << std::endl;
00241     if(state&STATE_POW_VOLT_ERR) std::cout << "STATE_POW_VOLT_ERR" << std::endl;
00242     if(state&STATE_POW_FET_TEMP) std::cout << "STATE_POW_FET_TEMP" << std::endl;
00243     if(state&STATE_POW_INTEGRALERR) std::cout << "STATE_POW_INTEGRALERR" << std::endl;
00244     if(state&STATE_BEYOND_HARD) std::cout << "STATE_BEYOND_HARD" << std::endl;
00245     if(state&STATE_BEYOND_SOFT) std::cout << "STATE_BEYOND_SOFT" << std::endl;
00246     if(state&STATE_LOGIC_VOLT) std::cout << "STATE_LOGIC_VOLT" << std::endl;
00247   }
00248 
00249   bool getStatus(amtec::GetStatus::Request& req, amtec::GetStatus::Response& resp)
00250   {
00251     amtec_mutex_.lock();
00252     resp.position_pan = -amtecGetActPos(&amtec_->dev, amtec_->pan.id);
00253     resp.position_tilt = amtecGetActPos(&amtec_->dev, amtec_->tilt.id);
00254     resp.velocity_pan = -amtecGetActVel(&amtec_->dev, amtec_->pan.id);
00255     resp.velocity_tilt = amtecGetActVel(&amtec_->dev, amtec_->tilt.id);
00256     amtec_mutex_.unlock();
00257     return true;
00258   }
00259 
00260   bool halt(amtec::Halt::Request& req, amtec::Halt::Response& resp)
00261   {
00262     bool ret = true;
00263     amtec_mutex_.lock();
00264     ret = ret && amtecHalt(&amtec_->dev, amtec_->pan.id);
00265     ret = ret && amtecHalt(&amtec_->dev, amtec_->tilt.id);
00266     amtec_mutex_.unlock();
00267     return ret;
00268   }
00269 
00270   bool home(amtec::Home::Request& req, amtec::Home::Response& resp)
00271   {
00272     bool ret;
00273     amtec_mutex_.lock();
00274     ret = amtecHome(&amtec_->dev, amtec_->pan.id);
00275     ret = ret && amtecHome(&amtec_->dev, amtec_->tilt.id);
00276     amtec_mutex_.unlock();
00277     return ret;
00278   }
00279 
00280   bool reset(amtec::Reset::Request& req, amtec::Reset::Response& resp)
00281   {
00282     bool ret;
00283     amtec_mutex_.lock();
00284     ret = amtecReset(&amtec_->dev, amtec_->pan.id);
00285     ret = ret && amtecReset(&amtec_->dev, amtec_->tilt.id);
00286     amtec_mutex_.unlock();
00287     return ret;
00288   }
00289 
00290   bool setPosition(amtec::SetPosition::Request& req, amtec::SetPosition::Response& resp)
00291   {
00292     bool ret;
00293     amtec_mutex_.lock();
00294     ret = amtecMotionFRamp(&amtec_->dev, amtec_->pan.id, -req.position_pan);
00295 
00296     // to avoid gratuitous locking if already at requested tilt position
00297     if (fabs(req.position_tilt - amtec_tilt_state_message_.position) > 0.001)
00298       ret = ret && amtecMotionFRamp(&amtec_->dev, amtec_->tilt.id, req.position_tilt);
00299 
00300     amtec_mutex_.unlock();
00301     return ret;
00302   }
00303 
00304   bool setVelocity(amtec::SetVelocity::Request& req, amtec::SetVelocity::Response& resp)
00305   {
00306     bool ret;
00307     amtec_mutex_.lock();
00308     ret = amtecMotionFVel(&amtec_->dev, amtec_->pan.id, -req.velocity_pan);
00309     ret = ret && amtecMotionFVel(&amtec_->dev, amtec_->tilt.id, req.velocity_tilt);
00310     amtec_mutex_.unlock();
00311     return ret;
00312   }
00313 
00314   bool targetAcceleration(amtec::TargetAcceleration::Request& req, amtec::TargetAcceleration::Response& resp)
00315   {
00316     bool ret = true;
00317     amtec_mutex_.lock();
00318     amtecSetTargetAcc(&amtec_->dev, amtec_->pan.id, req.acceleration_pan);
00319     amtecSetTargetAcc(&amtec_->dev, amtec_->tilt.id, req.acceleration_tilt);
00320     amtec_mutex_.unlock();
00321     return ret;
00322   }
00323 
00324   bool targetVelocity(amtec::TargetVelocity::Request& req, amtec::TargetVelocity::Response& resp)
00325   {
00326     bool ret = true;
00327     amtec_mutex_.lock();
00328     amtecSetTargetVel(&amtec_->dev, amtec_->pan.id, req.velocity_pan);
00329     amtecSetTargetVel(&amtec_->dev, amtec_->tilt.id, req.velocity_tilt);
00330     amtec_mutex_.unlock();
00331     return ret;
00332   }
00333 
00334   bool sweepPan(amtec::SweepPan::Request& req, amtec::SweepPan::Response& resp)
00335   {
00336     bool ret;
00337     amtec_mutex_.lock();
00338     ret = amtecMotionFCosLoop(&amtec_->dev, amtec_->pan.id, req.sweep_amplitude, req.sweep_period);
00339     amtec_mutex_.unlock();
00340     return ret;
00341   }
00342 
00343   bool sweepTilt(amtec::SweepTilt::Request& req, amtec::SweepTilt::Response& resp)
00344   {
00345     bool ret;
00346     amtec_mutex_.lock();
00347     ret = amtecMotionFCosLoop(&amtec_->dev, amtec_->tilt.id, req.sweep_amplitude, req.sweep_period);
00348     amtec_mutex_.unlock();
00349     return ret;
00350   }
00351 
00352   bool spin()
00353   {
00354     ros::Rate r(100); // 10 ms or 100 Hz
00355     while (node_.ok())
00356     {
00357       amtec_mutex_.lock();
00358       amtec_pan_state_message_.state = amtecGetCubeState(&amtec_->dev, amtec_->pan.id);
00359       amtec_pan_state_message_.position = -amtecGetActPos(&amtec_->dev, amtec_->pan.id);
00360       amtec_pan_state_message_.velocity = -amtecGetActVel(&amtec_->dev, amtec_->pan.id);
00361       amtec_tilt_state_message_.state = amtecGetCubeState(&amtec_->dev, amtec_->tilt.id);
00362       amtec_tilt_state_message_.position = amtecGetActPos(&amtec_->dev, amtec_->tilt.id);
00363       amtec_tilt_state_message_.velocity = amtecGetActVel(&amtec_->dev, amtec_->tilt.id);
00364       //TODO(duhadway): check assumption that positions are given in radians
00365       parent_to_amtec_.setRotation(btQuaternion( amtec_pan_state_message_.position, -amtec_tilt_state_message_.position, 0 ));
00366       amtec_mutex_.unlock();
00367 
00368       pan_state_pub_.publish(amtec_pan_state_message_);
00369       tilt_state_pub_.publish(amtec_tilt_state_message_);
00370       tf_.sendTransform( tf::StampedTransform(parent_to_amtec_, ros::Time::now(), parent_frame_, amtec_frame_));
00371 
00372       ros::spinOnce();
00373       r.sleep();
00374     }
00375     return true;
00376   }
00377 };
00378 
00379 int main(int argc, char **argv)
00380 {
00381   ros::init(argc, argv, "amtec");
00382   AmtecNode a;
00383   a.spin();
00384   return 0;
00385 }


amtec
Author(s): Charles DuHadway, Benjamin Pitzer (Maintained by Benjamin Pitzer)
autogenerated on Mon Oct 6 2014 10:10:36