00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <vector>
00037 #include <boost/scoped_ptr.hpp>
00038 #include <ros/ros.h>
00039 #include <ros/console.h>
00040 #include <actionlib/server/simple_action_server.h>
00041 #include <interval_intersection/interval_intersection.hpp>
00042
00043 #include <calibration_msgs/Interval.h>
00044 #include <interval_intersection/ConfigAction.h>
00045
00046 using namespace std;
00047 using namespace interval_intersection;
00048
00049 class IntervalIntersectionAction
00050 {
00051 public:
00052 IntervalIntersectionAction() : as_("interval_intersection_config", false)
00053 {
00054 as_.registerGoalCallback( boost::bind(&IntervalIntersectionAction::goalCallback, this) );
00055 as_.registerPreemptCallback( boost::bind(&IntervalIntersectionAction::preemptCallback, this) );
00056 pub_ = nh_.advertise<calibration_msgs::Interval>("interval", 1);
00057
00058 ROS_DEBUG("Start interval intersection with no input topics");
00059 as_.start();
00060 }
00061
00062 void goalCallback()
00063 {
00064 boost::mutex::scoped_lock lock(run_mutex_);
00065
00066
00067 if (as_.isActive())
00068 {
00069 tearDown();
00070 as_.setPreempted();
00071 }
00072
00073
00074 interval_intersection::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00075 assert(goal);
00076
00077
00078 intersect_nh_.reset(new ros::NodeHandle);
00079 intersect_.reset(new IntervalIntersector( boost::bind(&IntervalIntersectionAction::publishResult, this, _1)));
00080
00081 subscribers_.resize(goal->topics.size());
00082 for (unsigned int i=0; i<goal->topics.size(); i++)
00083 {
00084 ROS_DEBUG("Subscribing to: %s", goal->topics[i].c_str());
00085 subscribers_[i] = intersect_nh_->subscribe<calibration_msgs::Interval>(goal->topics[i], 200, intersect_->getNewInputStream());
00086 }
00087 }
00088
00089 void preemptCallback()
00090 {
00091 boost::mutex::scoped_lock lock(run_mutex_);
00092 tearDown();
00093 as_.setPreempted();
00094 }
00095
00096 void tearDown()
00097 {
00098 assert(intersect_);
00099 assert(intersect_nh_);
00100
00101
00102 ROS_DEBUG("Shutting Down IntervalIntersection");
00103 intersect_nh_->shutdown();
00104
00105 subscribers_.clear();
00106 intersect_.reset();
00107 intersect_nh_.reset();
00108 }
00109
00110 void publishResult(calibration_msgs::Interval interval)
00111 {
00112 ROS_DEBUG("Publishing");
00113 pub_.publish(interval);
00114 }
00115
00116 private:
00117 boost::mutex run_mutex_;
00118 actionlib::SimpleActionServer<interval_intersection::ConfigAction> as_;
00119
00120 boost::scoped_ptr<ros::NodeHandle> intersect_nh_;
00121 boost::scoped_ptr<IntervalIntersector> intersect_;
00122 vector<ros::Subscriber> subscribers_;
00123
00124 ros::Publisher pub_;
00125 ros::NodeHandle nh_;
00126 };
00127
00128 int main(int argc, char** argv)
00129 {
00130 ros::init(argc, argv, "intersection_action_node");
00131
00132 ros::NodeHandle n;
00133
00134 IntervalIntersectionAction intersect_action;
00135
00136 ros::spin();
00137
00138 return 0;
00139 }