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
00026
00027
00028
00029
00030
00031
00032
00033
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
00049 #include <amtec/AmtecState.h>
00050
00051
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
00092 AmtecNode() : node_(), amtec_(NULL), tf_()
00093 {
00094 std::string device, parity;
00095
00096
00097 ROS_INFO("Initializing Amtec");
00098 amtec_ = amtecInitialize();
00099
00100 parent_to_amtec_.setIdentity();
00101
00102 ros::NodeHandle private_nh("~");
00103
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
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
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
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
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
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);
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
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 }
