IndividualMarkers.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 "ar_track_alvar/CvTestbed.h"
00039 #include "ar_track_alvar/MarkerDetector.h"
00040 #include "ar_track_alvar/Shared.h"
00041 #include <cv_bridge/cv_bridge.h>
00042 #include <ar_track_alvar_msgs/AlvarMarker.h>
00043 #include <ar_track_alvar_msgs/AlvarMarkers.h>
00044 #include <tf/transform_listener.h>
00045 #include <tf/transform_broadcaster.h>
00046 #include <sensor_msgs/image_encodings.h>
00047 #include <pcl_conversions/pcl_conversions.h>
00048 #include <dynamic_reconfigure/server.h>
00049 #include <ar_track_alvar/ParamsConfig.h>
00050 #include <Eigen/StdVector>
00051 
00052 namespace gm=geometry_msgs;
00053 namespace ata=ar_track_alvar;
00054 
00055 typedef pcl::PointXYZRGB ARPoint;
00056 typedef pcl::PointCloud<ARPoint> ARCloud;
00057 
00058 using namespace alvar;
00059 using namespace std;
00060 using boost::make_shared;
00061 
00062 bool init=true;
00063 Camera *cam;
00064 cv_bridge::CvImagePtr cv_ptr_;
00065 image_transport::Subscriber cam_sub_;
00066 ros::Subscriber cloud_sub_;
00067 ros::Publisher arMarkerPub_;
00068 ros::Publisher rvizMarkerPub_;
00069 ros::Publisher rvizMarkerPub2_;
00070 ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_;
00071 visualization_msgs::Marker rvizMarker_;
00072 tf::TransformListener *tf_listener;
00073 tf::TransformBroadcaster *tf_broadcaster;
00074 MarkerDetector<MarkerData> marker_detector;
00075 
00076 bool enableSwitched = false;
00077 bool enabled = true;
00078 bool output_frame_from_msg;
00079 double max_frequency;
00080 double marker_size;
00081 double max_new_marker_error;
00082 double max_track_error;
00083 std::string cam_image_topic;
00084 std::string cam_info_topic;
00085 std::string output_frame;
00086 int marker_resolution = 5; // default marker resolution
00087 int marker_margin = 2; // default marker margin
00088 
00089 
00090 //Debugging utility function
00091 void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad)
00092 {
00093   visualization_msgs::Marker rvizMarker;
00094 
00095   rvizMarker.header.frame_id = frame;
00096   rvizMarker.header.stamp = ros::Time::now();
00097   rvizMarker.id = id;
00098   rvizMarker.ns = "3dpts";
00099 
00100   rvizMarker.scale.x = rad;
00101   rvizMarker.scale.y = rad;
00102   rvizMarker.scale.z = rad;
00103 
00104   rvizMarker.type = visualization_msgs::Marker::SPHERE_LIST;
00105   rvizMarker.action = visualization_msgs::Marker::ADD;
00106 
00107   if(color==1){
00108     rvizMarker.color.r = 0.0f;
00109     rvizMarker.color.g = 1.0f;
00110     rvizMarker.color.b = 1.0f;
00111     rvizMarker.color.a = 1.0;
00112   }
00113   if(color==2){
00114     rvizMarker.color.r = 1.0f;
00115     rvizMarker.color.g = 0.0f;
00116     rvizMarker.color.b = 1.0f;
00117     rvizMarker.color.a = 1.0;
00118   }
00119   if(color==3){
00120     rvizMarker.color.r = 1.0f;
00121     rvizMarker.color.g = 1.0f;
00122     rvizMarker.color.b = 0.0f;
00123     rvizMarker.color.a = 1.0;
00124   }
00125 
00126   gm::Point p;
00127   for(int i=0; i<cloud->points.size(); i++){
00128     p.x = cloud->points[i].x;
00129     p.y = cloud->points[i].y;
00130     p.z = cloud->points[i].z;
00131     rvizMarker.points.push_back(p);
00132   }
00133 
00134   rvizMarker.lifetime = ros::Duration (1.0);
00135   rvizMarkerPub2_.publish (rvizMarker);
00136 }
00137 
00138 
00139 void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int id)
00140 {
00141   visualization_msgs::Marker rvizMarker;
00142 
00143   rvizMarker.header.frame_id = frame;
00144   rvizMarker.header.stamp = ros::Time::now();
00145   rvizMarker.id = id;
00146   rvizMarker.ns = "arrow";
00147 
00148   rvizMarker.scale.x = 0.01;
00149   rvizMarker.scale.y = 0.01;
00150   rvizMarker.scale.z = 0.1;
00151 
00152   rvizMarker.type = visualization_msgs::Marker::ARROW;
00153   rvizMarker.action = visualization_msgs::Marker::ADD;
00154 
00155   for(int i=0; i<3; i++){
00156     rvizMarker.points.clear();
00157     rvizMarker.points.push_back(start);
00158     gm::Point end;
00159     end.x = start.x + mat[0][i];
00160     end.y = start.y + mat[1][i];
00161     end.z = start.z + mat[2][i];
00162     rvizMarker.points.push_back(end);
00163     rvizMarker.id += 10*i;
00164     rvizMarker.lifetime = ros::Duration (1.0);
00165 
00166     if(color==1){
00167       rvizMarker.color.r = 1.0f;
00168       rvizMarker.color.g = 0.0f;
00169       rvizMarker.color.b = 0.0f;
00170       rvizMarker.color.a = 1.0;
00171     }
00172     if(color==2){
00173       rvizMarker.color.r = 0.0f;
00174       rvizMarker.color.g = 1.0f;
00175       rvizMarker.color.b = 0.0f;
00176       rvizMarker.color.a = 1.0;
00177     }
00178     if(color==3){
00179       rvizMarker.color.r = 0.0f;
00180       rvizMarker.color.g = 0.0f;
00181       rvizMarker.color.b = 1.0f;
00182       rvizMarker.color.a = 1.0;
00183     }
00184     color += 1;
00185 
00186     rvizMarkerPub2_.publish (rvizMarker);
00187   }
00188 }
00189 
00190 
00191 int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr selected_points, const ARCloud &cloud, Pose &p){
00192 
00193   ata::PlaneFitResult res = ata::fitPlane(selected_points);
00194   gm::PoseStamped pose;
00195   pose.header.stamp = pcl_conversions::fromPCL(cloud.header).stamp;
00196   pose.header.frame_id = cloud.header.frame_id;
00197   pose.pose.position = ata::centroid(*res.inliers);
00198 
00199   draw3dPoints(selected_points, cloud.header.frame_id, 1, id, 0.005);
00200 
00201   //Get 2 points that point forward in marker x direction
00202   int i1,i2;
00203   if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) ||
00204      isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z))
00205     {
00206       if(isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z) ||
00207          isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z))
00208         {
00209           return -1;
00210         }
00211       else{
00212         i1 = 1;
00213         i2 = 2;
00214       }
00215     }
00216   else{
00217     i1 = 0;
00218     i2 = 3;
00219   }
00220 
00221   //Get 2 points the point forward in marker y direction
00222   int i3,i4;
00223   if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) ||
00224      isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z))
00225     {
00226       if(isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z) ||
00227          isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z))
00228         {
00229           return -1;
00230         }
00231       else{
00232         i3 = 2;
00233         i4 = 3;
00234       }
00235     }
00236   else{
00237     i3 = 1;
00238     i4 = 0;
00239   }
00240 
00241   ARCloud::Ptr orient_points(new ARCloud());
00242   orient_points->points.push_back(corners_3D[i1]);
00243   draw3dPoints(orient_points, cloud.header.frame_id, 3, id+1000, 0.008);
00244 
00245   orient_points->clear();
00246   orient_points->points.push_back(corners_3D[i2]);
00247   draw3dPoints(orient_points, cloud.header.frame_id, 2, id+2000, 0.008);
00248 
00249   int succ;
00250   succ = ata::extractOrientation(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], pose.pose.orientation);
00251   if(succ < 0) return -1;
00252 
00253   tf::Matrix3x3 mat;
00254   succ = ata::extractFrame(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], mat);
00255   if(succ < 0) return -1;
00256 
00257   drawArrow(pose.pose.position, mat, cloud.header.frame_id, 1, id);
00258 
00259   p.translation[0] = pose.pose.position.x * 100.0;
00260   p.translation[1] = pose.pose.position.y * 100.0;
00261   p.translation[2] = pose.pose.position.z * 100.0;
00262   p.quaternion[1] = pose.pose.orientation.x;
00263   p.quaternion[2] = pose.pose.orientation.y;
00264   p.quaternion[3] = pose.pose.orientation.z;
00265   p.quaternion[0] = pose.pose.orientation.w;
00266 
00267   return 0;
00268 }
00269 
00270 
00271 void GetMarkerPoses(IplImage *image, ARCloud &cloud) {
00272 
00273   //Detect and track the markers
00274   if (marker_detector.Detect(image, cam, true, false, max_new_marker_error,
00275                              max_track_error, CVSEQ, true))
00276     {
00277       ROS_DEBUG_STREAM("--------------------------");
00278       for (size_t i=0; i<marker_detector.markers->size(); i++)
00279         {
00280           vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels;
00281           Marker *m = &((*marker_detector.markers)[i]);
00282           int id = m->GetId();
00283           ROS_DEBUG_STREAM("******* ID: " << id);
00284 
00285           int resol = m->GetRes();
00286           int ori = m->ros_orientation;
00287 
00288           PointDouble pt1, pt2, pt3, pt4;
00289           pt4 = m->ros_marker_points_img[0];
00290           pt3 = m->ros_marker_points_img[resol-1];
00291           pt1 = m->ros_marker_points_img[(resol*resol)-resol];
00292           pt2 = m->ros_marker_points_img[(resol*resol)-1];
00293 
00294           m->ros_corners_3D[0] = cloud(pt1.x, pt1.y);
00295           m->ros_corners_3D[1] = cloud(pt2.x, pt2.y);
00296           m->ros_corners_3D[2] = cloud(pt3.x, pt3.y);
00297           m->ros_corners_3D[3] = cloud(pt4.x, pt4.y);
00298 
00299           if(ori >= 0 && ori < 4){
00300             if(ori != 0){
00301               std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end());
00302             }
00303           }
00304           else
00305             ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id);
00306 
00307           //Get the 3D marker points
00308           BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img)
00309             pixels.push_back(cv::Point(p.x, p.y));
00310           ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels);
00311 
00312           //Use the kinect data to find a plane and pose for the marker
00313           int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose);
00314         }
00315     }
00316 }
00317 
00318 
00319 
00320 void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
00321 {
00322   sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
00323 
00324   // If desired, use the frame in the message's header.
00325   if (output_frame_from_msg)
00326     output_frame = msg->header.frame_id;
00327 
00328   //If we've already gotten the cam info, then go ahead
00329   if(cam->getCamInfo_){
00330     //Convert cloud to PCL
00331     ARCloud cloud;
00332     pcl::fromROSMsg(*msg, cloud);
00333 
00334     //Get an OpenCV image from the cloud
00335     pcl::toROSMsg (*msg, *image_msg);
00336     image_msg->header.stamp = msg->header.stamp;
00337     image_msg->header.frame_id = msg->header.frame_id;
00338 
00339 
00340     //Convert the image
00341     cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
00342 
00343     //Get the estimated pose of the main markers by using all the markers in each bundle
00344 
00345     // GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
00346     // us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
00347     // do this conversion here -jbinney
00348     IplImage ipl_image = cv_ptr_->image;
00349 
00350 
00351     //Use the kinect to improve the pose
00352     Pose ret_pose;
00353     GetMarkerPoses(&ipl_image, cloud);
00354 
00355     tf::StampedTransform CamToOutput;
00356     if (image_msg->header.frame_id == output_frame) {
00357       CamToOutput.setIdentity();
00358     } else {
00359       try {
00360         tf_listener->waitForTransform(output_frame, image_msg->header.frame_id,
00361                                       image_msg->header.stamp, ros::Duration(1.0));
00362         tf_listener->lookupTransform(output_frame, image_msg->header.frame_id,
00363                                      image_msg->header.stamp, CamToOutput);
00364       } catch (tf::TransformException ex) {
00365         ROS_ERROR("%s",ex.what());
00366       }
00367     }
00368 
00369     try{
00370 
00371 
00372       arPoseMarkers_.markers.clear ();
00373       for (size_t i=0; i<marker_detector.markers->size(); i++)
00374         {
00375           //Get the pose relative to the camera
00376           int id = (*(marker_detector.markers))[i].GetId();
00377           Pose p = (*(marker_detector.markers))[i].pose;
00378 
00379           double px = p.translation[0]/100.0;
00380           double py = p.translation[1]/100.0;
00381           double pz = p.translation[2]/100.0;
00382           double qx = p.quaternion[1];
00383           double qy = p.quaternion[2];
00384           double qz = p.quaternion[3];
00385           double qw = p.quaternion[0];
00386 
00387       tf::Quaternion rotation (qx,qy,qz,qw);
00388       tf::Vector3 origin (px,py,pz);
00389       tf::Transform t (rotation, origin);
00390       tf::Vector3 markerOrigin (0, 0, 0);
00391       tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
00392       tf::Transform markerPose = t * m; // marker pose in the camera frame
00393 
00394           //Publish the transform from the camera to the marker
00395           std::string markerFrame = "ar_marker_";
00396           std::stringstream out;
00397           out << id;
00398           std::string id_string = out.str();
00399           markerFrame += id_string;
00400           tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
00401           tf_broadcaster->sendTransform(camToMarker);
00402 
00403           //Create the rviz visualization messages
00404           tf::poseTFToMsg (markerPose, rvizMarker_.pose);
00405           rvizMarker_.header.frame_id = image_msg->header.frame_id;
00406           rvizMarker_.header.stamp = image_msg->header.stamp;
00407           rvizMarker_.id = id;
00408 
00409           rvizMarker_.scale.x = 1.0 * marker_size/100.0;
00410           rvizMarker_.scale.y = 1.0 * marker_size/100.0;
00411           rvizMarker_.scale.z = 0.2 * marker_size/100.0;
00412           rvizMarker_.ns = "basic_shapes";
00413           rvizMarker_.type = visualization_msgs::Marker::CUBE;
00414           rvizMarker_.action = visualization_msgs::Marker::ADD;
00415           switch (id)
00416             {
00417             case 0:
00418               rvizMarker_.color.r = 0.0f;
00419               rvizMarker_.color.g = 0.0f;
00420               rvizMarker_.color.b = 1.0f;
00421               rvizMarker_.color.a = 1.0;
00422               break;
00423             case 1:
00424               rvizMarker_.color.r = 1.0f;
00425               rvizMarker_.color.g = 0.0f;
00426               rvizMarker_.color.b = 0.0f;
00427               rvizMarker_.color.a = 1.0;
00428               break;
00429             case 2:
00430               rvizMarker_.color.r = 0.0f;
00431               rvizMarker_.color.g = 1.0f;
00432               rvizMarker_.color.b = 0.0f;
00433               rvizMarker_.color.a = 1.0;
00434               break;
00435             case 3:
00436               rvizMarker_.color.r = 0.0f;
00437               rvizMarker_.color.g = 0.5f;
00438               rvizMarker_.color.b = 0.5f;
00439               rvizMarker_.color.a = 1.0;
00440               break;
00441             case 4:
00442               rvizMarker_.color.r = 0.5f;
00443               rvizMarker_.color.g = 0.5f;
00444               rvizMarker_.color.b = 0.0;
00445               rvizMarker_.color.a = 1.0;
00446               break;
00447             default:
00448               rvizMarker_.color.r = 0.5f;
00449               rvizMarker_.color.g = 0.0f;
00450               rvizMarker_.color.b = 0.5f;
00451               rvizMarker_.color.a = 1.0;
00452               break;
00453             }
00454           rvizMarker_.lifetime = ros::Duration (1.0);
00455           rvizMarkerPub_.publish (rvizMarker_);
00456 
00457           //Get the pose of the tag in the camera frame, then the output frame (usually torso)
00458           tf::Transform tagPoseOutput = CamToOutput * markerPose;
00459 
00460           //Create the pose marker messages
00461           ar_track_alvar_msgs::AlvarMarker ar_pose_marker;
00462           tf::poseTFToMsg (tagPoseOutput, ar_pose_marker.pose.pose);
00463           ar_pose_marker.header.frame_id = output_frame;
00464           ar_pose_marker.header.stamp = image_msg->header.stamp;
00465           ar_pose_marker.id = id;
00466           arPoseMarkers_.markers.push_back (ar_pose_marker);
00467         }
00468       arPoseMarkers_.header.stamp = image_msg->header.stamp;
00469       arMarkerPub_.publish (arPoseMarkers_);
00470     }
00471     catch (cv_bridge::Exception& e){
00472       ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
00473     }
00474   }
00475 }
00476 
00477 void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level)
00478 {
00479   ROS_INFO("AR tracker reconfigured: %s %.2f %.2f %.2f %.2f", config.enabled ? "ENABLED" : "DISABLED",
00480            config.max_frequency, config.marker_size, config.max_new_marker_error, config.max_track_error);
00481 
00482   enableSwitched = enabled != config.enabled;
00483 
00484   enabled = config.enabled;
00485   max_frequency = config.max_frequency;
00486   marker_size = config.marker_size;
00487   max_new_marker_error = config.max_new_marker_error;
00488   max_track_error = config.max_track_error;
00489 }
00490 
00491 int main(int argc, char *argv[])
00492 {
00493   ros::init (argc, argv, "marker_detect");
00494   ros::NodeHandle n, pn("~");
00495 
00496   if(argc > 1) {
00497     ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings.");
00498 
00499     if(argc < 7){
00500       std::cout << std::endl;
00501       cout << "Not enough arguments provided." << endl;
00502       cout << "Usage: ./individualMarkers <marker size in cm> <max new marker error> <max track error> "
00503            << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin>]";
00504       std::cout << std::endl;
00505       return 0;
00506     }
00507 
00508     // Get params from command line
00509     marker_size = atof(argv[1]);
00510     max_new_marker_error = atof(argv[2]);
00511     max_track_error = atof(argv[3]);
00512     cam_image_topic = argv[4];
00513     cam_info_topic = argv[5];
00514     output_frame = argv[6];
00515 
00516     if (argc > 7) {
00517       max_frequency = atof(argv[7]);
00518       pn.setParam("max_frequency", max_frequency);
00519     }
00520     if (argc > 8)
00521       marker_resolution = atoi(argv[8]);
00522     if (argc > 9)
00523       marker_margin = atoi(argv[9]);
00524 
00525   } else {
00526     // Get params from ros param server.
00527     pn.param("marker_size", marker_size, 10.0);
00528     pn.param("max_new_marker_error", max_new_marker_error, 0.08);
00529     pn.param("max_track_error", max_track_error, 0.2);
00530     pn.param("max_frequency", max_frequency, 8.0);
00531     pn.setParam("max_frequency", max_frequency);  // in case it was not set.
00532     pn.param("marker_resolution", marker_resolution, 5);
00533     pn.param("marker_margin", marker_margin, 2);
00534     pn.param("output_frame_from_msg", output_frame_from_msg, false);
00535 
00536     if (!output_frame_from_msg && !pn.getParam("output_frame", output_frame)) {
00537       ROS_ERROR("Param 'output_frame' has to be set if the output frame is not "
00538                 "derived from the point cloud message.");
00539       exit(EXIT_FAILURE);
00540     }
00541 
00542     // Camera input topics. Use remapping to map to your camera topics.
00543     cam_image_topic = "camera_image";
00544     cam_info_topic = "camera_info";
00545   }
00546 
00547   // Set dynamically configurable parameters so they don't get replaced by default values
00548   pn.setParam("marker_size", marker_size);
00549   pn.setParam("max_new_marker_error", max_new_marker_error);
00550   pn.setParam("max_track_error", max_track_error);
00551 
00552   marker_detector.SetMarkerSize(marker_size, marker_resolution, marker_margin);
00553 
00554   cam = new Camera(n, cam_info_topic);
00555 
00556   if (!output_frame_from_msg) {
00557     // TF listener is only required when output frame != camera frame.
00558     tf_listener = new tf::TransformListener(n);
00559   }
00560   tf_broadcaster = new tf::TransformBroadcaster();
00561   arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0);
00562   rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00563   rvizMarkerPub2_ = n.advertise < visualization_msgs::Marker > ("ARmarker_points", 0);
00564 
00565   // Prepare dynamic reconfiguration
00566   dynamic_reconfigure::Server < ar_track_alvar::ParamsConfig > server;
00567   dynamic_reconfigure::Server<ar_track_alvar::ParamsConfig>::CallbackType f;
00568 
00569   f = boost::bind(&configCallback, _1, _2);
00570   server.setCallback(f);
00571 
00572   //Give tf a chance to catch up before the camera callback starts asking for transforms
00573   // It will also reconfigure parameters for the first time, setting the default values
00574   ros::Duration(1.0).sleep();
00575   ros::spinOnce();
00576 
00577   if (enabled == true)
00578   {
00579     // This always happens, as enable is true by default
00580     ROS_INFO("Subscribing to image topic");
00581     cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback);
00582   }
00583 
00584   // Run at the configured rate, discarding pointcloud msgs if necessary
00585   ros::Rate rate(max_frequency);
00586 
00587   while (ros::ok())
00588   {
00589     ros::spinOnce();
00590     rate.sleep();
00591 
00592     if (std::abs((rate.expectedCycleTime() - ros::Duration(1.0 / max_frequency)).toSec()) > 0.001)
00593     {
00594       // Change rate dynamically; if must be above 0, as 0 will provoke a segfault on next spinOnce
00595       ROS_DEBUG("Changing frequency from %.2f to %.2f", 1.0 / rate.expectedCycleTime().toSec(), max_frequency);
00596       rate = ros::Rate(max_frequency);
00597     }
00598 
00599     if (enableSwitched == true)
00600     {
00601       // Enable/disable switch: subscribe/unsubscribe to make use of pointcloud processing nodelet
00602       // lazy publishing policy; in CPU-scarce computer as TurtleBot's laptop this is a huge saving
00603       if (enabled == false)
00604         cloud_sub_.shutdown();
00605       else
00606         cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback);
00607 
00608       enableSwitched = false;
00609     }
00610   }
00611 
00612   return 0;
00613 }


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