spin_laser_snapshotter.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <ros/ros.h>
00037 
00038 // Services
00039 #include "laser_assembler/AssembleScans2.h"
00040 
00041 // Messages
00042 #include "sensor_msgs/PointCloud2.h"
00043 #include "sensor_msgs/JointState.h"
00044 
00045 #include <tf_conversions/tf_eigen.h>
00046 #include <tf/transform_listener.h>
00047 /***
00048  * This uses the point_cloud_assembler's build_cloud service call to grab all the scans/clouds between two tilt-laser shutters
00049  * params
00050  *  * "~target_frame_id" (string) - This is the frame that the scanned data transformed into.  The
00051  *                                  output clouds are also published in this frame.
00052  *  * "~num_skips" (int)          - If set to N>0, then the snapshotter will skip N signals before
00053  *                                  requesting a new snapshot. This will make the snapshots be N times
00054  *                                  larger. Default 0 - no skipping.
00055  */
00056 
00057 namespace spin_laser_snapshotter
00058 {
00059 
00060 class SpinLaserSnapshotter
00061 {
00062 
00063 public:
00064   ros::NodeHandle n_;
00065   ros::NodeHandle private_ns_;
00066   ros::Publisher pub_;
00067   ros::Subscriber sub_;
00068 
00069   ros::Time prev_signal_;
00070   double prev_angle_;
00071 
00072   bool first_time_;
00073   bool negative_direction_;
00074   int num_skips_;
00075   int num_skips_left_;
00076   double rate_;
00077   std::string fixed_frame_;
00078   std::string spindle_frame_;
00079   std::string motor_frame_;
00080   tf::TransformListener* listener_;
00081   ros::Timer timer_;
00082   SpinLaserSnapshotter() : private_ns_("~")
00083   {
00084     prev_signal_.fromNSec(0);
00085 
00086     pub_ = n_.advertise<sensor_msgs::PointCloud2> ("full_cloud2", 1);
00087 
00088     private_ns_.param("num_skips", num_skips_, 0);
00089     num_skips_left_=num_skips_;
00090 
00091     prev_angle_ = -1;
00092     first_time_ = true;
00093     private_ns_.param("negative_direction", negative_direction_, false);
00094     ROS_INFO_STREAM("negative_direction: " << negative_direction_);
00095     bool use_tf;
00096     private_ns_.param("use_tf", use_tf, false);
00097     if (!use_tf) {
00098       sub_ = n_.subscribe("joint_states", 40, &SpinLaserSnapshotter::scannerSignalCallback, this);
00099     }
00100     else {
00101       listener_ = new tf::TransformListener();
00102       private_ns_.param("tf_polling_rate", rate_, 30.0);
00103       private_ns_.param("spindle_frame", spindle_frame_, std::string("multisense/spindle"));
00104       private_ns_.param("motor_frame", motor_frame_, std::string("multisense/motor"));
00105       timer_ = private_ns_.createTimer(
00106         ros::Duration(1.0 / rate_), boost::bind(
00107           &SpinLaserSnapshotter::timerCallback, this, _1));
00108     }
00109   }
00110 
00111   ~SpinLaserSnapshotter()
00112   {
00113 
00114   }
00115 
00116   void timerCallback(const ros::TimerEvent& event)
00117   {
00118     // only if it is possible to transform from motor_frame_ -> spindle_frame_
00119     if (listener_->waitForTransform(motor_frame_, spindle_frame_, event.current_real,
00120                                     ros::Duration(1 / rate_))) {
00121       tf::StampedTransform transform;
00122       listener_->lookupTransform(spindle_frame_, motor_frame_, event.current_real, transform);
00123       // compute quaternion into eigen
00124       Eigen::Affine3d t;
00125       tf::transformTFToEigen(transform, t);
00126       double yaw   = atan2(t(1,0), t(0,0));
00127       sensor_msgs::JointState js;
00128       js.header.stamp = transform.stamp_;
00129       js.position.push_back(yaw);
00130       const sensor_msgs::JointState js_const (js);
00131       scannerSignalCallback(boost::make_shared<sensor_msgs::JointState>(js_const));
00132     }
00133   }
00134   
00135   void scannerSignalCallback(const sensor_msgs::JointStateConstPtr& js)
00136   {
00137     double theta = js->position[0];
00138     if (negative_direction_) {
00139       theta = - theta;
00140     }
00141     double ang = fmod(theta, 2 * M_PI);
00142 
00143     // ROS_DEBUG("ang = %lf, prev_angle = %lf, %lf", ang, prev_angle_, prev_signal_.toSec());
00144 
00145     if ( prev_angle_ < 0 ) {
00146       prev_angle_ = ang;
00147       return;
00148     }
00149     if ((ang - prev_angle_) >= - M_PI) {
00150       prev_angle_ = ang;
00151       return;
00152     }
00153 
00154     if (prev_signal_.toSec() == 0.0) {
00155       first_time_ = true;
00156     }
00157 
00158     ros::Time stmp = js->header.stamp;
00159     if (first_time_)
00160     {
00161       prev_signal_ = stmp;
00162       first_time_ = false;
00163     }
00164     else
00165     {
00166       if (num_skips_ > 0)
00167       {
00168         if (num_skips_left_ > 0)
00169         {
00170           num_skips_left_ -= 1;
00171           return;
00172         }
00173         else
00174         {
00175           num_skips_left_ = num_skips_;
00176         }
00177       }
00178 
00179       laser_assembler::AssembleScans2::Request req;
00180       laser_assembler::AssembleScans2::Response res;
00181 
00182       req.begin = prev_signal_;
00183       req.end   = stmp;
00184       if (!ros::service::call("assemble_scans2", req, res))
00185         ROS_ERROR("Failed to call service on point cloud assembler or laser scan assembler.");
00186 
00187       pub_.publish(res.cloud);
00188       ROS_DEBUG("Snapshotter::Published Cloud size=%u", res.cloud.width * res.cloud.height);
00189 
00190       prev_signal_ = stmp;
00191       prev_angle_ = -1;
00192     }
00193   }
00194 };
00195 }
00196 
00197 using namespace spin_laser_snapshotter;
00198 
00199 int main(int argc, char **argv)
00200 {
00201   ros::init(argc, argv, "spin_laser_snapshotter");
00202   SpinLaserSnapshotter snapshotter ;
00203   ros::spin();
00204   return 0;
00205 }


jsk_tilt_laser
Author(s): YoheiKakiuchi
autogenerated on Fri Sep 8 2017 03:38:51