arm_configurations.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
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
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 #ifndef _ARM_CONFIGURATIONS_H_
00034 #define _ARM_CONFIGURATIONS_H_
00035 
00036 #include <ros/ros.h>
00037 
00038 #include <cmath>
00039 
00040 #include <geometry_msgs/Vector3.h>
00041 
00042 #include "object_manipulator/tools/exceptions.h"
00043 
00044 #include "configuration_loader.h"
00045 
00046 namespace object_manipulator {
00047 
00048 class ArmConfigurations : public ConfigurationLoader
00049 {
00050  private:
00051 
00052  public:
00053  ArmConfigurations() {}
00054 
00055   inline std::vector< double > position(std::string arm_name, std::string position)
00056   {
00057     std::string name = "/arm_configurations/" + position + "/position/" + arm_name;
00058     std::vector<double> values = getVectorDoubleParam(name);
00059     if ( values.size() != 7 )  throw BadParamException(name);
00060     return values;
00061   }
00062 
00063   inline std::vector< std::vector<double> > trajectory(std::string arm_name, std::string position)
00064   {
00065     std::string name = "/arm_configurations/" + position + "/trajectory/" + arm_name;
00066     std::vector<double> values = getVectorDoubleParam(name);
00067     if ( values.size() % 7 != 0 )  throw BadParamException(name);
00068     std::vector< std::vector<double> > traj;
00069     int waypoints = values.size() / 7;
00070     traj.resize( waypoints );
00071     for(int i = 0; i < waypoints; i++)
00072     {
00073       std::vector<double> pos;
00074       traj[i].insert(pos.begin(), values.begin() + i*7, values.begin() + (i+1)*7 );
00075     }
00076     return traj;
00077   }
00078 
00079 };
00080 
00082 inline ArmConfigurations& armConfigurations()
00083 {
00084   static ArmConfigurations arm_configs;
00085   return arm_configs;
00086 }
00087 
00088 } //namespace object_manipulator
00089 
00090 #endif


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Mon Oct 6 2014 02:59:50