pr2_fk_ref.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 <kdl_parser/kdl_parser.hpp>
00041 #include <pr2_calibration_launch/FkTest.h>
00042 #include <kdl/tree.hpp>
00043 #include <kdl/chainfksolverpos_recursive.hpp>
00044 
00045 using namespace std;
00046 using namespace pr2_calibration_launch;
00047 
00048 bool fk(KDL::Tree* tree, FkTest::Request& req, FkTest::Response& resp)
00049 {
00050   KDL::Chain my_chain;
00051   bool success;
00052   success = tree->getChain(req.root, req.tip, my_chain);
00053 
00054   if (!success)
00055   {
00056     ROS_ERROR("Error extracting chain from [%s] to [%s]\n", req.root.c_str(), req.tip.c_str());
00057     return false;
00058   }
00059 
00060   KDL::ChainFkSolverPos_recursive solver(my_chain);
00061 
00062   KDL::JntArray joints(my_chain.getNrOfJoints());
00063   for (unsigned int i=0; i<my_chain.getNrOfJoints(); i++)
00064     joints(i) = req.joint_positions[i];
00065 
00066   KDL::Frame out_frame;
00067   if (solver.JntToCart(joints, out_frame))
00068   {
00069     ROS_ERROR("Error running KDL solver");
00070     return false;
00071   }
00072 
00073   resp.pos.resize(3);
00074 
00075   // Copy over translation vector
00076   resp.pos[0] = out_frame.p.data[0];
00077   resp.pos[1] = out_frame.p.data[1];
00078   resp.pos[2] = out_frame.p.data[2];
00079 
00080   // Copy over rotation matrix
00081   resp.rot.resize(9);
00082   for (unsigned int i=0; i<9; i++)
00083     resp.rot[i] = out_frame.M.data[i];
00084 
00085   return true;
00086 }
00087 
00088 int main(int argc, char** argv)
00089 {
00090   ros::init(argc, argv, "fk_reference");
00091   ros::NodeHandle n;
00092   ros::NodeHandle pn("~");
00093 
00094   KDL::Tree my_tree;
00095   string robot_desc_string;
00096   if (!n.getParam("robot_description", robot_desc_string))
00097     ROS_FATAL("Couldn't get a robot_description from the param server");
00098 
00099   if (!kdl_parser::treeFromString(robot_desc_string, my_tree))
00100   {
00101      ROS_ERROR("Failed to construct kdl tree");
00102      return false;
00103   }
00104 
00105   boost::function< bool(FkTest::Request&,  FkTest::Response&) > func = boost::bind(fk, &my_tree, _1, _2) ;
00106   ros::ServiceServer fk_service = n.advertiseService("fk", func);
00107 
00108   ros::spin();
00109 
00110   return 0;
00111 }


pr2_calibration_launch
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:37