interval_intersection_action.cpp
Go to the documentation of this file.
1 /**********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #include <vector>
37 #include <boost/scoped_ptr.hpp>
38 #include <ros/ros.h>
39 #include <ros/console.h>
42 
43 #include <calibration_msgs/Interval.h>
44 #include <calibration_msgs/IntervalStatus.h>
45 #include <interval_intersection/ConfigAction.h>
46 
47 using namespace std;
48 using namespace interval_intersection;
49 
51 {
52 public:
53  IntervalIntersectionAction() : as_("interval_intersection_config", false)
54  {
55  as_.registerGoalCallback( boost::bind(&IntervalIntersectionAction::goalCallback, this) );
56  as_.registerPreemptCallback( boost::bind(&IntervalIntersectionAction::preemptCallback, this) );
57  pub_ = nh_.advertise<calibration_msgs::Interval>("intersected_interval", 1);
58  status_pub_ = nh_.advertise<calibration_msgs::IntervalStatus>(
59  "intersected_interval_status", 1);
60 
61  // set up our status timer
62  status_timer_ = nh_.createTimer(ros::Duration(1.0),
63  boost::bind(&IntervalIntersectionAction::publishStatus, this));
64 
65  ROS_DEBUG("Start interval intersection with no input topics");
66  as_.start();
67  }
68 
69  void goalCallback()
70  {
71  boost::mutex::scoped_lock lock(run_mutex_);
72 
73  // Stop currently running goal, if there's a goal running
74  if (as_.isActive())
75  {
76  tearDown();
77  as_.setPreempted();
78  }
79 
80  // Get the new goal from the action server
81  interval_intersection::ConfigGoalConstPtr goal = as_.acceptNewGoal();
82  assert(goal);
83 
84  // Reconfigure the node
85  intersect_nh_.reset(new ros::NodeHandle);
86  intersect_.reset(new IntervalIntersector( boost::bind(&IntervalIntersectionAction::publishResult, this, _1)));
87  // Subscribe to all the requested topics
88  subscribers_.resize(goal->topics.size());
89  topics_.resize(goal->topics.size());
90  for (unsigned int i=0; i<goal->topics.size(); i++)
91  {
92  ROS_DEBUG("Subscribing to: %s", goal->topics[i].c_str());
93  subscribers_[i] = intersect_nh_->subscribe<calibration_msgs::Interval>(goal->topics[i], 200, intersect_->getNewInputStream());
94  topics_[i] = goal->topics[i];
95  }
96  }
97 
99  {
100  boost::mutex::scoped_lock lock(run_mutex_);
101  tearDown();
102  as_.setPreempted();
103  }
104 
105  void tearDown()
106  {
107  assert(intersect_);
108  assert(intersect_nh_);
109 
110  // Shutdown the subnode, to ensure that none of the callbacks are called
111  ROS_DEBUG("Shutting Down IntervalIntersection");
112  intersect_nh_->shutdown();
113 
114  subscribers_.clear();
115  intersect_.reset();
116  intersect_nh_.reset();
117  }
118 
119  void publishResult(calibration_msgs::Interval interval)
120  {
121  ROS_DEBUG("Publishing");
122  pub_.publish(interval);
123  }
124 
125  void publishStatus() {
126  ROS_DEBUG("Publishing interval intersection status");
127  if ( intersect_ ) {
128  // get status from intersector
129  calibration_msgs::IntervalStatus status = intersect_->get_status();
130  // fill in timestamp
131  status.header.stamp = ros::Time::now();
132  // fill in topic names
133  for ( size_t i=0; i<status.yeild_rates.size(); ++i ) {
134  status.names[i] = topics_[i];
135  ROS_DEBUG("Topic %s has a success rate of %f",
136  status.names[i].c_str(), status.yeild_rates[i]);
137  }
138  // publish!
139  status_pub_.publish(status);
140  }
141  }
142 
143 private:
144  boost::mutex run_mutex_;
146 
147  boost::scoped_ptr<ros::NodeHandle> intersect_nh_;
148  boost::scoped_ptr<IntervalIntersector> intersect_;
149  vector<ros::Subscriber> subscribers_;
150  vector<std::string> topics_;
151 
156 };
157 
158 int main(int argc, char** argv)
159 {
160  ros::init(argc, argv, "intersection_action_node");
161  IntervalIntersectionAction intersect_action;
162  ros::spin();
163  return 0;
164 }
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
boost::scoped_ptr< IntervalIntersector > intersect_
actionlib::SimpleActionServer< interval_intersection::ConfigAction > as_
boost::scoped_ptr< ros::NodeHandle > intersect_nh_
ROSCPP_DECL void spin()
static Time now()
void publishResult(calibration_msgs::Interval interval)
#define ROS_DEBUG(...)


interval_intersection
Author(s): Romain Thibaux
autogenerated on Mon Feb 28 2022 22:03:20