aruco_ros/src/simple_single.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
36 #include <iostream>
37 #include <aruco/aruco.h>
38 #include <aruco/cvdrawingutils.h>
39 
40 #include <ros/ros.h>
42 #include <cv_bridge/cv_bridge.h>
44 #include <aruco_ros/aruco_ros_utils.h>
46 #include <tf/transform_listener.h>
47 #include <visualization_msgs/Marker.h>
48 
49 #include <dynamic_reconfigure/server.h>
50 #include <aruco_ros/ArucoThresholdConfig.h>
51 using namespace aruco;
52 
54 {
55 private:
56  cv::Mat inImage;
61  vector<Marker> markers;
69  ros::Publisher marker_pub; //rviz visualization marker
71  std::string marker_frame;
72  std::string camera_frame;
73  std::string reference_frame;
74 
75  double marker_size;
76  int marker_id;
78 
82 
84 
85  dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig> dyn_rec_server;
86 
87 public:
89  : cam_info_received(false),
90  nh("~"),
91  it(nh)
92  {
93 
94  std::string refinementMethod;
95  nh.param<std::string>("corner_refinement", refinementMethod, "LINES");
96  if ( refinementMethod == "SUBPIX" )
98  else if ( refinementMethod == "HARRIS" )
100  else if ( refinementMethod == "NONE" )
102  else
104 
105  //Print parameters of aruco marker detector:
106  ROS_INFO_STREAM("Corner refinement method: " << mDetector.getCornerRefinementMethod());
107  ROS_INFO_STREAM("Threshold method: " << mDetector.getThresholdMethod());
108  double th1, th2;
109  mDetector.getThresholdParams(th1, th2);
110  ROS_INFO_STREAM("Threshold method: " << " th1: " << th1 << " th2: " << th2);
111  float mins, maxs;
112  mDetector.getMinMaxSize(mins, maxs);
113  ROS_INFO_STREAM("Marker size min: " << mins << " max: " << maxs);
114  ROS_INFO_STREAM("Desired speed: " << mDetector.getDesiredSpeed());
115 
116 
117 
118  image_sub = it.subscribe("/image", 1, &ArucoSimple::image_callback, this);
119  cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoSimple::cam_info_callback, this);
120 
121  image_pub = it.advertise("result", 1);
122  debug_pub = it.advertise("debug", 1);
123  pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
124  transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
125  position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
126  marker_pub = nh.advertise<visualization_msgs::Marker>("marker", 10);
127  pixel_pub = nh.advertise<geometry_msgs::PointStamped>("pixel", 10);
128 
129  nh.param<double>("marker_size", marker_size, 0.05);
130  nh.param<int>("marker_id", marker_id, 300);
131  nh.param<std::string>("reference_frame", reference_frame, "");
132  nh.param<std::string>("camera_frame", camera_frame, "");
133  nh.param<std::string>("marker_frame", marker_frame, "");
134  nh.param<bool>("image_is_rectified", useRectifiedImages, true);
135  nh.param<bool>("rotate_marker_axis", rotate_marker_axis_, true);
136 
137  ROS_ASSERT(camera_frame != "" && marker_frame != "");
138 
139  if ( reference_frame.empty() )
140  reference_frame = camera_frame;
141 
142  ROS_INFO("Aruco node started with marker size of %f m and marker id to track: %d",
143  marker_size, marker_id);
144  ROS_INFO("Aruco node will publish pose to TF with %s as parent and %s as child.",
145  reference_frame.c_str(), marker_frame.c_str());
146 
147  dyn_rec_server.setCallback(boost::bind(&ArucoSimple::reconf_callback, this, _1, _2));
148  }
149 
150  bool getTransform(const std::string& refFrame,
151  const std::string& childFrame,
153  {
154  std::string errMsg;
155 
156  if ( !_tfListener.waitForTransform(refFrame,
157  childFrame,
158  ros::Time(0),
159  ros::Duration(0.5),
160  ros::Duration(0.01),
161  &errMsg)
162  )
163  {
164  ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg);
165  return false;
166  }
167  else
168  {
169  try
170  {
171  _tfListener.lookupTransform( refFrame, childFrame,
172  ros::Time(0), //get latest available
173  transform);
174  }
175  catch ( const tf::TransformException& e)
176  {
177  ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame);
178  return false;
179  }
180 
181  }
182  return true;
183  }
184 
185 
186  void image_callback(const sensor_msgs::ImageConstPtr& msg)
187  {
188  if ((image_pub.getNumSubscribers() == 0) &&
189  (debug_pub.getNumSubscribers() == 0) &&
190  (pose_pub.getNumSubscribers() == 0) &&
191  (transform_pub.getNumSubscribers() == 0) &&
192  (position_pub.getNumSubscribers() == 0) &&
193  (marker_pub.getNumSubscribers() == 0) &&
194  (pixel_pub.getNumSubscribers() == 0))
195  {
196  ROS_DEBUG("No subscribers, not looking for aruco markers");
197  return;
198  }
199 
200  static tf::TransformBroadcaster br;
201  if(cam_info_received)
202  {
203  ros::Time curr_stamp(ros::Time::now());
204  cv_bridge::CvImagePtr cv_ptr;
205  try
206  {
208  inImage = cv_ptr->image;
209 
210  //detection results will go into "markers"
211  markers.clear();
212  //Ok, let's detect
213  mDetector.detect(inImage, markers, camParam, marker_size, false);
214  //for each marker, draw info and its boundaries in the image
215  for(size_t i=0; i<markers.size(); ++i)
216  {
217  // only publishing the selected marker
218  if(markers[i].id == marker_id)
219  {
220  tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i], rotate_marker_axis_);
221  tf::StampedTransform cameraToReference;
222  cameraToReference.setIdentity();
223 
224  if ( reference_frame != camera_frame )
225  {
226  getTransform(reference_frame,
227  camera_frame,
228  cameraToReference);
229  }
230 
231  transform =
232  static_cast<tf::Transform>(cameraToReference)
233  * static_cast<tf::Transform>(rightToLeft)
234  * transform;
235 
236  tf::StampedTransform stampedTransform(transform, curr_stamp,
237  reference_frame, marker_frame);
238  br.sendTransform(stampedTransform);
239  geometry_msgs::PoseStamped poseMsg;
240  tf::poseTFToMsg(transform, poseMsg.pose);
241  poseMsg.header.frame_id = reference_frame;
242  poseMsg.header.stamp = curr_stamp;
243  pose_pub.publish(poseMsg);
244 
245  geometry_msgs::TransformStamped transformMsg;
246  tf::transformStampedTFToMsg(stampedTransform, transformMsg);
247  transform_pub.publish(transformMsg);
248 
249  geometry_msgs::Vector3Stamped positionMsg;
250  positionMsg.header = transformMsg.header;
251  positionMsg.vector = transformMsg.transform.translation;
252  position_pub.publish(positionMsg);
253 
254  geometry_msgs::PointStamped pixelMsg;
255  pixelMsg.header = transformMsg.header;
256  pixelMsg.point.x = markers[i].getCenter().x;
257  pixelMsg.point.y = markers[i].getCenter().y;
258  pixelMsg.point.z = 0;
259  pixel_pub.publish(pixelMsg);
260 
261  //Publish rviz marker representing the ArUco marker patch
262  visualization_msgs::Marker visMarker;
263  visMarker.header = transformMsg.header;
264  visMarker.id = 1;
265  visMarker.type = visualization_msgs::Marker::CUBE;
266  visMarker.action = visualization_msgs::Marker::ADD;
267  visMarker.pose = poseMsg.pose;
268  visMarker.scale.x = marker_size;
269  visMarker.scale.y = 0.001;
270  visMarker.scale.z = marker_size;
271  visMarker.color.r = 1.0;
272  visMarker.color.g = 0;
273  visMarker.color.b = 0;
274  visMarker.color.a = 1.0;
275  visMarker.lifetime = ros::Duration(3.0);
276  marker_pub.publish(visMarker);
277 
278  }
279  // but drawing all the detected markers
280  markers[i].draw(inImage,cv::Scalar(0,0,255),2);
281  }
282 
283  //draw a 3d cube in each marker if there is 3d info
284  if(camParam.isValid() && marker_size>0)
285  {
286  for(size_t i=0; i<markers.size(); ++i)
287  {
288  CvDrawingUtils::draw3dAxis(inImage, markers[i], camParam);
289  }
290  }
291 
292  if(image_pub.getNumSubscribers() > 0)
293  {
294  //show input with augmented information
295  cv_bridge::CvImage out_msg;
296  out_msg.header.stamp = curr_stamp;
298  out_msg.image = inImage;
299  image_pub.publish(out_msg.toImageMsg());
300  }
301 
302  if(debug_pub.getNumSubscribers() > 0)
303  {
304  //show also the internal image resulting from the threshold operation
305  cv_bridge::CvImage debug_msg;
306  debug_msg.header.stamp = curr_stamp;
308  debug_msg.image = mDetector.getThresholdedImage();
309  debug_pub.publish(debug_msg.toImageMsg());
310  }
311  }
312  catch (cv_bridge::Exception& e)
313  {
314  ROS_ERROR("cv_bridge exception: %s", e.what());
315  return;
316  }
317  }
318  }
319 
320  // wait for one camerainfo, then shut down that subscriber
321  void cam_info_callback(const sensor_msgs::CameraInfo &msg)
322  {
323  camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages);
324 
325  // handle cartesian offset between stereo pairs
326  // see the sensor_msgs/CamaraInfo documentation for details
327  rightToLeft.setIdentity();
328  rightToLeft.setOrigin(
329  tf::Vector3(
330  -msg.P[3]/msg.P[0],
331  -msg.P[7]/msg.P[5],
332  0.0));
333 
334  cam_info_received = true;
335  cam_info_sub.shutdown();
336  }
337 
338 
339  void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
340  {
341  mDetector.setThresholdParams(config.param1,config.param2);
342  if (config.normalizeImage)
343  {
344  ROS_WARN("normalizeImageIllumination is unimplemented!");
345  }
346  }
347 };
348 
349 
350 int main(int argc,char **argv)
351 {
352  ros::init(argc, argv, "aruco_simple");
353 
354  ArucoSimple node;
355 
356  ros::spin();
357 }
double marker_size
bool getTransform(const std::string &refFrame, const std::string &childFrame, tf::StampedTransform &transform)
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, cv::Mat camMatrix=cv::Mat(), cv::Mat distCoeff=cv::Mat(), float markerSizeMeters=-1, bool setYPerpendicular=true)
void setThresholdParams(double param1, double param2)
void getMinMaxSize(float &min, float &max)
tf::StampedTransform rightToLeft
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void getThresholdParams(double &param1, double &param2) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string encoding
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN(...)
CornerRefinementMethod getCornerRefinementMethod() const
void image_callback(const sensor_msgs::ImageConstPtr &msg)
uint32_t getNumSubscribers() const
aruco::CameraParameters camParam
ROSCPP_DECL void spin(Spinner &spinner)
void setIdentity()
pcl::PointCloud< myPointXYZRID > transform(pcl::PointCloud< myPointXYZRID > pc, float x, float y, float z, float rot_x, float rot_y, float rot_z)
void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
ros::Publisher transform_pub
tf::Transform arucoMarker2Tf(const aruco::Marker &marker, bool rotate_marker_axis=true)
vector< Marker > markers
image_transport::Publisher image_pub
image_transport::Subscriber image_sub
void publish(const sensor_msgs::Image &message) const
int main(int argc, char **argv)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void sendTransform(const StampedTransform &transform)
bool useRectifiedImages
static void draw3dAxis(cv::Mat &Image, Marker &m, const CameraParameters &CP)
ThresholdMethods getThresholdMethod() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Main class for marker detection.
aruco::CameraParameters rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo &cam_info, bool useRectifiedParameters)
rosCameraInfo2ArucoCamParams gets the camera intrinsics from a CameraInfo message and copies them to ...
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
uint32_t getNumSubscribers() const
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
image_transport::Publisher debug_pub
void cam_info_callback(const sensor_msgs::CameraInfo &msg)
config
static Time now()
dynamic_reconfigure::Server< aruco_ros::ArucoThresholdConfig > dyn_rec_server
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
tf::TransformListener _tfListener
#define ROS_ERROR_STREAM(args)
#define ROS_ASSERT(cond)
void setCornerRefinementMethod(CornerRefinementMethod method)
#define ROS_ERROR(...)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
sensor_msgs::ImagePtr toImageMsg() const
ros::Subscriber cam_info_sub
std_msgs::Header header
image_transport::ImageTransport it
#define ROS_DEBUG(...)


lidar_camera_calibration
Author(s):
autogenerated on Sat Feb 6 2021 03:39:37