IndividualMarkersNoKinect.cpp
Go to the documentation of this file.
00001 /*
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2012, Scott Niekum
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 the 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  author: Scott Niekum
00035 */
00036 
00037 
00038 #include <std_msgs/Bool.h>
00039 #include "ar_track_alvar/CvTestbed.h"
00040 #include "ar_track_alvar/MarkerDetector.h"
00041 #include "ar_track_alvar/Shared.h"
00042 #include <cv_bridge/cv_bridge.h>
00043 #include <ar_track_alvar_msgs/AlvarMarker.h>
00044 #include <ar_track_alvar_msgs/AlvarMarkers.h>
00045 #include <tf/transform_listener.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <sensor_msgs/image_encodings.h>
00048 #include <dynamic_reconfigure/server.h>
00049 #include <ar_track_alvar/ParamsConfig.h>
00050 
00051 using namespace alvar;
00052 using namespace std;
00053 
00054 bool init=true;
00055 Camera *cam;
00056 cv_bridge::CvImagePtr cv_ptr_;
00057 image_transport::Subscriber cam_sub_;
00058 ros::Publisher arMarkerPub_;
00059 ros::Publisher rvizMarkerPub_;
00060 ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_;
00061 visualization_msgs::Marker rvizMarker_;
00062 tf::TransformListener *tf_listener;
00063 tf::TransformBroadcaster *tf_broadcaster;
00064 MarkerDetector<MarkerData> marker_detector;
00065 
00066 bool enableSwitched = false;
00067 bool enabled = true;
00068 double max_frequency;
00069 double marker_size;
00070 double max_new_marker_error;
00071 double max_track_error;
00072 std::string cam_image_topic;
00073 std::string cam_info_topic;
00074 std::string output_frame;
00075 int marker_resolution = 5; // default marker resolution
00076 int marker_margin = 2; // default marker margin
00077 
00078 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
00079 
00080 
00081 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
00082 {
00083     //If we've already gotten the cam info, then go ahead
00084         if(cam->getCamInfo_){
00085                 try{
00086                         tf::StampedTransform CamToOutput;
00087                         try{
00088                                         tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, ros::Duration(1.0));
00089                                         tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, CamToOutput);
00090                                 }
00091                         catch (tf::TransformException ex){
00092                                 ROS_ERROR("%s",ex.what());
00093                         }
00094 
00095 
00096             //Convert the image
00097             cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
00098 
00099             //Get the estimated pose of the main markers by using all the markers in each bundle
00100 
00101             // GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
00102             // us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
00103             // do this conversion here -jbinney
00104             IplImage ipl_image = cv_ptr_->image;
00105 
00106             marker_detector.Detect(&ipl_image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true);
00107             arPoseMarkers_.markers.clear ();
00108                         for (size_t i=0; i<marker_detector.markers->size(); i++)
00109                         {
00110                                 //Get the pose relative to the camera
00111                         int id = (*(marker_detector.markers))[i].GetId();
00112                                 Pose p = (*(marker_detector.markers))[i].pose;
00113                                 double px = p.translation[0]/100.0;
00114                                 double py = p.translation[1]/100.0;
00115                                 double pz = p.translation[2]/100.0;
00116                                 double qx = p.quaternion[1];
00117                                 double qy = p.quaternion[2];
00118                                 double qz = p.quaternion[3];
00119                                 double qw = p.quaternion[0];
00120 
00121                 tf::Quaternion rotation (qx,qy,qz,qw);
00122                 tf::Vector3 origin (px,py,pz);
00123                 tf::Transform t (rotation, origin);
00124                 tf::Vector3 markerOrigin (0, 0, 0);
00125                 tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
00126                 tf::Transform markerPose = t * m; // marker pose in the camera frame
00127 
00128                 tf::Vector3 z_axis_cam = tf::Transform(rotation, tf::Vector3(0,0,0)) * tf::Vector3(0, 0, 1);
00129 //                ROS_INFO("%02i Z in cam frame: %f %f %f",id, z_axis_cam.x(), z_axis_cam.y(), z_axis_cam.z());
00131                 if (z_axis_cam.z() > 0)
00132                 {
00133                     continue;
00134                 }
00135 
00136                                 //Publish the transform from the camera to the marker
00137                                 std::string markerFrame = "ar_marker_";
00138                                 std::stringstream out;
00139                                 out << id;
00140                                 std::string id_string = out.str();
00141                                 markerFrame += id_string;
00142                                 tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
00143                         tf_broadcaster->sendTransform(camToMarker);
00144 
00145                                 //Create the rviz visualization messages
00146                                 tf::poseTFToMsg (markerPose, rvizMarker_.pose);
00147                                 rvizMarker_.header.frame_id = image_msg->header.frame_id;
00148                                 rvizMarker_.header.stamp = image_msg->header.stamp;
00149                                 rvizMarker_.id = id;
00150 
00151                                 rvizMarker_.scale.x = 1.0 * marker_size/100.0;
00152                                 rvizMarker_.scale.y = 1.0 * marker_size/100.0;
00153                                 rvizMarker_.scale.z = 0.2 * marker_size/100.0;
00154                                 rvizMarker_.ns = "basic_shapes";
00155                                 rvizMarker_.type = visualization_msgs::Marker::CUBE;
00156                                 rvizMarker_.action = visualization_msgs::Marker::ADD;
00157                                 switch (id)
00158                                 {
00159                                   case 0:
00160                                     rvizMarker_.color.r = 0.0f;
00161                                     rvizMarker_.color.g = 0.0f;
00162                                     rvizMarker_.color.b = 1.0f;
00163                                     rvizMarker_.color.a = 1.0;
00164                                     break;
00165                                   case 1:
00166                                     rvizMarker_.color.r = 1.0f;
00167                                     rvizMarker_.color.g = 0.0f;
00168                                     rvizMarker_.color.b = 0.0f;
00169                                     rvizMarker_.color.a = 1.0;
00170                                     break;
00171                                   case 2:
00172                                     rvizMarker_.color.r = 0.0f;
00173                                     rvizMarker_.color.g = 1.0f;
00174                                     rvizMarker_.color.b = 0.0f;
00175                                     rvizMarker_.color.a = 1.0;
00176                                     break;
00177                                   case 3:
00178                                     rvizMarker_.color.r = 0.0f;
00179                                     rvizMarker_.color.g = 0.5f;
00180                                     rvizMarker_.color.b = 0.5f;
00181                                     rvizMarker_.color.a = 1.0;
00182                                     break;
00183                                   case 4:
00184                                     rvizMarker_.color.r = 0.5f;
00185                                     rvizMarker_.color.g = 0.5f;
00186                                     rvizMarker_.color.b = 0.0;
00187                                     rvizMarker_.color.a = 1.0;
00188                                     break;
00189                                   default:
00190                                     rvizMarker_.color.r = 0.5f;
00191                                     rvizMarker_.color.g = 0.0f;
00192                                     rvizMarker_.color.b = 0.5f;
00193                                     rvizMarker_.color.a = 1.0;
00194                                     break;
00195                                 }
00196                                 rvizMarker_.lifetime = ros::Duration (1.0);
00197                                 rvizMarkerPub_.publish (rvizMarker_);
00198 
00199                                 //Get the pose of the tag in the camera frame, then the output frame (usually torso)
00200                                 tf::Transform tagPoseOutput = CamToOutput * markerPose;
00201 
00202                                 //Create the pose marker messages
00203                                 ar_track_alvar_msgs::AlvarMarker ar_pose_marker;
00204                                 tf::poseTFToMsg (tagPoseOutput, ar_pose_marker.pose.pose);
00205                         ar_pose_marker.header.frame_id = output_frame;
00206                             ar_pose_marker.header.stamp = image_msg->header.stamp;
00207                             ar_pose_marker.id = id;
00208                             arPoseMarkers_.markers.push_back (ar_pose_marker);
00209                         }
00210                         arMarkerPub_.publish (arPoseMarkers_);
00211                 }
00212         catch (cv_bridge::Exception& e){
00213                 ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
00214         }
00215         }
00216 }
00217 
00218 void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level)
00219 {
00220   ROS_INFO("AR tracker reconfigured: %s %.2f %.2f %.2f %.2f", config.enabled ? "ENABLED" : "DISABLED",
00221            config.max_frequency, config.marker_size, config.max_new_marker_error, config.max_track_error);
00222 
00223   enableSwitched = enabled != config.enabled;
00224 
00225   enabled = config.enabled;
00226   max_frequency = config.max_frequency;
00227   marker_size = config.marker_size;
00228   max_new_marker_error = config.max_new_marker_error;
00229   max_track_error = config.max_track_error;
00230 }
00231 
00232 void enableCallback(const std_msgs::BoolConstPtr& msg)
00233 {
00234     enableSwitched = enabled != msg->data;
00235     enabled = msg->data;
00236 }
00237 
00238 int main(int argc, char *argv[])
00239 {
00240         ros::init (argc, argv, "marker_detect");
00241         ros::NodeHandle n, pn("~");
00242 
00243   if(argc > 1) {
00244     ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings.");
00245 
00246     if(argc < 7){
00247       std::cout << std::endl;
00248       cout << "Not enough arguments provided." << endl;
00249       cout << "Usage: ./individualMarkersNoKinect <marker size in cm> <max new marker error> <max track error> "
00250            << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin>]";
00251       std::cout << std::endl;
00252       return 0;
00253     }
00254 
00255     // Get params from command line
00256     marker_size = atof(argv[1]);
00257     max_new_marker_error = atof(argv[2]);
00258     max_track_error = atof(argv[3]);
00259     cam_image_topic = argv[4];
00260     cam_info_topic = argv[5];
00261     output_frame = argv[6];
00262 
00263     if (argc > 7) {
00264       max_frequency = atof(argv[7]);
00265       pn.setParam("max_frequency", max_frequency);
00266     }
00267     if (argc > 8)
00268       marker_resolution = atoi(argv[8]);
00269     if (argc > 9)
00270       marker_margin = atoi(argv[9]);
00271 
00272   } else {
00273     // Get params from ros param server.
00274     pn.param("marker_size", marker_size, 10.0);
00275     pn.param("max_new_marker_error", max_new_marker_error, 0.08);
00276     pn.param("max_track_error", max_track_error, 0.2);
00277     pn.param("max_frequency", max_frequency, 8.0);
00278     pn.setParam("max_frequency", max_frequency);  // in case it was not set.
00279     pn.param("marker_resolution", marker_resolution, 5);
00280     pn.param("marker_margin", marker_margin, 2);
00281     if (!pn.getParam("output_frame", output_frame)) {
00282       ROS_ERROR("Param 'output_frame' has to be set.");
00283       exit(EXIT_FAILURE);
00284     }
00285 
00286     // Camera input topics. Use remapping to map to your camera topics.
00287     cam_image_topic = "camera_image";
00288     cam_info_topic = "camera_info";
00289   }
00290 
00291   // Set dynamically configurable parameters so they don't get replaced by default values
00292   pn.setParam("marker_size", marker_size);
00293   pn.setParam("max_new_marker_error", max_new_marker_error);
00294   pn.setParam("max_track_error", max_track_error);
00295 
00296         marker_detector.SetMarkerSize(marker_size, marker_resolution, marker_margin);
00297 
00298         cam = new Camera(n, cam_info_topic);
00299         tf_listener = new tf::TransformListener(n);
00300         tf_broadcaster = new tf::TransformBroadcaster();
00301         arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0);
00302         rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00303 
00304   // Prepare dynamic reconfiguration
00305   dynamic_reconfigure::Server < ar_track_alvar::ParamsConfig > server;
00306   dynamic_reconfigure::Server<ar_track_alvar::ParamsConfig>::CallbackType f;
00307 
00308   f = boost::bind(&configCallback, _1, _2);
00309   server.setCallback(f);
00310 
00311         //Give tf a chance to catch up before the camera callback starts asking for transforms
00312   // It will also reconfigure parameters for the first time, setting the default values
00313         ros::Duration(1.0).sleep();
00314         ros::spinOnce();
00315 
00316         image_transport::ImageTransport it_(n);
00317 
00318   // Run at the configured rate, discarding pointcloud msgs if necessary
00319   ros::Rate rate(max_frequency);
00320 
00323   ros::Subscriber enable_sub_ = pn.subscribe("enable_detection", 1, &enableCallback);
00324 
00325   enableSwitched = true;
00326   while (ros::ok())
00327   {
00328     ros::spinOnce();
00329     rate.sleep();
00330 
00331     if (std::abs((rate.expectedCycleTime() - ros::Duration(1.0 / max_frequency)).toSec()) > 0.001)
00332     {
00333       // Change rate dynamically; if must be above 0, as 0 will provoke a segfault on next spinOnce
00334       ROS_DEBUG("Changing frequency from %.2f to %.2f", 1.0 / rate.expectedCycleTime().toSec(), max_frequency);
00335       rate = ros::Rate(max_frequency);
00336     }
00337 
00338     if (enableSwitched)
00339     {
00340       // Enable/disable switch: subscribe/unsubscribe to make use of pointcloud processing nodelet
00341       // lazy publishing policy; in CPU-scarce computer as TurtleBot's laptop this is a huge saving
00342         if (enabled)
00343             cam_sub_ = it_.subscribe(cam_image_topic, 1, &getCapCallback);
00344         else
00345             cam_sub_.shutdown();
00346         enableSwitched = false;
00347     }
00348   }
00349 
00350     return 0;
00351 }


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02