find_fiducial_pose.cpp
Go to the documentation of this file.
00001 /*********************************************************************          
00002  * Software License Agreement (BSD License)                                     
00003  *                                                                              
00004  *  Copyright (c) 2008, Willow Garage, Inc.                                     
00005  *  All rights reserved.                                                        
00006  *                                                                              
00007  *  Redistribution and use in source and binary forms, with or without          
00008  *  modification, are permitted provided that the following conditions          
00009  *  are met:                                                                    
00010  *                                                                              
00011  *   * Redistributions of source code must retain the above copyright           
00012  *     notice, this list of conditions and the following disclaimer.            
00013  *   * Redistributions in binary form must reproduce the above                  
00014  *     copyright notice, this list of conditions and the following              
00015  *     disclaimer in the documentation and/or other materials provided          
00016  *     with the distribution.                                                   
00017  *   * Neither the name of Willow Garage nor the names of its                   
00018  *     contributors may be used to endorse or promote products derived          
00019  *     from this software without specific prior written permission.            
00020  *                                                                              
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS         
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT           
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS           
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE              
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,         
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,        
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;            
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER            
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT          
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN           
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE             
00032  *  POSSIBILITY OF SUCH DAMAGE.                                                 
00033  **********************************************************************/
00034 
00035 /* Author: Melonee Wise */
00036 
00037 #include <ros/ros.h>
00038 #include <actionlib/server/simple_action_server.h>
00039 #include <image_transport/image_transport.h>
00040 #include <image_geometry/pinhole_camera_model.h>
00041 #include <tf/transform_listener.h>
00042 #include <opencv2/core/core.hpp>
00043 #include <cv_bridge/cv_bridge.h>
00044 #include <Eigen/Dense>
00045 #include <Eigen/Geometry>
00046 #include <Eigen/StdVector>
00047 #include <tf/tf.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include "turtlebot_actions/FindFiducialAction.h"
00050 #include "turtlebot_actions/detect_calibration_pattern.h"
00051 
00052 class FindFiducialAction
00053 {
00054 public:
00055 
00056   FindFiducialAction(std::string name) :
00057     as_(nh_, name, true),
00058     action_name_(name),
00059     it_(nh_)
00060   {
00061     //register the goal and feeback callbacks                                   
00062     as_.registerGoalCallback(boost::bind(&FindFiducialAction::goalCB, this));
00063     as_.registerPreemptCallback(boost::bind(&FindFiducialAction::preemptCB, this));
00064 
00065   }
00066 
00067   ~FindFiducialAction(void)
00068   {
00069   }
00070 
00071   void goalCB()
00072   {
00073     ROS_INFO("%s: Received new goal", action_name_.c_str());
00074 
00075     typedef boost::shared_ptr<const turtlebot_actions::FindFiducialGoal> GoalPtr;
00076     GoalPtr goal = as_.acceptNewGoal();
00077 
00078     cv::Size grid_size(goal->pattern_width,goal->pattern_height);
00079     detector_.setPattern(grid_size, goal->pattern_size, Pattern(goal->pattern_type));
00080 
00081     ros::Duration(1.0).sleep();
00082     //subscribe to the image topic of interest
00083     std::string image_topic = goal->camera_name + "/image_rect_mono";
00084     sub_ = it_.subscribeCamera(image_topic, 1, &FindFiducialAction::detectCB, this);
00085 
00086     pub_timer_ = nh_.createTimer(tf_listener_.getCacheLength() - ros::Duration(1.0), boost::bind(&FindFiducialAction::timeoutCB, this, _1),true);
00087   }
00088 
00089   void timeoutCB(const ros::TimerEvent& e)
00090   {
00091     if(sub_.getNumPublishers() == 0)
00092       ROS_INFO("%s: Aborted, there are no publishers on goal topic.", action_name_.c_str());    
00093     else
00094       ROS_INFO("%s: Aborted, there are publishers on goal topic, but detection took too long.", action_name_.c_str());    
00095     
00096     sub_.shutdown();
00097     as_.setAborted();
00098   }
00099 
00100   void preemptCB()
00101   {
00102     ROS_INFO("%s: Preempted", action_name_.c_str());
00103     // set the action state to preempted                                        
00104     pub_timer_.stop();
00105     as_.setPreempted();
00106     sub_.shutdown();
00107   }
00108 
00109   void detectCB(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
00110   {
00111     // make sure that the action is active                            
00112     if (!as_.isActive())
00113       return;
00114 
00115     ROS_INFO("%s: Received image, performing detection", action_name_.c_str());
00116     // Convert image message
00117 
00118     try
00119     {
00120       img_bridge_ = cv_bridge::toCvCopy(image_msg, "mono8");
00121     }
00122     catch (cv_bridge::Exception& ex)
00123     {
00124       ROS_ERROR("[calibrate] Failed to convert image");
00125       return;
00126     }
00127 
00128 
00129     ROS_INFO("%s: created cv::Mat", action_name_.c_str());
00130     cam_model_.fromCameraInfo(info_msg);
00131     detector_.setCameraMatrices(cam_model_.intrinsicMatrix(), cam_model_.distortionCoeffs());
00132 
00133     Eigen::Vector3f translation;
00134     Eigen::Quaternionf orientation;
00135 
00136     if (detector_.detectPattern(img_bridge_->image, translation, orientation))
00137     {
00138       ROS_INFO("detected fiducial");
00139       tf::Transform fiducial_transform;
00140       fiducial_transform.setOrigin( tf::Vector3(translation.x(), translation.y(), translation.z()) );
00141       fiducial_transform.setRotation( tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()) );
00142 
00143       tf::StampedTransform fiducial_transform_stamped(fiducial_transform, image_msg->header.stamp, cam_model_.tfFrame(), "fiducial_frame");
00144       tf_broadcaster_.sendTransform(fiducial_transform_stamped);
00145 
00146       tf::Pose fiducial_pose;
00147       fiducial_pose.setRotation(tf::Quaternion(0, 0, 0, 1.0));
00148       fiducial_pose = fiducial_transform * fiducial_pose;
00149       tf::Stamped<tf::Pose> fiducial_pose_stamped(fiducial_pose, image_msg->header.stamp, cam_model_.tfFrame());
00150 
00151       tf::poseStampedTFToMsg(fiducial_pose_stamped, result_.pose);
00152       as_.setSucceeded(result_);
00153       pub_timer_.stop();
00154       sub_.shutdown();
00155     }
00156 
00157   }
00158 
00159 protected:
00160 
00161   ros::NodeHandle nh_;
00162   actionlib::SimpleActionServer<turtlebot_actions::FindFiducialAction> as_;
00163   std::string action_name_;
00164   PatternDetector detector_;
00165 
00166   image_transport::ImageTransport it_;
00167   image_transport::CameraSubscriber sub_;
00168   cv_bridge::CvImagePtr img_bridge_;
00169   image_geometry::PinholeCameraModel cam_model_;
00170 
00171   tf::TransformListener tf_listener_;
00172   tf::TransformBroadcaster tf_broadcaster_;
00173 
00174   ros::Timer pub_timer_;
00175 
00176   // create messages that are used to published feedback/result                 
00177   turtlebot_actions::FindFiducialFeedback feedback_;
00178   turtlebot_actions::FindFiducialResult result_;
00179 
00180 
00181 };
00182 
00183 
00184 int main(int argc, char** argv)
00185 {
00186   ros::init(argc, argv, "find_fiducial_pose");
00187 
00188   FindFiducialAction find_fiducial(ros::this_node::getName());
00189   ros::spin();
00190 
00191   return 0;
00192 }
00193 
00194 
00195 


turtlebot_actions
Author(s): Helen Oleynikova, Melonee Wise
autogenerated on Wed Aug 26 2015 16:34:28