Go to the documentation of this file.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 #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
00050 void connect();
00051 bool ok()
00052 {
00053 return m_pantilt != NULL;
00054 }
00055 void disconnect();
00056
00057
00058 void spinCallback(const ros::TimerEvent&);
00059
00060
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
00097 if (ok())
00098 {
00099 disconnect();
00100 }
00101
00102
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
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
00151 m_joint_pub = m_node.advertise
00152 <sensor_msgs::JointState>("state", 1);
00153
00154
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;
00165 m_pantilt = NULL;
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
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
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 }
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
00240 flir_ptu_driver::Node ptu_node(n);
00241 ptu_node.connect();
00242
00243
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
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 }