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
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),
00058 action_name_(name),
00059 it_(nh_)
00060 {
00061
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
00083 std::string image_topic = goal->camera_name + "/image_rect";
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
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
00112 if (!as_.isActive())
00113 return;
00114
00115 ROS_INFO("%s: Received image, performing detection", action_name_.c_str());
00116
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
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