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 #include <assert.h>
00034 #include <sys/time.h>
00035 #include <unistd.h>
00036
00037 #include <ros/ros.h>
00038 #include <tf/transform_datatypes.h>
00039 #include <tf2/LinearMath/Transform.h>
00040 #include <tf2_ros/buffer.h>
00041 #include <tf2_ros/transform_broadcaster.h>
00042 #include <tf2_ros/transform_listener.h>
00043 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00044 #include <visualization_msgs/Marker.h>
00045 #include <image_transport/image_transport.h>
00046 #include <cv_bridge/cv_bridge.h>
00047 #include <sensor_msgs/image_encodings.h>
00048
00049 #include <list>
00050 #include <string>
00051
00052 #include "fiducial_lib/File.hpp"
00053 #include "fiducial_lib/Fiducials.hpp"
00054
00055 #include "fiducial_pose/rosrpp.h"
00056
00057 #include "fiducial_pose/Fiducial.h"
00058 #include "fiducial_pose/FiducialTransform.h"
00059 #include "fiducial_pose/FiducialTransformArray.h"
00060
00061 class FiducialsNode {
00062 private:
00063 ros::Publisher * marker_pub;
00064 ros::Publisher * vertices_pub;
00065 ros::Publisher * pose_pub;
00066 fiducial_pose::FiducialTransformArray fiducialTransformArray;
00067
00068 ros::Subscriber caminfo_sub;
00069 image_transport::Subscriber img_sub;
00070 bool processing_image;
00071
00072 RosRpp * pose_est;
00073
00074
00075 tf2_ros::TransformBroadcaster tf_pub;
00076 tf2_ros::Buffer tf_buffer;
00077 tf2_ros::TransformListener tf_sub;
00078
00079 std::string world_frame;
00080 std::string pose_frame;
00081 std::string odom_frame;
00082 bool use_odom;
00083
00084
00085
00086 bool publish_tf;
00087
00088
00089
00090 bool publish_markers;
00091
00092
00093 std::string last_camera_frame;
00094
00095 int last_image_seq;
00096 ros::Time last_image_time;
00097
00098
00099 bool publish_images;
00100
00101
00102 bool estimate_pose;
00103 double fiducial_len;
00104 bool undistort_points;
00105
00106 image_transport::Publisher image_pub;
00107
00108 const double scale;
00109 std::string fiducial_namespace;
00110 std::string position_namespace;
00111
00112 std_msgs::ColorRGBA tag_color;
00113 std_msgs::ColorRGBA hidden_tag_color;
00114 std_msgs::ColorRGBA position_color;
00115
00116 Fiducials fiducials;
00117 std::string tag_height_file;
00118 std::string data_directory;
00119 std::string map_file;
00120 std::string log_file;
00121
00122 std::vector<fiducial_pose::Fiducial> detected_fiducials;
00123
00124 geometry_msgs::Pose scale_position(double x, double y, double z,
00125 double theta);
00126 visualization_msgs::Marker createMarker(std::string ns, int id);
00127
00128 static void arc_announce(void *t, int from_id, double from_x,
00129 double from_y, double from_z, int to_id, double to_x, double to_y,
00130 double to_z, double goodness, bool in_spanning_tree);
00131
00132 static void tag_announce(void *t, int id, double x, double y, double z,
00133 double twist, double diagonal, double distance_per_pixel, bool visible,
00134 int hop_count);
00135 void tag_cb(int id, double x, double y, double z, double twist, double dx,
00136 double dy, double dz, bool visible);
00137
00138 static void location_announce(void *t, int id, double x, double y,
00139 double z, double bearing);
00140 void location_cb(int id, double x, double y, double z, double bearing);
00141
00142 static void fiducial_announce(void *t,
00143 int id, int direction, double world_diagonal,
00144 double x0, double y0, double x1, double y1,
00145 double x2, double y2, double x3, double y3);
00146
00147 void fiducial_cb(int id, int direction, double world_diagonal,
00148 double x0, double y0, double x1, double y1,
00149 double x2, double y2, double x3, double y3);
00150
00151 void imageCallback(const sensor_msgs::ImageConstPtr & msg);
00152 void processImage(const sensor_msgs::ImageConstPtr & msg);
00153 void camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg);
00154
00155 boost::thread* update_thread;
00156 public:
00157 FiducialsNode(ros::NodeHandle &nh);
00158 ~FiducialsNode();
00159 };
00160
00161 FiducialsNode::~FiducialsNode() {
00162 if (update_thread) {
00163 update_thread->join();
00164 delete update_thread;
00165 update_thread = NULL;
00166 }
00167 }
00168
00169 geometry_msgs::Pose FiducialsNode::scale_position(double x, double y,
00170 double z, double theta) {
00171 geometry_msgs::Pose res;
00172 res.position.x = x / scale;
00173 res.position.y = y / scale;
00174 res.position.z = z / scale;
00175
00176 res.orientation = tf::createQuaternionMsgFromYaw(theta);
00177
00178 return res;
00179 }
00180
00181 visualization_msgs::Marker FiducialsNode::createMarker(std::string ns, int id) {
00182 visualization_msgs::Marker marker;
00183 marker.header.stamp = ros::Time::now();
00184 marker.header.frame_id = world_frame;
00185 marker.ns = ns;
00186 marker.id = id;
00187 return marker;
00188 }
00189
00190 void FiducialsNode::arc_announce(void *t, int from_id, double from_x,
00191 double from_y, double from_z, int to_id, double to_x, double to_y,
00192 double to_z, double goodness, bool in_spanning_tree) {
00193 }
00194
00195 void FiducialsNode::tag_announce(void *t, int id, double x, double y, double z,
00196 double twist, double diagonal, double distance_per_pixel, bool visible,
00197 int hop_count) {
00198 ROS_INFO("tag_announce:id=%d x=%f y=%f twist=%f",
00199 id, x, y, twist);
00200 FiducialsNode * ths = (FiducialsNode*)t;
00201
00202 double dx = (diagonal * distance_per_pixel) / 1.4142135623730950488016887;
00203 double dy = dx;
00204 double dz = 1.0;
00205 ths->tag_cb(id, x, y, z, twist, dx, dy, dz, visible);
00206 }
00207
00208 void FiducialsNode::fiducial_announce(void *t,
00209 int id, int direction, double world_diagonal,
00210 double x0, double y0, double x1, double y1,
00211 double x2, double y2, double x3, double y3)
00212 {
00213
00214 FiducialsNode * ths = (FiducialsNode*)t;
00215 ths->fiducial_cb(id, direction, world_diagonal,
00216 x0, y0, x1, y1, x2, y2, x3, y3);
00217 }
00218
00219 void FiducialsNode::fiducial_cb(int id, int direction, double world_diagonal,
00220 double x0, double y0, double x1, double y1,
00221 double x2, double y2, double x3, double y3)
00222 {
00223 fiducial_pose::Fiducial fid;
00224
00225 ROS_INFO("fiducial: id=%d dir=%d diag=%f (%.2f,%.2f), (%.2f,%.2f), (%.2f,%.2f), (%.2f,%.2f)",
00226 id, direction, world_diagonal, x0, y0, x1, y1, x2, y2, x3, y3);
00227
00228 fid.header.stamp = last_image_time;
00229 fid.header.frame_id = last_camera_frame;
00230 fid.image_seq = last_image_seq;
00231 fid.direction = direction;
00232 fid.fiducial_id = id;
00233 fid.x0 = x0; fid.y0 = y0;
00234 fid.x1 = x1; fid.y1 = y1;
00235 fid.x2 = x2; fid.y2 = y2;
00236 fid.x3 = x3; fid.y3 = y3;
00237
00238 vertices_pub->publish(fid);
00239 detected_fiducials.push_back(fid);
00240
00241 if (estimate_pose) {
00242 fiducial_pose::FiducialTransform ft;
00243 geometry_msgs::Transform trans;
00244 ft.transform = trans;
00245 if (pose_est->fiducialCallback(&fid, &ft)) {
00246 fiducialTransformArray.transforms.push_back(ft);
00247 }
00248 }
00249 }
00250
00251
00252
00253 void FiducialsNode::tag_cb(int id, double x, double y, double z, double twist,
00254 double dx, double dy, double dz, bool visible) {
00255
00256 if (!publish_markers)
00257 return;
00258
00259 visualization_msgs::Marker marker = createMarker(fiducial_namespace, id);
00260 marker.type = visualization_msgs::Marker::CUBE;
00261 marker.action = visualization_msgs::Marker::ADD;
00262
00263 marker.pose = scale_position(x, y, z, 0);
00264
00265 marker.scale.x = dx / scale;
00266 marker.scale.y = dy / scale;
00267 marker.scale.z = dz / scale;
00268
00269 if( visible ) {
00270 marker.color = tag_color;
00271 } else {
00272 marker.color = hidden_tag_color;
00273 }
00274
00275 marker.lifetime = ros::Duration();
00276
00277 marker_pub->publish(marker);
00278
00279
00280 char str_id[12];
00281 snprintf(str_id, 12, "%d", id);
00282 marker.text = str_id;
00283 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00284 marker.pose.position.z += (marker.scale.z/2.0) + 0.05;
00285 marker.color.r = marker.color.g = marker.color.b = 1.0;
00286 marker.scale.x = marker.scale.y = marker.scale.z = 0.2;
00287 marker.id = id + 10000;
00288 marker.ns = fiducial_namespace + "_text";
00289 marker_pub->publish(marker);
00290 }
00291
00292 tf2::Transform msg_to_tf(geometry_msgs::TransformStamped &msg) {
00293 return tf2::Transform(
00294 tf2::Quaternion(
00295 msg.transform.rotation.x,
00296 msg.transform.rotation.y,
00297 msg.transform.rotation.z,
00298 msg.transform.rotation.w),
00299 tf2::Vector3(
00300 msg.transform.translation.x,
00301 msg.transform.translation.y,
00302 msg.transform.translation.z));
00303 }
00304
00305 void FiducialsNode::location_announce(void * t, int id, double x, double y,
00306 double z,double bearing) {
00307 FiducialsNode * ths = (FiducialsNode*)t;
00308 ths->location_cb(id, x, y, z, bearing);
00309 }
00310
00311 void FiducialsNode::location_cb(int id, double x, double y, double z,
00312 double bearing) {
00313 ROS_INFO("location_announce:id=%d x=%f y=%f bearing=%f",
00314 id, x, y, bearing * 180. / 3.1415926);
00315
00316 visualization_msgs::Marker marker = createMarker(position_namespace, id);
00317 ros::Time now = marker.header.stamp;
00318
00319 marker.type = visualization_msgs::Marker::ARROW;
00320 marker.action = visualization_msgs::Marker::ADD;
00321
00322 marker.pose = scale_position(x, y, z, bearing);
00323
00324 marker.scale.x = 0.2 / scale;
00325 marker.scale.y = 0.05 / scale;
00326 marker.scale.z = 0.05 / scale;
00327
00328 marker.color = position_color;
00329
00330 marker.lifetime = ros::Duration();
00331
00332 if (publish_markers) {
00333 marker_pub->publish(marker);
00334 }
00335
00336
00337
00338 double tf_x = marker.pose.position.x;
00339 double tf_y = marker.pose.position.y;
00340 double tf_yaw = bearing;
00341
00342
00343 if( use_odom ) {
00344
00345
00346
00347 std::string tf_err;
00348 if( tf_buffer.canTransform(pose_frame, odom_frame, now,
00349 ros::Duration(0.1), &tf_err ) ) {
00350
00351 tf2::Quaternion tf_quat;
00352 tf_quat.setRPY(0.0, 0.0, tf_yaw);
00353
00354 tf2::Transform pose(tf_quat, tf2::Vector3(tf_x, tf_y, 0));
00355
00356
00357 if( last_camera_frame.length() > 0 ) {
00358 if( tf_buffer.canTransform(pose_frame, last_camera_frame, now,
00359 ros::Duration(0.1), &tf_err) ) {
00360 geometry_msgs::TransformStamped camera_tf;
00361 camera_tf = tf_buffer.lookupTransform(pose_frame,
00362 last_camera_frame, now);
00363 tf2::Transform camera = msg_to_tf(camera_tf);
00364 pose = pose * camera.inverse();
00365 } else {
00366 ROS_ERROR("Cannot look up transform from %s to %s: %s",
00367 pose_frame.c_str(), last_camera_frame.c_str(), tf_err.c_str());
00368 }
00369 }
00370
00371 geometry_msgs::TransformStamped odom;
00372 odom = tf_buffer.lookupTransform(odom_frame, pose_frame, now);
00373 tf2::Transform odom_tf = msg_to_tf(odom);
00374
00375
00376
00377
00378 tf2::Transform odom_correction = (odom_tf * pose.inverse()).inverse();
00379
00380 geometry_msgs::TransformStamped transform;
00381 tf2::Vector3 odom_correction_v = odom_correction.getOrigin();
00382 transform.transform.translation.x = odom_correction_v.getX();
00383 transform.transform.translation.y = odom_correction_v.getY();
00384 transform.transform.translation.z = odom_correction_v.getZ();
00385
00386 tf2::Quaternion odom_correction_q = odom_correction.getRotation();
00387 transform.transform.rotation.x = odom_correction_q.getX();
00388 transform.transform.rotation.y = odom_correction_q.getY();
00389 transform.transform.rotation.z = odom_correction_q.getZ();
00390 transform.transform.rotation.w = odom_correction_q.getW();
00391
00392 transform.header.stamp = now;
00393 transform.header.frame_id = world_frame;
00394 transform.child_frame_id = odom_frame;
00395
00396
00397
00398 if (publish_tf)
00399 tf_pub.sendTransform(transform);
00400 } else {
00401 ROS_ERROR("Can't look up base transform from %s to %s: %s",
00402 pose_frame.c_str(),
00403 odom_frame.c_str(),
00404 tf_err.c_str());
00405 }
00406 } else {
00407
00408 geometry_msgs::TransformStamped transform;
00409 transform.header.stamp = now;
00410 transform.header.frame_id = world_frame;
00411 transform.child_frame_id = pose_frame;
00412
00413 transform.transform.translation.x = tf_x;
00414 transform.transform.translation.y = tf_y;
00415 transform.transform.translation.z = 0.0;
00416 transform.transform.rotation = tf::createQuaternionMsgFromYaw(tf_yaw);
00417
00418 if (publish_tf)
00419 tf_pub.sendTransform(transform);
00420 }
00421 }
00422
00423 void FiducialsNode::camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
00424 {
00425 if (pose_est) {
00426 pose_est->camInfoCallback(msg);
00427 }
00428 }
00429
00430 void FiducialsNode::imageCallback(const sensor_msgs::ImageConstPtr & msg)
00431 {
00432 if (!processing_image) {
00433 processing_image = true;
00434 if (update_thread) {
00435 update_thread->join();
00436 delete update_thread;
00437 update_thread = NULL;
00438 }
00439 processing_image = true;
00440 update_thread = new boost::thread(boost::bind(&FiducialsNode::processImage, this, msg));
00441 } else {
00442 ROS_INFO("Dropping image");
00443 }
00444 }
00445
00446 void FiducialsNode::processImage(const sensor_msgs::ImageConstPtr & msg)
00447 {
00448 processing_image = true;
00449
00450 last_camera_frame = msg->header.frame_id;
00451 last_image_seq = msg->header.seq;
00452 last_image_time = msg->header.stamp;
00453 ROS_INFO("Got image seq %d", last_image_seq);
00454
00455 fiducialTransformArray.transforms.clear();
00456 fiducialTransformArray.header.stamp = msg->header.stamp;
00457 fiducialTransformArray.image_seq = msg->header.seq;
00458
00459 try {
00460 cv_bridge::CvImageConstPtr cv_img;
00461 cv_img = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
00462 IplImage *image = new IplImage(cv_img->image);
00463 if(fiducials == NULL) {
00464 ROS_INFO("Got first image! Setting up Fiducials library");
00465
00466 Fiducials_Create fiducials_create =
00467 Fiducials_Create__one_and_only();
00468 fiducials_create->fiducials_path = data_directory.c_str();
00469 fiducials_create->lens_calibrate_file_name = (String_Const)0;
00470 fiducials_create->announce_object = (Memory)this;
00471 fiducials_create->arc_announce_routine = arc_announce;
00472 fiducials_create->location_announce_routine = location_announce;
00473 fiducials_create->tag_announce_routine = tag_announce;
00474 fiducials_create->log_file_name = log_file.c_str();
00475 fiducials_create->map_base_name = map_file.c_str();
00476 fiducials_create->tag_heights_file_name = tag_height_file.c_str();
00477 fiducials_create->fiducial_announce_routine = fiducial_announce;
00478 fiducials_create->do_2d_slam = publish_tf;
00479
00480
00481 fiducials = Fiducials__create(image, fiducials_create);
00482 }
00483 Fiducials__image_set(fiducials, image);
00484 Fiducials_Results results = Fiducials__process(fiducials);
00485 ROS_INFO("Processed image");
00486 if (publish_images) {
00487 for (unsigned i=0; i < detected_fiducials.size(); i++) {
00488 fiducial_pose::Fiducial& fid = detected_fiducials[i];
00489 cvLine(image, cvPoint(fid.x0, fid.y0), cvPoint(fid.x1, fid.y1), CV_RGB(255, 0, 0), 3);
00490 cvLine(image, cvPoint(fid.x1, fid.y1), cvPoint(fid.x2, fid.y2), CV_RGB(255, 0, 0), 3);
00491 cvLine(image, cvPoint(fid.x2, fid.y2), cvPoint(fid.x3, fid.y3), CV_RGB(255, 0, 0), 3);
00492 cvLine(image, cvPoint(fid.x3, fid.y3), cvPoint(fid.x0, fid.y0), CV_RGB(255, 0, 0), 3);
00493 }
00494 detected_fiducials.clear();
00495 image_pub.publish(msg);
00496 }
00497 } catch(cv_bridge::Exception & e) {
00498 ROS_ERROR("cv_bridge exception: %s", e.what());
00499 }
00500
00501 pose_pub->publish(fiducialTransformArray);
00502 ROS_INFO("Finished processing image seq %d", last_image_seq);
00503 processing_image = false;
00504 }
00505
00506 FiducialsNode::FiducialsNode(ros::NodeHandle & nh) : scale(0.75), tf_sub(tf_buffer) {
00507 processing_image = false;
00508 update_thread = NULL;
00509 fiducial_namespace = "fiducials";
00510 position_namespace = "position";
00511
00512 tag_color.r = 0.0f;
00513 tag_color.g = 1.0f;
00514 tag_color.b = 0.0f;
00515 tag_color.a = 1.0f;
00516
00517
00518 hidden_tag_color.r = 1.0f;
00519 hidden_tag_color.g = 0.0f;
00520 hidden_tag_color.b = 0.0f;
00521 hidden_tag_color.a = 1.0f;
00522
00523
00524 position_color.r = 0.0f;
00525 position_color.g = 0.0f;
00526 position_color.b = 1.0f;
00527 position_color.a = 1.0f;
00528
00529 nh.param<std::string>("tag_height", tag_height_file, "");
00530 nh.param<std::string>("data_directory", data_directory, ".");
00531 nh.param<std::string>("map_file", map_file, "ROS_Map");
00532 nh.param<std::string>("log_file", log_file, "fiducials.log.txt");
00533 nh.param<std::string>("map_frame", world_frame, "map");
00534 nh.param<std::string>("pose_frame", pose_frame, "base_link");
00535 nh.param<bool>("publish_tf", publish_tf, false);
00536
00537 if (publish_tf) {
00538 ROS_INFO("Publishing transform from %s to %s", world_frame.c_str(),
00539 pose_frame.c_str());
00540
00541 if(nh.hasParam("odom_frame") ) {
00542 use_odom = true;
00543 nh.getParam("odom_frame", odom_frame);
00544 ROS_INFO("Using odometry frame %s", odom_frame.c_str());
00545 } else {
00546 use_odom = false;
00547 ROS_INFO("Not using odometry");
00548 }
00549 }
00550
00551 nh.param<bool>("publish_images", publish_images, false);
00552 nh.param<bool>("publish_markers", publish_markers, false);
00553 nh.param<bool>("estimate_pose", estimate_pose, true);
00554
00555 nh.param<double>("fiducial_len", fiducial_len, 0.146);
00556 nh.param<bool>("undistort_points", undistort_points, false);
00557
00558 image_transport::ImageTransport img_transport(nh);
00559
00560 if (publish_images) {
00561 image_pub = img_transport.advertise("fiducial_images", 1);
00562 }
00563
00564 if (publish_markers) {
00565 marker_pub = new ros::Publisher(nh.advertise<visualization_msgs::Marker>("fiducials", 1));
00566 }
00567
00568 vertices_pub = new ros::Publisher(nh.advertise<fiducial_pose::Fiducial>("vertices", 1));
00569
00570 if (estimate_pose) {
00571 pose_pub = new ros::Publisher(nh.advertise<fiducial_pose::FiducialTransformArray>("fiducial_transforms", 1));
00572 pose_est = new RosRpp(fiducial_len, undistort_points);
00573 }
00574 else {
00575 pose_est = NULL;
00576 }
00577
00578 fiducials = NULL;
00579
00580
00581
00582
00583 img_sub = img_transport.subscribe("camera", 1,
00584 &FiducialsNode::imageCallback, this);
00585
00586 caminfo_sub = nh.subscribe("camera_info", 1,
00587 &FiducialsNode::camInfoCallback, this);
00588
00589 ROS_INFO("Fiducials Localization ready");
00590 }
00591
00592 int main(int argc, char ** argv) {
00593 ros::init(argc, argv, "fiducial_detect");
00594 ros::NodeHandle nh("~");
00595
00596 FiducialsNode * node = new FiducialsNode(nh);
00597
00598 ros::spin();
00599
00600 return 0;
00601 }