vision_detect_plug.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <actionlib/server/simple_action_server.h>
00003 #include <pr2_plugs_msgs/VisionPlugDetectionAction.h>
00004 #include <image_transport/image_transport.h>
00005 #include <tf/transform_listener.h>
00006 
00007 #include <checkerboard_pose_estimation/ros_detector.h>
00008 
00009 class DetectPlugAction
00010 {
00011 public:
00012   DetectPlugAction(const std::string& name)
00013     : it_(nh_),
00014       as_(nh_, name, false),
00015       name_(name)
00016   {
00017     // Load configuration
00018     if (!detector_.initFromParameters()) {
00019       ROS_FATAL("%s: One or more required parameters not set, shutting down", name_.c_str());
00020       nh_.shutdown();
00021       return;
00022     }
00023     
00024     // Register the goal and preempt callbacks
00025     as_.registerGoalCallback(boost::bind(&DetectPlugAction::goalCb, this));
00026     as_.registerPreemptCallback(boost::bind(&DetectPlugAction::preemptCb, this));
00027     as_.start();
00028     
00029     ROS_INFO("Finished registering callbacks");
00030   }
00031 
00032   void timeoutCb(const ros::TimerEvent& e)
00033   {
00034     if(sub_.getNumPublishers() == 0)
00035       ROS_INFO("%s: Aborted, there are no publishers on goal topic.", name_.c_str());    
00036     else
00037       ROS_INFO("%s: Aborted, there are publishers on goal topic, but detection took too long.", name_.c_str());    
00038 
00039     sub_.shutdown();
00040     as_.setAborted();
00041   }
00042 
00043   void goalCb()
00044   {
00045     ROS_INFO("%s: Received new goal", name_.c_str());
00046     
00047     // Accept the new goal
00048     typedef boost::shared_ptr<const pr2_plugs_msgs::VisionPlugDetectionGoal> GoalPtr;
00049     GoalPtr goal = as_.acceptNewGoal();
00051     typedef checkerboard_pose_estimation::Detector Side;
00052     detector_.getDetector().setOriginSide(goal->origin_on_right ? Side::RIGHT : Side::LEFT);
00053     tf::poseStampedMsgToTF(goal->prior, plug_prior_);
00054 
00055     // Subscribe to the streams from the requested camera
00056     ros::Duration(1.0).sleep();
00057     std::string image_topic = goal->camera_name + "/image_rect";
00058     sub_ = it_.subscribeCamera(image_topic, 1, &DetectPlugAction::detectCb, this);
00059 
00060     //create a timer for how long we can actually transform images before we have to abort
00061     pub_timer_ = nh_.createTimer(tf_listener_.getCacheLength() - ros::Duration(1.0), boost::bind(&DetectPlugAction::timeoutCb, this, _1), true);
00062   }
00063 
00064   void preemptCb()
00065   {
00066     ROS_INFO("%s: Preempted", name_.c_str());
00067     as_.setPreempted();
00068     pub_timer_.stop();
00069     sub_.shutdown();
00070   }
00071 
00072   void detectCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
00073   {
00074     if (!as_.isActive()) return;
00075     
00076     ROS_INFO("%s: Received image, performing detection", name_.c_str());
00077 
00078     tf::Stamped<tf::Pose> plug_pose;
00079     if (detector_.detectObject(image_msg, info_msg, plug_prior_, tf_listener_, plug_pose)) {
00080       // Report success
00081       pr2_plugs_msgs::VisionPlugDetectionResult result;
00082       tf::poseStampedTFToMsg(plug_pose, result.plug_pose);
00083       as_.setSucceeded(result);
00084       pub_timer_.stop();
00085       
00086       // Shut down the camera subscription and wait for the next goal
00087       sub_.shutdown();
00088     }
00089   }
00090 
00091 protected:
00092   ros::NodeHandle nh_;
00093   image_transport::ImageTransport it_;
00094   actionlib::SimpleActionServer<pr2_plugs_msgs::VisionPlugDetectionAction> as_;
00095   std::string name_;
00096   image_transport::CameraSubscriber sub_;
00097   tf::TransformListener tf_listener_;
00098   tf::Stamped<tf::Pose> plug_prior_;
00099   checkerboard_pose_estimation::RosDetector detector_;
00100   ros::Timer pub_timer_;
00101 };
00102 
00103 int main(int argc, char** argv)
00104 {
00105   ros::init(argc, argv, "vision_detect_plug");
00106   DetectPlugAction action(ros::this_node::getName());
00107   ros::spin();
00108 
00109   return 0;
00110 }


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13