vision_detect_outlet.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/VisionOutletDetectionAction.h>
00004 
00005 #include <cv_bridge/cv_bridge.h>
00006 #include <image_transport/image_transport.h>
00007 #include <image_geometry/pinhole_camera_model.h>
00008 #include <tf/transform_listener.h>
00009 #include <tf/transform_broadcaster.h>
00010 
00011 #include <outlet_pose_estimation/detector.h>
00012 #include <outlet_pose_estimation/estimator.h>
00013 
00014 using namespace outlet_pose_estimation;
00015 using namespace visual_pose_estimation;
00016 
00017 bool initFromParameters(Detector& detector, PoseEstimator& estimator)
00018 {
00019   // Read all the parameters
00020   ros::NodeHandle local_nh("~");
00021   bool have_all_required = true;
00022 
00023   std::string outlet_template_path;
00024   if (!local_nh.getParam("outlet_template", outlet_template_path)) {
00025     ROS_ERROR("Outlet template [~outlet_template] unspecified");
00026     have_all_required = false;
00027   }
00028 
00029   if (!have_all_required) return false;
00030 
00031   // Initialize detector
00032   if (!detector.loadTemplate(outlet_template_path)) {
00033     ROS_ERROR("Could not load outlet template from file %s", outlet_template_path.c_str());
00034     return false;
00035   }
00036 
00037   // Initialize pose estimator with outlet object model
00038   estimator = createOutletEstimator(detector.getTemplate());
00039 
00040   return true;
00041 }
00042 
00043 class DetectOutletAction
00044 {
00045 public:
00046   DetectOutletAction(const std::string& name)
00047     : it_(nh_),
00048       as_(nh_, name, false),
00049       action_name_(name),
00050       update_transformed_prior_(true)
00051   {
00052     // Load configuration
00053     if (!initFromParameters(detector_, pose_estimator_)) {
00054       ROS_FATAL("%s: Error initializing from parameters, shutting down", action_name_.c_str());
00055       nh_.shutdown();
00056       return;
00057     }
00058 
00059     // Advertise visualization topics
00060     display_pub_ = it_.advertise("display_image", 1);
00061     //marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
00062     
00063     // Register the goal and preempt callbacks
00064     as_.registerGoalCallback(boost::bind(&DetectOutletAction::goalCb, this));
00065     as_.registerPreemptCallback(boost::bind(&DetectOutletAction::preemptCb, this));
00066 
00067     as_.start();
00068   }
00069 
00070   void timeoutCb(const ros::TimerEvent& e)
00071   {
00072     if(sub_.getNumPublishers() == 0)
00073       ROS_INFO("%s: Aborted, there are no publishers on goal topic.", action_name_.c_str());    
00074     else
00075       ROS_INFO("%s: Aborted, there are publishers on goal topic, but detection took too long.", action_name_.c_str());    
00076 
00077     sub_.shutdown();
00078     as_.setAborted();
00079   }
00080 
00081   void goalCb()
00082   {
00083     ROS_INFO("%s: Received new goal", action_name_.c_str());
00084     // Accept the new goal
00085     typedef boost::shared_ptr<const pr2_plugs_msgs::VisionOutletDetectionGoal> GoalPtr;
00086     GoalPtr goal = as_.acceptNewGoal();
00088     tf::poseStampedMsgToTF(goal->prior, prior_);
00089     tf::vector3StampedMsgToTF(goal->wall_normal, wall_normal_);
00090     update_transformed_prior_ = true;
00091 
00092     // Subscribe to the streams from the requested camera
00093     ros::Duration(1.0).sleep();
00094     std::string image_topic = goal->camera_name + "/image_rect_color";
00095     sub_ = it_.subscribeCamera(image_topic, 1, &DetectOutletAction::detectCb, this);
00096 
00097     //create a timer for how long we can actually transform images before we have to abort
00098     pub_timer_ = nh_.createTimer(tf_listener_.getCacheLength() - ros::Duration(1.0), boost::bind(&DetectOutletAction::timeoutCb, this, _1), true);
00099   }
00100 
00101   void preemptCb()
00102   {
00103     ROS_INFO("%s: Preempted", action_name_.c_str());
00104     as_.setPreempted();
00105     pub_timer_.stop();
00106     sub_.shutdown();
00107   }
00108 
00109   void detectCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
00110   {
00111     if (!as_.isActive()) return;
00112     
00113     ROS_INFO("%s: Received image, performing detection", action_name_.c_str());
00114 
00115     // Convert image message
00117     cv::Mat image = cv_bridge::toCvShare(image_msg, "bgr8")->image;
00118 
00119     // Detect the outlet
00120     std::vector<cv::Point2f> image_points;
00121     if (!detector_.detect(image, image_points)) {
00122       ROS_DEBUG("%s: Failed to detect outlet", action_name_.c_str());
00123       publishDisplayImage(image, image_points, false);
00125       return;
00126     }
00127 
00128     // Get the prior in the camera frame. Currently we transform the prior once, on the first
00129     // image received for a new goal, and assume that the camera frame does not move wrt to
00130     // the original frame of the prior while that goal is active.
00131     cam_model_.fromCameraInfo(info_msg);
00132     if (update_transformed_prior_) {
00133       // Transform the prior and wall normal into the camera coordinate frame.
00134       tf::Stamped<tf::Vector3> wall_normal_in_camera;
00135       try {
00137         tf_listener_.transformPose(cam_model_.tfFrame(), prior_, prior_in_camera_);
00138         tf_listener_.transformVector(cam_model_.tfFrame(), wall_normal_, wall_normal_in_camera);
00139       }
00140       catch (tf::TransformException& ex) {
00141         ROS_WARN("%s: TF exception\n%s", action_name_.c_str(), ex.what());
00142         return;
00143       }
00144 
00145       // Now we tweak the orientation of the original prior to align with the wall normal.
00146       // First pull out a couple prior basis vectors of interest.
00147       tf::Vector3 prior_x_basis = prior_in_camera_.getBasis().getColumn(0);
00148       tf::Vector3 prior_z_basis = prior_in_camera_.getBasis().getColumn(2);
00149       // New forward vector is set to the wall normal.
00150       tf::Vector3 adjusted_x_basis = wall_normal_in_camera.normalized();
00151       // We assume normal is into the wall, so positive Z in the camera frame.
00152       if (adjusted_x_basis.z() < 0)
00153         adjusted_x_basis = -adjusted_x_basis;
00154       // The wall normal should not differ wildly from the prior X (forward) basis vector.
00155       static const double ANGLE_THRESHOLD = 0.175; // ~10 degrees
00156       double angle = adjusted_x_basis.angle(prior_x_basis);
00157       if (angle > ANGLE_THRESHOLD) {
00158         ROS_ERROR("Wall normal differs from prior orientation by %f radians, threshold is %f. Aborting.",
00159                   angle, ANGLE_THRESHOLD);
00160         as_.setAborted();
00161         pub_timer_.stop();
00162         sub_.shutdown();
00163         return;
00164       }
00165       // New left vector calculated as cross product of the original up vector and the wall normal.
00166       tf::Vector3 adjusted_y_basis = prior_z_basis.cross(adjusted_x_basis).normalized();
00167       // New up vector calculated from the other two basis vectors.
00168       tf::Vector3 adjusted_z_basis = adjusted_x_basis.cross(adjusted_y_basis);
00169       // Fill in the new basis vectors as columns.
00170       prior_in_camera_.getBasis().setValue(adjusted_x_basis.x(), adjusted_y_basis.x(), adjusted_z_basis.x(),
00171                                            adjusted_x_basis.y(), adjusted_y_basis.y(), adjusted_z_basis.y(),
00172                                            adjusted_x_basis.z(), adjusted_y_basis.z(), adjusted_z_basis.z());
00173 
00174       update_transformed_prior_ = false;
00175     }
00176 
00177     // Estimate the outlet pose
00178     tf::Pose pose = pose_estimator_.solveWithPrior(image_points, cam_model_, prior_in_camera_);
00179 
00180     // Report success
00181     pr2_plugs_msgs::VisionOutletDetectionResult result;
00182     tf::poseTFToMsg(pose, result.outlet_pose.pose);
00183     result.outlet_pose.header.stamp = image_msg->header.stamp;
00184     result.outlet_pose.header.frame_id = cam_model_.tfFrame();
00185     as_.setSucceeded(result);
00186     pub_timer_.stop();
00187 
00188     // Publish visualization messages
00189     tf_broadcaster_.sendTransform(tf::StampedTransform(prior_, prior_.stamp_,
00190                                                        prior_.frame_id_, "outlet_prior_frame"));
00191     tf_broadcaster_.sendTransform(tf::StampedTransform(prior_in_camera_, prior_in_camera_.stamp_,
00192                                                        prior_in_camera_.frame_id_,
00193                                                        "outlet_adjusted_prior_frame"));
00194     tf_broadcaster_.sendTransform(tf::StampedTransform(pose, image_msg->header.stamp,
00195                                                        cam_model_.tfFrame(), "outlet_frame"));
00196     publishDisplayImage(image, image_points, true);
00197 
00198     // Shut down the camera subscription and wait for the next goal
00199     sub_.shutdown();
00200   }
00201 
00202   void publishDisplayImage(const cv::Mat& source, const std::vector<cv::Point2f>& image_points, bool success)
00203   {
00204     if (display_pub_.getNumSubscribers() == 0) return;
00205     detector_.getDisplayImage(source, image_points, success, display_img_cv_);
00206     cv_bridge::CvImage cv_image(std_msgs::Header(), "bgr8", display_img_cv_);
00207     display_pub_.publish(*(cv_image.toImageMsg()));
00208   }
00209 
00210 protected:
00211   ros::NodeHandle nh_;
00212   image_transport::ImageTransport it_;
00213   actionlib::SimpleActionServer<pr2_plugs_msgs::VisionOutletDetectionAction> as_;
00214   std::string action_name_;
00215 
00216   // Subscriptions
00217   image_transport::CameraSubscriber sub_;
00218 
00219   ros::Timer pub_timer_;
00220 
00221   // Publications
00222   image_transport::Publisher display_pub_;
00223   sensor_msgs::Image display_img_;
00224   cv::Mat display_img_cv_;
00225   ros::Publisher marker_pub_;
00226 
00227   // TF
00228   tf::TransformListener tf_listener_;
00229   tf::TransformBroadcaster tf_broadcaster_;
00230 
00231   // Message processing
00232   image_geometry::PinholeCameraModel cam_model_;
00233   outlet_pose_estimation::Detector detector_;
00234   visual_pose_estimation::PoseEstimator pose_estimator_;
00235   tf::Stamped<tf::Pose> prior_;
00236   tf::Stamped<tf::Pose> prior_in_camera_;
00237   tf::Stamped<tf::Vector3> wall_normal_;
00238   bool update_transformed_prior_;
00239 };
00240 
00241 int main(int argc, char** argv)
00242 {
00243   ros::init(argc, argv, "vision_detect_outlet");
00244   DetectOutletAction action(ros::this_node::getName());
00245   ros::spin();
00246 
00247   return 0;
00248 }


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