image_cb_detector_action.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009-2012, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 
37 #include <boost/thread.hpp>
38 #include <ros/console.h>
39 #include <ros/ros.h>
43 #include <sensor_msgs/Image.h>
44 #include <image_cb_detector/ConfigAction.h>
45 #include <calibration_msgs/Interval.h>
46 
47 using namespace image_cb_detector;
48 
50 {
51 public:
52  ImageCbDetectorAction() : as_("cb_detector_config", false), it_(nh_)
53  {
54  as_.registerGoalCallback( boost::bind(&ImageCbDetectorAction::goalCallback, this) );
55  as_.registerPreemptCallback( boost::bind(&ImageCbDetectorAction::preemptCallback, this) );
56 
57  pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("features",1);
58  sub_ = it_.subscribe("image", 2, boost::bind(&ImageCbDetectorAction::imageCallback, this, _1));
59  as_.start();
60  }
61 
62  void goalCallback()
63  {
64  boost::mutex::scoped_lock lock(run_mutex_);
65 
66  // Stop the previously running goal (if it exists)
67  if (as_.isActive())
68  as_.setPreempted();
69 
70  // Get the new goal from the action server
71  image_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
72  assert(goal);
73 
74  // Try to reconfigure the settler object
75  bool success = detector_.configure(*goal);
76 
77  // Detect possible failure
78  if (!success)
79  as_.setAborted();
80  }
81 
83  {
84  boost::mutex::scoped_lock lock(run_mutex_);
85 
86  // Don't need to do any cleanup. Immeadiately turn it off
87  as_.setPreempted();
88  }
89 
90  void imageCallback(const sensor_msgs::ImageConstPtr& image)
91  {
92  boost::mutex::scoped_lock lock(run_mutex_);
93 
94  if (as_.isActive())
95  {
96  calibration_msgs::CalibrationPattern features;
97  bool success;
98  success = detector_.detect(image, features);
99 
100  if (!success)
101  {
102  ROS_ERROR("Error trying to detect checkerboard, not going to publish CalibrationPattern");
103  return;
104  }
105 
106  pub_.publish(features);
107  }
108  }
109 
110 private:
111  boost::mutex run_mutex_;
115 
119 
120 };
121 
122 int main(int argc, char** argv)
123 {
124  ros::init(argc, argv, "image_cb_detector_action");
125  ros::NodeHandle n;
126  ImageCbDetectorAction detector_action;
127  ros::spin();
128  return 0;
129 }
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
image_transport::ImageTransport it_
ROSCPP_DECL void spin(Spinner &spinner)
actionlib::SimpleActionServer< image_cb_detector::ConfigAction > as_
image_transport::Subscriber sub_
#define ROS_ERROR(...)
void imageCallback(const sensor_msgs::ImageConstPtr &image)


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Fri Apr 2 2021 02:12:56