find_fiducial_pose.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  **********************************************************************/
34 
35 /* Author: Melonee Wise */
36 
37 #include <ros/ros.h>
41 #include <tf/transform_listener.h>
42 #include <opencv2/core/core.hpp>
43 #include <cv_bridge/cv_bridge.h>
44 #include <Eigen/Dense>
45 #include <Eigen/Geometry>
46 #include <Eigen/StdVector>
47 #include <tf/tf.h>
49 #include "turtlebot_actions/FindFiducialAction.h"
51 
53 {
54 public:
55 
56  FindFiducialAction(std::string name) :
57  as_(nh_, name, true),
58  action_name_(name),
59  it_(nh_)
60  {
61  //register the goal and feeback callbacks
64 
65  }
66 
68  {
69  }
70 
71  void goalCB()
72  {
73  ROS_INFO("%s: Received new goal", action_name_.c_str());
74 
76  GoalPtr goal = as_.acceptNewGoal();
77 
78  cv::Size grid_size(goal->pattern_width,goal->pattern_height);
79  detector_.setPattern(grid_size, goal->pattern_size, Pattern(goal->pattern_type));
80 
81  ros::Duration(1.0).sleep();
82  //subscribe to the image topic of interest
83  std::string image_topic = goal->camera_name + "/image_rect_mono";
84  sub_ = it_.subscribeCamera(image_topic, 1, &FindFiducialAction::detectCB, this);
85 
87  }
88 
89  void timeoutCB(const ros::TimerEvent& e)
90  {
91  if(sub_.getNumPublishers() == 0)
92  ROS_INFO("%s: Aborted, there are no publishers on goal topic.", action_name_.c_str());
93  else
94  ROS_INFO("%s: Aborted, there are publishers on goal topic, but detection took too long.", action_name_.c_str());
95 
96  sub_.shutdown();
97  as_.setAborted();
98  }
99 
100  void preemptCB()
101  {
102  ROS_INFO("%s: Preempted", action_name_.c_str());
103  // set the action state to preempted
104  pub_timer_.stop();
105  as_.setPreempted();
106  sub_.shutdown();
107  }
108 
109  void detectCB(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
110  {
111  // make sure that the action is active
112  if (!as_.isActive())
113  return;
114 
115  ROS_INFO("%s: Received image, performing detection", action_name_.c_str());
116  // Convert image message
117 
118  try
119  {
120  img_bridge_ = cv_bridge::toCvCopy(image_msg, "mono8");
121  }
122  catch (cv_bridge::Exception& ex)
123  {
124  ROS_ERROR("[calibrate] Failed to convert image");
125  return;
126  }
127 
128 
129  ROS_INFO("%s: created cv::Mat", action_name_.c_str());
130  cam_model_.fromCameraInfo(info_msg);
132 
133  Eigen::Vector3f translation;
134  Eigen::Quaternionf orientation;
135 
136  if (detector_.detectPattern(img_bridge_->image, translation, orientation))
137  {
138  ROS_INFO("detected fiducial");
139  tf::Transform fiducial_transform;
140  fiducial_transform.setOrigin( tf::Vector3(translation.x(), translation.y(), translation.z()) );
141  fiducial_transform.setRotation( tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()) );
142 
143  tf::StampedTransform fiducial_transform_stamped(fiducial_transform, image_msg->header.stamp, cam_model_.tfFrame(), "fiducial_frame");
144  tf_broadcaster_.sendTransform(fiducial_transform_stamped);
145 
146  tf::Pose fiducial_pose;
147  fiducial_pose.setRotation(tf::Quaternion(0, 0, 0, 1.0));
148  fiducial_pose = fiducial_transform * fiducial_pose;
149  tf::Stamped<tf::Pose> fiducial_pose_stamped(fiducial_pose, image_msg->header.stamp, cam_model_.tfFrame());
150 
151  tf::poseStampedTFToMsg(fiducial_pose_stamped, result_.pose);
153  pub_timer_.stop();
154  sub_.shutdown();
155  }
156 
157  }
158 
159 protected:
160 
163  std::string action_name_;
165 
170 
173 
175 
176  // create messages that are used to published feedback/result
177  turtlebot_actions::FindFiducialFeedback feedback_;
178  turtlebot_actions::FindFiducialResult result_;
179 
180 
181 };
182 
183 
184 int main(int argc, char** argv)
185 {
186  ros::init(argc, argv, "find_fiducial_pose");
187 
189  ros::spin();
190 
191  return 0;
192 }
193 
194 
195 
ros::Duration getCacheLength()
const cv::Matx33d & intrinsicMatrix() const
boost::shared_ptr< const Goal > acceptNewGoal()
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
void detectCB(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
cv_bridge::CvImagePtr img_bridge_
CameraSubscriber subscribeCamera(const std::string &base_topic, uint32_t queue_size, const CameraSubscriber::Callback &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
turtlebot_actions::FindFiducialResult result_
bool sleep() const
void stop()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
PatternDetector detector_
ROSCPP_DECL const std::string & getName()
tf::TransformListener tf_listener_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL void spin(Spinner &spinner)
image_transport::CameraSubscriber sub_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
void setPattern(cv::Size grid_size_, float square_size_, Pattern pattern_type_, cv::Point3f offset_=cv::Point3f())
#define ROS_INFO(...)
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void registerPreemptCallback(boost::function< void()> cb)
void sendTransform(const StampedTransform &transform)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
image_geometry::PinholeCameraModel cam_model_
void setCameraMatrices(cv::Matx33d K_, cv::Matx33d D_)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
image_transport::ImageTransport it_
tf::TransformBroadcaster tf_broadcaster_
GLuint const GLchar * name
actionlib::SimpleActionServer< turtlebot_actions::FindFiducialAction > as_
uint32_t getNumPublishers() const
FindFiducialAction(std::string name)
const cv::Mat_< double > & distortionCoeffs() const
int main(int argc, char **argv)
void timeoutCB(const ros::TimerEvent &e)
void registerGoalCallback(boost::function< void()> cb)
#define ROS_ERROR(...)
int detectPattern(cv::Mat &inm, Eigen::Vector3f &translation, Eigen::Quaternionf &orientation)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
turtlebot_actions::FindFiducialFeedback feedback_


turtlebot_actions
Author(s): Helen Oleynikova, Melonee Wise
autogenerated on Mon Jun 10 2019 15:43:57