actarray_cloud_assembler.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: actarray_cloud_assembler.cpp 200 2009-09-21 20:21:21Z dejanpan $
00028  *
00029  */
00030 
00042 // ROS core
00043 #include <ros/ros.h>
00044 // ROS messages
00045 #include <sensor_msgs/PointCloud.h>
00046 #include <sensor_msgs/LaserScan.h>
00047 #include <player_log_actarray/PlayerActarray.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include <angles/angles.h>
00050 #include <Eigen/Core>
00051 
00052 #include<iostream>
00053 #include <fstream>
00054 #include <boost/algorithm/string.hpp>
00055 #include <boost/thread/mutex.hpp>
00056 //for get_log_files()
00057 #include <boost/filesystem/operations.hpp>
00058 #include <boost/filesystem/fstream.hpp>
00059 #include "boost/filesystem.hpp"
00060 //for savePCDFile ()
00061 #include <point_cloud_mapping/cloud_io.h>
00062 #include <list>
00063 
00064 using namespace std;
00065 using namespace ros;
00066 using namespace sensor_msgs;
00067 using namespace player_log_actarray;
00068 using namespace cloud_io;
00069 using namespace boost::filesystem; 
00070 
00071 struct DH
00072 {
00073   int type;
00074   double t, d, l, a; // theta, d, a, alpha => t, d, l, a
00075 };
00076 
00077 class ActarrayCloudAssembler
00078 {
00079 protected:
00080   string tf_frame_;
00081   NodeHandle nh_;
00082   tf::TransformBroadcaster broadcaster_;
00083   tf::Stamped<tf::Transform> transform_;
00084   boost::mutex s_lock_, a_lock_;
00085 
00086   int total_laser_scans_;
00087 public:
00088   // ROS messages
00089   list<LaserScanConstPtr> scans_;
00090   vector<PlayerActarrayConstPtr> actarrays_;
00091   double first_act_stamp_;
00092 
00093   PointCloud cloud_;
00094 
00095   Subscriber actarray_sub_, laserscan_sub_;
00096 
00097   Publisher cloud_pub_;
00098 
00099   vector<DH> arm_params_;
00100   // Parameters
00101   Eigen::Vector4d translation_;
00102   double min_distance_, max_distance_, laser_min_angle_, laser_max_angle_;
00103   bool left_arm_;
00104   //arguments only used if assemble pcds from multiple files
00105   int save_to_pcd_laser_, save_to_pcd_actarray_;
00106   vector<string> file_list_;
00107   string cur_file_, dir_;
00108 
00110   ActarrayCloudAssembler () : tf_frame_ ("laser_tilt_mount_link"),
00111                               transform_ (btTransform (btQuaternion (0, 0, 0), btVector3 (0, 0, 0)), Time::now (), tf_frame_, tf_frame_),
00112                               total_laser_scans_ (0),
00113                               left_arm_ (true)
00114   {
00115     nh_.param ("~dir", dir_, string(""));     //dir to fetch .log files from
00116     //get the list of .log files
00117     if(dir_ != "")
00118       {
00119         get_log_files(dir_, file_list_);
00120         save_to_pcd_laser_ = 0, save_to_pcd_actarray_ = 0;
00121       }
00122     nh_.param ("~min_distance", min_distance_, .7);     // minimum distance range to be considered
00123     nh_.param ("~max_distance", max_distance_, 3.01);   // maximum distance range to be considered
00124 
00125     nh_.param ("~laser_min_angle", laser_min_angle_, DBL_MIN);    // minimum range angle to be considered
00126     nh_.param ("~laser_max_angle", laser_max_angle_, DBL_MAX);    // maximum range angle to be considered
00127 
00128     // To make the code general, no translations with respect to the rotation origin are given here as default
00129     nh_.param ("~translations_x", translation_ (0), 0.0);
00130     nh_.param ("~translations_y", translation_ (1), 0.0);
00131     nh_.param ("~translations_z", translation_ (2), 0.0);
00132 
00133     nh_.param ("~left", left_arm_, true);
00134     string armDH;
00135     ROS_INFO ("Using the following DH parameters for the arm: ");
00136     if (left_arm_)
00137       nh_.param ("~larm_DH", armDH, string ());
00138     else
00139       nh_.param ("~rarm_DH", armDH, string ());
00140     getDHParameters (armDH, arm_params_);
00141     printDHParameters (arm_params_);
00142 
00143     actarray_sub_  = nh_.subscribe ("/player_actarray", 1000, &ActarrayCloudAssembler::actarray_cb, this);
00144     laserscan_sub_ = nh_.subscribe ("/laser_scan", 1000, &ActarrayCloudAssembler::scan_cb, this);
00145 
00146     cloud_pub_ = nh_.advertise<PointCloud> ("/tilt_laser_cloud", 1);
00147 
00148     cloud_.header.frame_id = "laser_tilt_mount_link";
00149     cloud_.channels.resize (7);
00150     cloud_.channels[0].name = "intensity";
00151     cloud_.channels[1].name = "distance";
00152     cloud_.channels[2].name = "sid";
00153     cloud_.channels[3].name = "pid";
00154     cloud_.channels[4].name = "vx";
00155     cloud_.channels[5].name = "vy";
00156     cloud_.channels[6].name = "vz";
00157 
00158     ROS_INFO ("Using the following translation values: %f, %f, %f", translation_ (0), translation_ (1), translation_ (2));
00159     first_act_stamp_ = -1.0;
00160   }
00161 
00163   virtual ~ActarrayCloudAssembler () { }
00164 
00166   // Parse a string representing the DH parameters for an actarray and convert to our internal DH structure
00167   void
00168   getDHParameters (string armDH, vector<DH> &arm_params)
00169   {
00170     // Split the line into tokens
00171     vector<string> st;
00172     boost::split (st, armDH, boost::is_any_of (" "));
00173 
00174     arm_params.resize ((int)(st.size () / 4));
00175     int j = 0;
00176     for (unsigned int i = 0; i < arm_params.size (); i++)
00177       {
00178         arm_params[i].type = 0;
00179         arm_params[i].t = atof (st.at (j++).c_str ());
00180         arm_params[i].d = atof (st.at (j++).c_str ());
00181         arm_params[i].l = atof (st.at (j++).c_str ());
00182         arm_params[i].a = atof (st.at (j++).c_str ());
00183       }
00184   }
00185 
00187   // Print a vector of DH parameters to screen
00188   void
00189   printDHParameters (const vector<DH> &params)
00190   {
00191     for (unsigned int i = 0; i < params.size (); i++)
00192       fprintf (stderr, "t: %4.4f     d: %4.4f     l: %4.4f     a: %4.4f\n", params[i].t, params[i].d, params[i].l, params[i].a);
00193   }
00194 
00196   // Obtain a transformaion matrix for a joint defined by its DH parameters and joint values
00197   inline void
00198   getJointTransformation (const DH &param, const double &q, Eigen::Matrix4d &T)
00199   {
00200     double t = param.t, d = param.d;
00201     t += (param.type == 0) ? q : 0;
00202     d += (param.type == 0) ? 0 : q;
00203     double st = sin (t), ct = cos (t);
00204     double sa = sin (param.a), ca = cos (param.a);
00205     T <<
00206       ct, -st * ca, +st * sa, param.l * ct,
00207       st, +ct * ca, -ct * sa, param.l * st,
00208       0,        sa,       ca,            d,
00209       0,         0,        0,            1;
00210   }
00211 
00213   // Obtain a global transormation matrix for an array of joints
00214   inline void
00215   getGlobalTransformation (const vector<DH> &params, const vector<double> &joints, Eigen::Matrix4d &global_transformation)
00216   {
00217     // Compute the transformation matrix as a product of relative transformations
00218     Eigen::Matrix4d local_transformation;
00219     getJointTransformation (params.at (0), joints.at (0), global_transformation);
00220     for (unsigned int i = 1; i < joints.size (); i++)
00221       {
00222         getJointTransformation (params[i], joints[i], local_transformation);
00223         global_transformation *= local_transformation;
00224       }
00225   }
00226 
00228   bool
00229   interpolateActarrayValues (const LaserScanConstPtr &laser_packet, vector<PlayerActarrayConstPtr> &actarrays,
00230                              vector<double> &q_values)
00231   {
00232     PlayerActarrayConstPtr prev_act = actarrays.at (0);
00233     if (laser_packet->header.stamp < prev_act->header.stamp)
00234       return (false);
00235 
00236     bool found = false;
00237     for (unsigned int la = 1; la < actarrays.size (); la++)
00238       {
00239         PlayerActarrayConstPtr cur_act = actarrays.at (la);
00240 
00241         // Check if the laser entry is between these two actarray entries
00242         if ((prev_act->header.stamp.toSec () <= laser_packet->header.stamp.toSec ()) && (laser_packet->header.stamp.toSec () <= cur_act->header.stamp.toSec ()))
00243           {
00244             double t0 = (laser_packet->header.stamp - prev_act->header.stamp).toSec ();
00245             double t1 = (cur_act->header.stamp - prev_act->header.stamp).toSec ();
00246 
00247             // Interpolate joint values
00248             for (unsigned int q_idx = 0; q_idx < cur_act->joints.size (); q_idx++)
00249               q_values[q_idx] = prev_act->joints[q_idx] + t0 * (cur_act->joints[q_idx] - prev_act->joints[q_idx]) / t1;
00250             found = true;
00251             break;
00252           }
00253 
00254         prev_act = cur_act;
00255       }
00256 
00257     return (found);
00258   }
00259 
00261   // PlayerActarray message callback
00262   void
00263   actarray_cb (const PlayerActarrayConstPtr &actarray)
00264   {
00265     a_lock_.lock (); 
00266     actarrays_.push_back (actarray); 
00267     a_lock_.unlock ();
00268   }
00269 
00271   // LaserScan message callback
00272   void
00273   scan_cb (const LaserScanConstPtr &scan)
00274   {
00275     ++total_laser_scans_;
00276     s_lock_.lock ();
00277     scans_.push_back (scan);
00278     s_lock_.unlock ();
00279   }
00280   
00282   // Get a list of log files
00284   void get_log_files ( const path & directory, vector <string> &file_list, string suffix=".log", bool recurse_into_subdirs = false )
00285   {
00286     if( exists( directory ) )
00287       {
00288         directory_iterator end ;
00289         for( directory_iterator iter(directory) ; iter != end ; ++iter )
00290           if ( is_directory( *iter ) )
00291             {
00292               ROS_WARN("Directory %s", iter->string().c_str());
00293               //if( recurse_into_subdirs ) get_log_files(*iter) ;
00294             }
00295           else 
00296             {
00297               int len = iter->string().length();
00298               int npos =  iter->string().rfind(suffix);
00299               if((len - npos) == suffix.length())
00300                 file_list.push_back(iter->string());
00301             }
00302       }
00303   }
00304     
00306   // Update parameters from server
00308   int
00309   update_parameters_from_server()
00310   {
00311     nh_.getParam("/save_pcd_laser", save_to_pcd_laser_);
00312     nh_.getParam("/save_pcd_actarray", save_to_pcd_actarray_);
00313     if (save_to_pcd_laser_ == 1 && save_to_pcd_actarray_ == 1)
00314       {
00315         nh_.setParam("/save_pcd_actarray", 2);
00316         nh_.setParam("/save_pcd_laser", 2);
00317         sleep(1);
00318         string pcd_file = cur_file_ + ".pcd";
00319         ROS_INFO("Saving to file: %s", pcd_file.c_str());
00320         //save PointCloud to pcd file
00321         savePCDFile (pcd_file.c_str(), cloud_, false);
00322         if (file_list_.size() == 0)
00323           return -1;
00324         cur_file_ = file_list_.back(), file_list_.pop_back();
00325         ROS_INFO("Re-starting player_[laser|actarray]_log_to_msg: %s. Filelist size: %d", cur_file_.c_str(), file_list_.size());
00326         nh_.setParam("/log_filename", cur_file_);
00327         return 1;
00328       }
00329     return 0;
00330   }
00331 
00332 
00334   bool
00335   spin ()
00336   {
00337     int laser_packet_scan_id = 1, point_cloud_total = 0;
00338     vector<double> q_values;
00339     Eigen::Vector4d pt, pt_t, vp, vp_old;
00340     Eigen::Matrix4d robot_transform;
00341     int nr_points = 0;
00342     if(dir_ != "")
00343       {
00344         cur_file_ = file_list_.back(), file_list_.pop_back();
00345         nh_.setParam("/log_filename", cur_file_);
00346         ROS_INFO("Starting (first time) player_[laser|actarray]_log_to_msg: %s. Filelist size: %d", cur_file_.c_str(), file_list_.size());
00347       }
00348     while (nh_.ok ())
00349       {  
00350         if(dir_ != "")
00351           {
00352             int update = update_parameters_from_server();
00353             if(update == 1)
00354               {
00355                 laser_packet_scan_id = 1, point_cloud_total = 0;
00356                 q_values.clear();
00357                 for (int i = 0; i < 4; i++)
00358                   pt(i)=0, pt_t(i)=0, vp(i)=0, vp_old(0);
00359                 for (int i = 0; i < 4; i++)
00360                   {
00361                     for (int j = 0; j < 4; j++)
00362                       robot_transform(i, j)=0;
00363                   }     
00364                 nr_points = 0;
00365                 actarrays_.clear();
00366                 scans_.clear();
00367                 ROS_INFO("Assembler reseting all values!");
00368               }
00369             //no more files to convert
00370             if (update == -1)
00371               break;
00372           }
00373 
00374         // We need at least 2 actarray values and 1 laser scan to begin
00375         if (actarrays_.size () < 2 || scans_.size () == 0)
00376           {
00377             usleep (500);
00378             ros::spinOnce ();
00379             continue;
00380           }
00381   
00382         q_values.resize (actarrays_.at (0)->joints.size ());
00383   
00384         // For each buffered laser packet
00385         for (list<LaserScanConstPtr>::iterator it = scans_.begin (); it != scans_.end ();)
00386           {
00387             LaserScanConstPtr laser_packet = *it;
00388       
00389             // Interpolate actarray values
00390             if (!interpolateActarrayValues (laser_packet, actarrays_, q_values))
00391               {
00392                 ++it;
00393                 continue;
00394               }
00395       
00396             // Obtain the transformation corresponding to the current joint values
00397             getGlobalTransformation (arm_params_, q_values, robot_transform);
00398             // Calculate the viewpoint for the current interpolated joint angles
00399             vp_old = vp;
00400             vp = robot_transform * translation_;
00401       
00402             if (vp (0) == vp_old (0) && vp (1) == vp_old (1) && vp (2) == vp_old (2))
00403               {
00404                 s_lock_.lock (); it = scans_.erase (it); s_lock_.unlock ();
00405                 continue;
00406               }
00407       
00408             // Calculate the horizontal angles and the cartesian coordinates
00409             double angle_x    = laser_packet->angle_min;
00410             double resolution = laser_packet->angle_increment;
00411       
00412             if(dir_ == "")
00413               nr_points = 0;
00414            
00415             if(dir_ == "")
00416               cloud_.points.resize (laser_packet->ranges.size ());
00417             else
00418               cloud_.points.resize (cloud_.points.size() + laser_packet->ranges.size ());
00419       
00420             if(dir_ == "")
00421               for (unsigned int d = 0; d < cloud_.channels.size (); d++)
00422                 cloud_.channels[d].values.resize (laser_packet->ranges.size ());
00423             else
00424               {
00425                 for (unsigned int d = 0; d < cloud_.channels.size (); d++)
00426                   cloud_.channels[d].values.resize (cloud_.channels[d].values.size() + laser_packet->ranges.size ());
00427               }
00428       
00429             for (unsigned int i = 0; i < laser_packet->ranges.size (); i++)
00430               {
00431                 double distance = laser_packet->ranges[i];
00432                 double intensity = laser_packet->intensities[i];
00433                 if ((distance > max_distance_) || (distance < min_distance_))
00434                   {
00435                     angle_x += resolution;
00436                     continue;
00437                   }
00438                 if ((angle_x < angles::from_degrees (laser_min_angle_)) || (angle_x > angles::from_degrees (laser_max_angle_)))
00439                   {
00440                     angle_x += resolution;
00441                     continue;
00442                   }
00443     
00444                 // 2D
00445                 pt(0) = translation_ (0) + 0.0;
00446                 pt(1) = translation_ (1) + distance * cos (M_PI - angle_x);
00447                 pt(2) = translation_ (2) + distance * sin (M_PI - angle_x);
00448                 pt(3) = 1.0;
00449     
00450                 // Transform the point
00451                 pt_t = robot_transform * pt;
00452                 cloud_.points[nr_points].x = pt_t(0);
00453                 cloud_.points[nr_points].y = pt_t(1);
00454                 cloud_.points[nr_points].z = pt_t(2);
00455     
00456                 // Save the rest of the values
00457                 cloud_.channels[0].values[nr_points] = intensity;
00458                 cloud_.channels[1].values[nr_points] = distance;
00459                 cloud_.channels[2].values[nr_points] = i;
00460                 cloud_.channels[3].values[nr_points] = laser_packet_scan_id++;
00461                 cloud_.channels[4].values[nr_points] = vp (0);
00462                 cloud_.channels[5].values[nr_points] = vp (1);
00463                 cloud_.channels[6].values[nr_points] = vp (2);
00464                 nr_points++;
00465     
00466                 angle_x += resolution;
00467               }
00468            
00469             if (nr_points == 0)
00470               {
00471                 s_lock_.lock (); it = scans_.erase (it); s_lock_.unlock ();
00472                 continue;
00473               }
00474             cloud_.points.resize (nr_points);
00475             for (unsigned int d = 0; d < cloud_.channels.size (); d++)
00476               cloud_.channels[d].values.resize (nr_points);
00477             cloud_.header.stamp = Time::now ();
00478             ROS_INFO ("Publishing a PointCloud message (%d) with %d points and %d channels on topic /%s  %d.", 
00479                       ++point_cloud_total, (int)cloud_.points.size (), (int)cloud_.channels.size (), tf_frame_.c_str (), scans_.size());
00480             cloud_pub_.publish (cloud_);
00481             s_lock_.lock (); it = scans_.erase (it); s_lock_.unlock ();
00482           }
00483         ros::spinOnce ();
00484       }
00485     return (true);
00486   }
00487   
00488 };
00489 
00490 /* ---[ */
00491 int
00492 main (int argc, char** argv)
00493 {
00494   init (argc, argv, "actarray_cloud_assembler");
00495 
00496   ActarrayCloudAssembler p;
00497   p.spin ();
00498 
00499   return (0);
00500 }
00501 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


player_log_actarray
Author(s): Radu Bogdan Rusu
autogenerated on Thu May 23 2013 18:58:38