node.cpp
Go to the documentation of this file.
00001 /*
00002  * flir_ptu_driver ROS package
00003  * Copyright (C) 2014 Mike Purvis (mpurvis@clearpathrobotics.com)
00004  *
00005  * PTU ROS Package
00006  * Copyright (C) 2009 Erik Karulf (erik@cse.wustl.edu)
00007  *
00008  * Author: Toby Collett (University of Auckland)
00009  * Date: 2003-02-10
00010  *
00011  * Player - One Hell of a Robot Server
00012  * Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00013  *                     gerkey@usc.edu    kaspers@robotics.usc.edu
00014  *
00015  *  This program is free software; you can redistribute it and/or modify
00016  *  it under the terms of the GNU General Public License as published by
00017  *  the Free Software Foundation; either version 2 of the License, or
00018  *  (at your option) any later version.
00019  *
00020  *  This program is distributed in the hope that it will be useful,
00021  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023  *  GNU General Public License for more details.
00024  *
00025  *  You should have received a copy of the GNU General Public License
00026  *  along with this program; if not, write to the Free Software
00027  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00028  *
00029  */
00030 
00031 #include <diagnostic_updater/diagnostic_updater.h>
00032 #include <diagnostic_updater/publisher.h>
00033 #include <flir_ptu_driver/driver.h>
00034 #include <ros/ros.h>
00035 #include <sensor_msgs/JointState.h>
00036 #include <serial/serial.h>
00037 
00038 #include <string>
00039 
00040 namespace flir_ptu_driver
00041 {
00042 
00043 class Node
00044 {
00045 public:
00046   explicit Node(ros::NodeHandle& node_handle);
00047   ~Node();
00048 
00049   // Service Control
00050   void connect();
00051   bool ok()
00052   {
00053     return m_pantilt != NULL;
00054   }
00055   void disconnect();
00056 
00057   // Service Execution
00058   void spinCallback(const ros::TimerEvent&);
00059 
00060   // Callback Methods
00061   void cmdCallback(const sensor_msgs::JointState::ConstPtr& msg);
00062 
00063   void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
00064 
00065 protected:
00066   diagnostic_updater::Updater* m_updater;
00067   PTU* m_pantilt;
00068   ros::NodeHandle m_node;
00069   ros::Publisher  m_joint_pub;
00070   ros::Subscriber m_joint_sub;
00071 
00072   serial::Serial m_ser;
00073   std::string m_joint_name_prefix;
00074 };
00075 
00076 Node::Node(ros::NodeHandle& node_handle)
00077   : m_pantilt(NULL), m_node(node_handle)
00078 {
00079   m_updater = new diagnostic_updater::Updater();
00080   m_updater->setHardwareID("none");
00081   m_updater->add("PTU Status", this, &Node::produce_diagnostics);
00082 
00083   ros::param::param<std::string>("~joint_name_prefix", m_joint_name_prefix, "ptu_");
00084 }
00085 
00086 Node::~Node()
00087 {
00088   disconnect();
00089   delete m_updater;
00090 }
00091 
00094 void Node::connect()
00095 {
00096   // If we are reconnecting, first make sure to disconnect
00097   if (ok())
00098   {
00099     disconnect();
00100   }
00101 
00102   // Query for serial configuration
00103   std::string port;
00104   int32_t baud;
00105   ros::param::param<std::string>("~port", port, PTU_DEFAULT_PORT);
00106   ros::param::param<int32_t>("~baud", baud, PTU_DEFAULT_BAUD);
00107 
00108   // Connect to the PTU
00109   ROS_INFO_STREAM("Attempting to connect to FLIR PTU on " << port);
00110 
00111   try
00112   {
00113     m_ser.setPort(port);
00114     m_ser.setBaudrate(baud);
00115     serial::Timeout to = serial::Timeout(200, 200, 0, 200, 0);
00116     m_ser.setTimeout(to);
00117     m_ser.open();
00118   }
00119   catch (serial::IOException& e)
00120   {
00121     ROS_ERROR_STREAM("Unable to open port " << port);
00122     return;
00123   }
00124 
00125   ROS_INFO_STREAM("FLIR PTU serial port opened, now initializing.");
00126 
00127   m_pantilt = new PTU(&m_ser);
00128 
00129   if (!m_pantilt->initialize())
00130   {
00131     ROS_ERROR_STREAM("Could not initialize FLIR PTU on " << port);
00132     disconnect();
00133     return;
00134   }
00135 
00136   ROS_INFO("FLIR PTU initialized.");
00137 
00138   m_node.setParam("min_tilt", m_pantilt->getMin(PTU_TILT));
00139   m_node.setParam("max_tilt", m_pantilt->getMax(PTU_TILT));
00140   m_node.setParam("min_tilt_speed", m_pantilt->getMinSpeed(PTU_TILT));
00141   m_node.setParam("max_tilt_speed", m_pantilt->getMaxSpeed(PTU_TILT));
00142   m_node.setParam("tilt_step", m_pantilt->getResolution(PTU_TILT));
00143 
00144   m_node.setParam("min_pan", m_pantilt->getMin(PTU_PAN));
00145   m_node.setParam("max_pan", m_pantilt->getMax(PTU_PAN));
00146   m_node.setParam("min_pan_speed", m_pantilt->getMinSpeed(PTU_PAN));
00147   m_node.setParam("max_pan_speed", m_pantilt->getMaxSpeed(PTU_PAN));
00148   m_node.setParam("pan_step", m_pantilt->getResolution(PTU_PAN));
00149 
00150   // Publishers : Only publish the most recent reading
00151   m_joint_pub = m_node.advertise
00152                 <sensor_msgs::JointState>("state", 1);
00153 
00154   // Subscribers : Only subscribe to the most recent instructions
00155   m_joint_sub = m_node.subscribe
00156                 <sensor_msgs::JointState>("cmd", 1, &Node::cmdCallback, this);
00157 }
00158 
00160 void Node::disconnect()
00161 {
00162   if (m_pantilt != NULL)
00163   {
00164     delete m_pantilt;   // Closes the connection
00165     m_pantilt = NULL;   // Marks the service as disconnected
00166   }
00167 }
00168 
00170 void Node::cmdCallback(const sensor_msgs::JointState::ConstPtr& msg)
00171 {
00172   ROS_DEBUG("PTU command callback.");
00173   if (!ok()) return;
00174 
00175   if (msg->position.size() != 2 || msg->velocity.size() != 2)
00176   {
00177     ROS_ERROR("JointState command to PTU has wrong number of elements.");
00178     return;
00179   }
00180 
00181   double pan = msg->position[0];
00182   double tilt = msg->position[1];
00183   double panspeed = msg->velocity[0];
00184   double tiltspeed = msg->velocity[1];
00185   m_pantilt->setPosition(PTU_PAN, pan);
00186   m_pantilt->setPosition(PTU_TILT, tilt);
00187   m_pantilt->setSpeed(PTU_PAN, panspeed);
00188   m_pantilt->setSpeed(PTU_TILT, tiltspeed);
00189 }
00190 
00191 void Node::produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
00192 {
00193   stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All normal.");
00194   stat.add("PTU Mode", m_pantilt->getMode() == PTU_POSITION ? "Position" : "Velocity");
00195 }
00196 
00197 
00202 void Node::spinCallback(const ros::TimerEvent&)
00203 {
00204   if (!ok()) return;
00205 
00206   // Read Position & Speed
00207   double pan  = m_pantilt->getPosition(PTU_PAN);
00208   double tilt = m_pantilt->getPosition(PTU_TILT);
00209 
00210   double panspeed  = m_pantilt->getSpeed(PTU_PAN);
00211   double tiltspeed = m_pantilt->getSpeed(PTU_TILT);
00212 
00213   // Publish Position & Speed
00214   sensor_msgs::JointState joint_state;
00215   joint_state.header.stamp = ros::Time::now();
00216   joint_state.name.resize(2);
00217   joint_state.position.resize(2);
00218   joint_state.velocity.resize(2);
00219   joint_state.name[0] = m_joint_name_prefix + "pan";
00220   joint_state.position[0] = pan;
00221   joint_state.velocity[0] = panspeed;
00222   joint_state.name[1] = m_joint_name_prefix + "tilt";
00223   joint_state.position[1] = tilt;
00224   joint_state.velocity[1] = tiltspeed;
00225   m_joint_pub.publish(joint_state);
00226 
00227   m_updater->update();
00228 }
00229 
00230 }  // namespace flir_ptu_driver
00231 
00232 int main(int argc, char** argv)
00233 {
00234   ros::init(argc, argv, "ptu");
00235   ros::NodeHandle n;
00236 
00237   while (ros::ok())
00238   {
00239     // Connect to PTU
00240     flir_ptu_driver::Node ptu_node(n);
00241     ptu_node.connect();
00242 
00243     // Set up polling callback
00244     int hz;
00245     ros::param::param<int>("~hz", hz, PTU_DEFAULT_HZ);
00246     ros::Timer spin_timer = n.createTimer(ros::Duration(1 / hz),
00247         &flir_ptu_driver::Node::spinCallback, &ptu_node);
00248 
00249     // Spin until there's a problem or we're in shutdown
00250     ros::spin();
00251 
00252     if (!ptu_node.ok())
00253     {
00254       ROS_ERROR("FLIR PTU disconnected, attempting reconnection.");
00255       ros::Duration(1.0).sleep();
00256     }
00257   }
00258 
00259   return 0;
00260 }


flir_ptu_driver
Author(s): Erik Karulf , David V. Lu , Nick Hawes
autogenerated on Fri Aug 28 2015 10:39:27