laser_joint_projector_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 
00038 #include <ros/console.h>
00039 #include <ros/ros.h>
00040 #include <laser_joint_projector/laser_joint_projector.h>
00041 #include <kdl_parser/kdl_parser.hpp>
00042 #include <calibration_msgs/JointStateCalibrationPattern.h>
00043 
00044 using namespace laser_joint_projector;
00045 using namespace std;
00046 
00047 void featuresCallback(ros::Publisher* pub,
00048                       LaserJointProjector* projector,
00049                       const calibration_msgs::JointStateCalibrationPatternConstPtr& msg)
00050 {
00051   sensor_msgs::PointCloud cloud = projector->project(msg->joint_points, ros::Duration());
00052   cloud.header.stamp = msg->header.stamp;
00053   cloud.header.frame_id = "torso_lift_link"; 
00054   pub->publish(cloud);
00055 }
00056 
00057 int main(int argc, char** argv)
00058 {
00059   ros::init(argc, argv, "projector");
00060   ros::NodeHandle n;
00061 
00062   KDL::Tree my_tree;
00063   std::string robot_desc_string;
00064   if (!n.getParam("robot_description", robot_desc_string))
00065     ROS_FATAL("Couldn't get a robot_description from the param server");
00066 
00067   if (!kdl_parser::treeFromString(robot_desc_string, my_tree))
00068   {
00069      ROS_ERROR("Failed to construct kdl tree");
00070      return false;
00071   }
00072 
00073 //  KDL::Tree my_tree;
00074 //  string filename= "/u/vpradeep/ros/pkgs/ros-pkg/stacks/robot_calibration/laser_joint_projector/pr2.urdf";
00075 //  if (!kdl_parser::treeFromFile(filename, my_tree))
00076 //  {
00077 //     ROS_ERROR("Failed to construct kdl tree");
00078 //     return -1;
00079 //  }
00080 
00081   LaserJointProjector projector;
00082   projector.configure(my_tree, "torso_lift_link", "laser_tilt_link");
00083 
00084   // Output
00085   ros::Publisher pub = n.advertise<sensor_msgs::PointCloud>("checkerboard_points", 1);
00086 
00087   // Input
00088   boost::function<void (const calibration_msgs::JointStateCalibrationPatternConstPtr&)> cb
00089       = boost::bind(&featuresCallback, &pub, &projector, _1);
00090 
00091   ros::Subscriber sub = n.subscribe(std::string("joint_state_features"), 1, cb);
00092 
00093   ros::spin();
00094 
00095   return 0;
00096 }


laser_joint_projector
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:34