pantilt.cpp
Go to the documentation of this file.
00001 /*
00002  * pantilt
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the <ORGANIZATION> nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 // Author: Dallas Goecker
00032 // Author: Morgan Cormier // Modifications of the original package
00033 
00034 // Controls pan and tilt feature of Logitech Orbit camera.  Will probably work 
00035 // for other USB pand/tilt cameras.
00036 //
00037 // joy.axes[] values are expected to range between -1 and 1.
00038 
00039 #include <ros/ros.h>
00040 #include <corobot_msgs/PanTilt.h>
00041 
00042 #include <stdio.h>
00043 #include <stdlib.h>
00044 #include <unistd.h>
00045 #include <string.h>
00046 #include <fcntl.h>
00047 #include <linux/videodev2.h>
00048 #include <sys/ioctl.h>
00049 #include <errno.h>
00050 #include <diagnostic_updater/diagnostic_updater.h>
00051 #include <diagnostic_updater/publisher.h>
00052 #include <corobot_diagnostics/diagnostics.h>
00053 
00054 extern "C" {
00055   #include "dynctrl.h"
00056 }
00057 
00058 #define BITS_PER_DEGREE (64)
00059 #define MAX_PAN_DEG  (70)
00060 #define MIN_PAN_DEG  (-70)
00061 #define MAX_TILT_DEG (30)
00062 #define MIN_TILT_DEG (-30)
00063 #define MIN_STEP_DEG (2)
00064 
00065 int pantilt_reset(int fd);
00066 int pantilt_move(int fd, int pan, int tilt);
00067 void pantilt_check_speed(int fd);
00068 
00069 static int cam_fd;
00070 // use for diagnostics purpose
00071 int pantiltError = 0; 
00072 
00073 
00074 void pantiltCallback(const corobot_msgs::PanTiltConstPtr& msg)
00078 {
00079   if (msg->reset) {
00080     pantilt_reset(cam_fd);
00081     ROS_DEBUG("PanTilt reset");
00082   } else {
00083     ROS_DEBUG("PanTilt moving pan=%d, tilt=%d", msg->pan, msg->tilt);
00084     pantilt_move(cam_fd,msg->pan, msg->tilt);
00085   }
00086 }
00087 
00088 void pantilt_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00092 {
00093         if (!pantiltError)  
00094                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized");
00095         else if (pantiltError == 1)
00096         {
00097                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera pan tilt cannot be initialized");
00098                 stat.addf("Recommendation", CAMERA_DISCONNECTED);
00099         }
00100         else if (pantiltError == 2)
00101         {
00102                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera cannot be moved");
00103                 stat.addf("Recommendation", ERROR_MOVING_CAMERA);
00104         }
00105 }
00106 
00107 // reset the pan tilt position
00108 int pantilt_reset(int fd) {
00109   int result;
00110   result = uvcPanTilt(fd, 0, 0, 3);
00111 
00112   if (result == -1)
00113         pantiltError = 2;
00114   else
00115         pantiltError = 0;
00116 
00117   sleep(2);
00118   return result;
00119 }
00120 
00121 // move the pan tilt camera
00122 int pantilt_move(int fd, int pan, int tilt){
00123   int result;
00124   result = uvcPanTilt(fd, pan*BITS_PER_DEGREE, tilt*BITS_PER_DEGREE, 0);
00125 
00126   if (result == -1)
00127         pantiltError = 2;
00128   else
00129         pantiltError = 0;
00130   return result;
00131 }
00132 
00133 
00134 int main(int argc, char** argv)
00135 {
00136   std::string dev;
00137   std::string path;
00138   ros::init(argc, argv, "pantilt");
00139   ros::NodeHandle n;
00140   ros::NodeHandle nh("~");
00141   
00142   // port of the camera
00143   nh.param<std::string>("device",dev, "/dev/video0");
00144   nh.param<std::string>("script_path",path, "init_camera.sh");
00145 
00146   //create an updater that will send information on the diagnostics topics
00147   diagnostic_updater::Updater updater;
00148   updater.setHardwareIDf("PanTIlt");
00149   //function that will be executed with updater.update()
00150   updater.add("PanTilt", pantilt_diagnostic); 
00151 
00152   if ((cam_fd = open(dev.c_str(), O_RDWR)) == -1) {
00153     ROS_ERROR("PanTilt could not open %s interface\n",dev.c_str());
00154     pantiltError = 1;
00155   }
00156   else
00157     pantiltError = 0;
00158 
00159   ROS_INFO("PanTilt Opened: %s\n",dev.c_str());
00160 
00161 // Initialize the pan tilt camera
00162   initDynCtrls(cam_fd);
00163   ROS_INFO("PanTilt Initilized: %s\n",dev.c_str());
00164   ROS_DEBUG("PanTilt note int(1.4)=%d, int(-1.4)=%d", int(1.4), int(-1.4));
00165 
00166 //adversize the topics
00167   ros::Subscriber pantilt_sub = n.subscribe("/pantilt", 1, pantiltCallback);
00168   ros::Rate rate(40);
00169   while (ros::ok())
00170   {
00171         ros::spinOnce();
00172         // diagnostic updated
00173         updater.update();
00174         rate.sleep();
00175   }
00176 
00177   close(cam_fd);
00178   return 0;
00179 }
00180 
00181 
00182 
00183 
00184 
00185 
00186 
00187 
00188 
00189 
00190 


corobot_pantilt
Author(s):
autogenerated on Wed Aug 26 2015 11:10:04