time_alignment_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
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 
00036 
00037 #include <ros/ros.h>
00038 #include <laser_joint_projector/laser_joint_projector.h>
00039 #include <kdl_parser/kdl_parser.hpp>
00040 #include <calibration_msgs/JointStateCalibrationPattern.h>
00041 #include <std_msgs/Float64.h>
00042 #include <boost/thread/mutex.hpp>
00043 
00044 using namespace laser_joint_projector;
00045 using namespace std;
00046 
00047 class TimeAlignmentNode
00048 {
00049 public:
00050   TimeAlignmentNode()
00051   {
00052     cloud_num_ = 0;
00053 
00054     std::string robot_desc_string;
00055     nh_.param("robot_description", robot_desc_string, string());
00056     if (!kdl_parser::treeFromString(robot_desc_string, tree_))
00057        ROS_FATAL("Failed to construct kdl tree");
00058 
00059     pub0_ = nh_.advertise<sensor_msgs::PointCloud>("cb_cloud_0", 1);
00060     pub1_ = nh_.advertise<sensor_msgs::PointCloud>("cb_cloud_1", 1);
00061     sub_features_ = nh_.subscribe("joint_state_features", 1, &TimeAlignmentNode::featuresCallback, this);
00062     sub_offset_   = nh_.subscribe("keyboard_float", 1, &TimeAlignmentNode::offsetCallback, this);
00063 
00064     ros::NodeHandle private_nh_("~");
00065 
00066     private_nh_.param("root", root_, std::string("root_link"));
00067     private_nh_.param("tip",  tip_,  std::string("tip_link"));
00068 
00069     projector_.configure(tree_, root_, tip_);
00070   }
00071 
00072 private:
00073   ros::NodeHandle nh_;
00074   unsigned int cloud_num_;
00075   KDL::Tree tree_;
00076   ros::Publisher pub0_;
00077   ros::Publisher pub1_;
00078   ros::Subscriber sub_features_;
00079   ros::Subscriber sub_offset_;
00080   LaserJointProjector projector_;
00081 
00082   double offset_;
00083   std::string root_;
00084   std::string tip_;
00085 
00086   boost::mutex mutex_;
00087 
00088   calibration_msgs::JointStateCalibrationPatternConstPtr features_0_;
00089   calibration_msgs::JointStateCalibrationPatternConstPtr features_1_;
00090 
00091   void featuresCallback(const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
00092   {
00093     boost::mutex::scoped_lock lock(mutex_);
00094 
00095     if (cloud_num_ == 0)
00096       features_0_ = msg;
00097     else if (cloud_num_ == 1)
00098       features_1_ = msg;
00099     else
00100       ROS_FATAL("Cloud num is [%u]", cloud_num_);
00101 
00102     // Toggle the cloud #
00103     cloud_num_ = 1 - cloud_num_;
00104 
00105     publish();
00106   }
00107 
00108   void offsetCallback(const std_msgs::Float64ConstPtr& offset)
00109   {
00110     boost::mutex::scoped_lock lock(mutex_);
00111     offset_ = offset->data;
00112     publish();
00113   }
00114 
00115   void publish()
00116   {
00117     ros::Duration offset(offset_);
00118 
00119     sensor_msgs::PointCloud cloud_0;
00120     sensor_msgs::PointCloud cloud_1;
00121 
00122     if (features_0_)
00123     {
00124       cloud_0 = projector_.project(features_0_->joint_points, offset);
00125       cloud_0.header.stamp = features_0_->header.stamp;
00126     }
00127 
00128     if (features_1_)
00129     {
00130       cloud_1 = projector_.project(features_1_->joint_points, offset);
00131       cloud_1.header.stamp = features_1_->header.stamp;
00132     }
00133 
00134     cloud_0.header.frame_id = root_;
00135     cloud_1.header.frame_id = root_;
00136 
00137     pub0_.publish(cloud_0);
00138     pub1_.publish(cloud_1);
00139   }
00140 
00141 };
00142 
00143 
00144 int main(int argc, char** argv)
00145 {
00146   ros::init(argc, argv, "time_alignment");
00147 
00148   TimeAlignmentNode alignment_node;
00149 
00150   ros::spin();
00151 
00152   return 0;
00153 }


laser_joint_projector
Author(s): Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:06:30