monocam_settler_action.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, 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/ros.h>
41 
42 // Messages
43 #include <monocam_settler/ConfigAction.h>
44 #include <calibration_msgs/CalibrationPattern.h>
45 #include <calibration_msgs/Interval.h>
46 
47 using namespace monocam_settler;
48 
50 {
51 public:
52  MonocamSettlerAction() : as_("monocam_settler_config", false)
53  {
54  // Set up action callbacks
55  as_.registerGoalCallback( boost::bind(&MonocamSettlerAction::goalCallback, this) );
56  as_.registerPreemptCallback( boost::bind(&MonocamSettlerAction::preemptCallback, this) );
57 
58  pub_ = nh_.advertise<calibration_msgs::Interval>("settled_interval", 1);
59  sub_ = nh_.subscribe("features", 1, &MonocamSettlerAction::msgCallback, this);
60  as_.start();
61  }
62 
63  void goalCallback()
64  {
65  boost::mutex::scoped_lock lock(run_mutex_);
66 
67  // Stop the previously running goal (if it exists)
68  if (as_.isActive())
69  as_.setPreempted();
70 
71  // Get the new goal from the action server
72  monocam_settler::ConfigGoalConstPtr goal = as_.acceptNewGoal();
73  assert(goal);
74 
75  // Try to reconfigure the settler object
76  bool success = settler_.configure(*goal);
77 
78  // Detect possible reconfigure failure
79  if (!success)
80  as_.setAborted();
81  }
82 
84  {
85  boost::mutex::scoped_lock lock(run_mutex_);
86  as_.setPreempted();
87  }
88 
89  void msgCallback(const calibration_msgs::CalibrationPatternConstPtr& msg)
90  {
91  boost::mutex::scoped_lock lock(run_mutex_);
92 
93  if (as_.isActive())
94  {
95  bool success;
96 
97  calibration_msgs::Interval interval;
98  success = settler_.add(msg, interval);
99 
100  if (success)
101  pub_.publish(interval);
102  else
103  {
104  // Publish 'null' interval if we didn't find a checkerboard
105  interval.start = msg->header.stamp;
106  interval.end = msg->header.stamp;
107  pub_.publish(interval);
108  }
109  }
110  else
111  ROS_DEBUG("Got a feature msg, but not doing anything with it. No active goal, so node is currently idle");
112  }
113 
114 private:
119 
120  boost::mutex run_mutex_;
122 };
123 
124 int main(int argc, char** argv)
125 {
126  ros::init(argc, argv, "joint_states_settler_action");
127  ros::NodeHandle n;
128  MonocamSettlerAction settler_action;
129  ros::spin();
130  return 0;
131 }
132 
static const bool success[N]
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ROSCPP_DECL void spin()
void msgCallback(const calibration_msgs::CalibrationPatternConstPtr &msg)
actionlib::SimpleActionServer< monocam_settler::ConfigAction > as_
#define ROS_DEBUG(...)


monocam_settler
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 19:17:30