monocam_settler_action.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, 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/ros.h>
00039 #include <actionlib/server/simple_action_server.h>
00040 #include <monocam_settler/monocam_settler.h>
00041 
00042 // Messages
00043 #include <monocam_settler/ConfigAction.h>
00044 #include <calibration_msgs/CalibrationPattern.h>
00045 #include <calibration_msgs/Interval.h>
00046 
00047 using namespace monocam_settler;
00048 
00049 class MonocamSettlerAction
00050 {
00051 public:
00052   MonocamSettlerAction() : as_("monocam_settler_config", false)
00053   {
00054     // Set up action callbacks
00055     as_.registerGoalCallback( boost::bind(&MonocamSettlerAction::goalCallback, this) );
00056     as_.registerPreemptCallback( boost::bind(&MonocamSettlerAction::preemptCallback, this) );
00057 
00058     pub_ = nh_.advertise<calibration_msgs::Interval>("settled_interval", 1);
00059     sub_ = nh_.subscribe("features", 1, &MonocamSettlerAction::msgCallback, this);
00060     as_.start();
00061   }
00062 
00063   void goalCallback()
00064   {
00065     boost::mutex::scoped_lock lock(run_mutex_);
00066 
00067     // Stop the previously running goal (if it exists)
00068     if (as_.isActive())
00069       as_.setPreempted();
00070 
00071     // Get the new goal from the action server
00072     monocam_settler::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00073     assert(goal);
00074 
00075     // Try to reconfigure the settler object
00076     bool success = settler_.configure(*goal);
00077 
00078     // Detect possible reconfigure failure
00079     if (!success)
00080       as_.setAborted();
00081   }
00082 
00083   void preemptCallback()
00084   {
00085     boost::mutex::scoped_lock lock(run_mutex_);
00086     as_.setPreempted();
00087   }
00088 
00089   void msgCallback(const calibration_msgs::CalibrationPatternConstPtr& msg)
00090   {
00091     boost::mutex::scoped_lock lock(run_mutex_);
00092 
00093     if (as_.isActive())
00094     {
00095       bool success;
00096 
00097       calibration_msgs::Interval interval;
00098       success = settler_.add(msg, interval);
00099 
00100       if (success)
00101         pub_.publish(interval);
00102       else
00103       {
00104         // Publish 'null' interval if we didn't find a checkerboard
00105         interval.start = msg->header.stamp;
00106         interval.end =   msg->header.stamp;
00107         pub_.publish(interval);
00108       }
00109     }
00110     else
00111       ROS_DEBUG("Got a feature msg, but not doing anything with it. No active goal, so node is currently idle");
00112   }
00113 
00114 private:
00115   ros::NodeHandle nh_;
00116   actionlib::SimpleActionServer<monocam_settler::ConfigAction> as_;
00117   ros::Publisher pub_;
00118   ros::Subscriber sub_;
00119 
00120   boost::mutex run_mutex_;
00121   MonocamSettler settler_;
00122 };
00123 
00124 int main(int argc, char** argv)
00125 {
00126   ros::init(argc, argv, "joint_states_settler_action");
00127   ros::NodeHandle n;
00128   MonocamSettlerAction settler_action;
00129   ros::spin();
00130   return 0;
00131 }
00132 


monocam_settler
Author(s): Vijay Pradeep
autogenerated on Sun Oct 5 2014 22:43:58