image_cb_detector_action.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009-2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00036 
00037 #include <boost/thread.hpp>
00038 #include <ros/console.h>
00039 #include <ros/ros.h>
00040 #include <actionlib/server/simple_action_server.h>
00041 #include <image_cb_detector/image_cb_detector.h>
00042 #include <image_transport/image_transport.h>
00043 #include <sensor_msgs/Image.h>
00044 #include <image_cb_detector/ConfigAction.h>
00045 #include <calibration_msgs/Interval.h>
00046 
00047 using namespace image_cb_detector;
00048 
00049 class ImageCbDetectorAction
00050 {
00051 public:
00052   ImageCbDetectorAction() : as_("cb_detector_config", false), it_(nh_)
00053   {
00054     as_.registerGoalCallback( boost::bind(&ImageCbDetectorAction::goalCallback, this) );
00055     as_.registerPreemptCallback( boost::bind(&ImageCbDetectorAction::preemptCallback, this) );
00056 
00057     pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("features",1);
00058     sub_ = it_.subscribe("image", 2, boost::bind(&ImageCbDetectorAction::imageCallback, this, _1));
00059     as_.start();
00060   }
00061 
00062   void goalCallback()
00063   {
00064     boost::mutex::scoped_lock lock(run_mutex_);
00065 
00066     // Stop the previously running goal (if it exists)
00067     if (as_.isActive())
00068       as_.setPreempted();
00069 
00070     // Get the new goal from the action server
00071     image_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00072     assert(goal);
00073 
00074     // Try to reconfigure the settler object
00075     bool success = detector_.configure(*goal);
00076 
00077     // Detect possible failure
00078     if (!success)
00079       as_.setAborted();
00080   }
00081 
00082   void preemptCallback()
00083   {
00084     boost::mutex::scoped_lock lock(run_mutex_);
00085 
00086     // Don't need to do any cleanup. Immeadiately turn it off
00087     as_.setPreempted();
00088   }
00089 
00090   void imageCallback(const sensor_msgs::ImageConstPtr& image)
00091   {
00092     boost::mutex::scoped_lock lock(run_mutex_);
00093 
00094     if (as_.isActive())
00095     {
00096       calibration_msgs::CalibrationPattern features;
00097       bool success;
00098       success = detector_.detect(image, features);
00099 
00100       if (!success)
00101       {
00102         ROS_ERROR("Error trying to detect checkerboard, not going to publish CalibrationPattern");
00103         return;
00104       }
00105 
00106       pub_.publish(features);
00107     }
00108   }
00109 
00110 private:
00111   boost::mutex run_mutex_;
00112   ros::NodeHandle nh_;
00113   actionlib::SimpleActionServer<image_cb_detector::ConfigAction> as_;
00114   ImageCbDetector detector_;
00115 
00116   ros::Publisher pub_;
00117   image_transport::ImageTransport it_;
00118   image_transport::Subscriber sub_;
00119 
00120 };
00121 
00122 int main(int argc, char** argv)
00123 {
00124   ros::init(argc, argv, "image_cb_detector_action");
00125   ros::NodeHandle n;
00126   ImageCbDetectorAction detector_action;
00127   ros::spin();
00128   return 0;
00129 }


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Tue Sep 27 2016 04:06:37