orbit_pantilt.cpp
Go to the documentation of this file.
00001 
00002 //
00003 // JSK Logicool Orbit/Sphere control node
00004 //
00005 
00006 #include <ros/ros.h>
00007 #include <ros/names.h>
00008 
00009 #include <std_srvs/Empty.h>
00010 #include <sensor_msgs/JointState.h>
00011 #include <orbit_pantilt/JointCommand.h>
00012 
00013 #include <sys/stat.h>
00014 #include <sys/ioctl.h>
00015 #include <fcntl.h>
00016 #include <errno.h>
00017 
00018 #include <linux/videodev2.h>
00019 
00020 // dynamic reconfigure
00021 #include <dynamic_reconfigure/server.h>
00022 #include "orbit_pantilt/OrbitPanTiltConfig.h"
00023 
00024 class OrbitPanTilt {
00025 private:
00026     ros::NodeHandle nh_;
00027 
00028     // dynamic reconfigure
00029     typedef dynamic_reconfigure::Server<orbit_pantilt::OrbitPanTiltConfig> ReconfigureServer;
00030     ReconfigureServer reconfigure_server_;
00031 
00032     //
00033     int fd_;
00034     std::string device_;
00035     double pan_, tilt_; // [rad]
00036     int pan_ratio_, tilt_ratio_;
00037 
00038 public:
00039     int set_ext_ctrls(int fd, int id, int value) {
00040         struct v4l2_ext_control xctrls[1];
00041         struct v4l2_ext_controls ctrls;
00042 
00043         xctrls[0].id = id;
00044         xctrls[0].value = value;
00045 
00046         ctrls.count = 1;
00047         ctrls.controls = xctrls;
00048 
00049         if (-1 == ioctl(fd_, VIDIOC_S_EXT_CTRLS, &ctrls)) {
00050             throw std::runtime_error("ioctl failed");
00051             return 0;
00052         }
00053         return 1;
00054     }
00055 
00056     void config_cb (orbit_pantilt::OrbitPanTiltConfig &config, uint32_t level)
00057     {
00058         if ( config.pan_reset ) {
00059             if (!set_ext_ctrls(fd_, V4L2_CID_PAN_RESET, 1))
00060                 throw std::runtime_error(device_ + " does not support set pan reset");
00061             config.pan_reset = false;
00062             config.pan = 0;
00063             pan_ = 0;
00064         }
00065         if ( config.tilt_reset ) {
00066             if (!set_ext_ctrls(fd_, V4L2_CID_TILT_RESET, 1))
00067                 throw std::runtime_error(device_ + " does not support set tilt reset");
00068             config.tilt_reset = false;
00069             config.tilt = 0;
00070             tilt_ = 0;
00071         }
00072 
00073         printf("pan  %d %d\n", pan_, config.pan);
00074         if ( pan_ != config.pan ) {
00075             if (!set_ext_ctrls(fd_, V4L2_CID_PAN_RELATIVE, (pan_-config.pan)*pan_ratio_))
00076                 throw std::runtime_error(device_ + " does not support set pan relative");
00077             pan_ = config.pan;
00078         }
00079         printf("tilt %d %d\n", tilt_, config.tilt);
00080         if ( tilt_ != config.tilt ) {
00081             if (!set_ext_ctrls(fd_, V4L2_CID_TILT_RELATIVE, (tilt_-config.tilt)*tilt_ratio_))
00082                 throw std::runtime_error(device_ + " does not support set tilt relative");
00083             tilt_ = config.tilt;
00084         }
00085     }
00086 
00087     double min_max_angle(double raw_data_rad, double min_deg, double max_deg) {
00088       double min_rad = min_deg * M_PI / 180.0;
00089       double max_rad = max_deg * M_PI / 180.0;
00090       if (raw_data_rad < min_rad) { return min_rad; }
00091       if (raw_data_rad > max_rad) { return max_rad; }
00092       return raw_data_rad;
00093     }
00094 
00095     void command_cb(const orbit_pantilt::JointCommandConstPtr& msg) {
00096       double msgpan = min_max_angle(msg->pan, -94, 94);
00097       double msgtilt = min_max_angle(msg->tilt, -51, 51);
00098       if ( fabs(msgpan - pan_) > 0.5 * M_PI / 180.0 ) {
00099         if (!set_ext_ctrls(fd_, V4L2_CID_PAN_RELATIVE, round((msgpan-pan_)*pan_ratio_*180.0/M_PI)))
00100             throw std::runtime_error(device_ + " does not support set pan relative");
00101         pan_ = msgpan;
00102         nh_.setParam("pan", pan_*180.0/M_PI);
00103       }
00104       if ( fabs(msgtilt - tilt_) > 0.5 * M_PI / 180.0 ) {
00105         if (!set_ext_ctrls(fd_, V4L2_CID_TILT_RELATIVE, round((msgtilt-tilt_)*tilt_ratio_*180.0/M_PI)))
00106             throw std::runtime_error(device_ + " does not support set tilt relative");
00107         tilt_ = msgtilt;
00108         nh_.setParam("tilt", tilt_*180.0/M_PI);
00109       }
00110     }
00111 
00112     bool pan_reset(std_srvs::Empty::Request &req,
00113                    std_srvs::Empty::Response &res) {
00114         if (!set_ext_ctrls(fd_, V4L2_CID_PAN_RESET, 1))
00115             throw std::runtime_error(device_ + " does not support set pan reset");
00116         pan_ = 0;
00117         nh_.setParam("pan", 0);
00118         return true;
00119     }
00120     bool tilt_reset(std_srvs::Empty::Request &req,
00121                    std_srvs::Empty::Response &res) {
00122         if (!set_ext_ctrls(fd_, V4L2_CID_TILT_RESET, 1))
00123             throw std::runtime_error(device_ + " does not support set tilt reset");
00124         tilt_ = 0;
00125         nh_.setParam("tilt", 0);
00126         return true;
00127     }
00128 
00129 
00130     OrbitPanTilt() : nh_("~"), pan_(0), tilt_(0)
00131     {
00132 
00133         //
00134         device_ = "/dev/video0";
00135 
00136         nh_.getParam("device", device_);
00137         ROS_INFO("device : %s", device_.c_str());
00138 
00139         pan_ratio_ = 40;
00140         tilt_ratio_ = 40;
00141         nh_.getParam("pan_ratio", pan_ratio_);
00142         nh_.getParam("tilt_ratio", tilt_ratio_);
00143 
00144         ReconfigureServer::CallbackType f = boost::bind(&OrbitPanTilt::config_cb, this, _1, _2);
00145         reconfigure_server_.setCallback(f);
00146 
00147         ros::Publisher joint_state_pub = nh_.advertise<sensor_msgs::JointState> ("joint_states", 1);
00148         ros::Subscriber joint_command_sub = nh_.subscribe<orbit_pantilt::JointCommand> ("pan_tilt_command", 1, &OrbitPanTilt::command_cb, this);
00149         ros::ServiceServer pan_reset_srv  = nh_.advertiseService("pan_reset", &OrbitPanTilt::pan_reset, this);
00150         ros::ServiceServer tilt_reset_srv  = nh_.advertiseService("tilt_reset", &OrbitPanTilt::tilt_reset, this);
00151 
00152         printf("opening %s\n", device_.c_str());
00153         if ((fd_ = open(device_.c_str(), O_RDWR)) == -1)
00154             throw std::runtime_error("couldn't open " + device_);
00155 
00156         v4l2_format fmt;
00157         v4l2_capability cap;
00158 
00159         memset(&fmt, 0, sizeof(v4l2_format));
00160         memset(&cap, 0, sizeof(v4l2_capability));
00161         if (ioctl(fd_, VIDIOC_QUERYCAP, &cap) < 0)
00162             throw std::runtime_error("couldn't query " + device_);
00163         if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
00164             throw std::runtime_error(device_ + " does not support capture");
00165         if (!(cap.capabilities & V4L2_CAP_STREAMING))
00166             throw std::runtime_error(device_ + " does not support streaming");
00167 
00168         if (!set_ext_ctrls(fd_, V4L2_CID_PAN_RESET, 1))
00169             throw std::runtime_error(device_ + " does not support set pan reset");
00170         nh_.setParam("pan", 0);
00171         sleep(3);
00172         if (!set_ext_ctrls(fd_, V4L2_CID_TILT_RESET, 1))
00173             throw std::runtime_error(device_ + " does not support set tilt reset");
00174         nh_.setParam("tilt", 0);
00175         sleep(3);
00176 
00177         ros::Rate loop_rate(1);
00178         while( ros::ok() ) {
00179             sensor_msgs::JointState msg;
00180 
00181             msg.header.stamp = ros::Time::now();
00182             msg.name.push_back("pan");
00183             msg.name.push_back("tilt");
00184             msg.position.push_back(pan_);
00185             msg.position.push_back(tilt_);
00186 
00187             joint_state_pub.publish(msg);
00188 
00189             ros::spinOnce();
00190             loop_rate.sleep();
00191         }
00192 
00193         if ( fd_ > 0 ) {
00194             close(fd_);
00195         }
00196     }
00197 };
00198 
00199 int main(int argc, char **argv) {
00200   ros::init(argc, argv, "orbit_pantilt");
00201 
00202   try {
00203       OrbitPanTilt orbit_pantilt;
00204   }
00205   catch (std::runtime_error &ex) {
00206       printf("ERROR: could not set some settings.  \n %s \n", ex.what());
00207   }
00208   return 0;
00209 }


orbit_pantilt
Author(s): k-okada
autogenerated on Mon Oct 6 2014 01:17:41