Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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 }
00089
00090 #endif