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 _CAMERA_CONFIGURATIONS_H_
00034 #define _CAMERA_CONFIGURATIONS_H_
00035
00036 #include <ros/ros.h>
00037
00038 #include <cmath>
00039
00040 #include <geometry_msgs/Vector3Stamped.h>
00041 #include <geometry_msgs/PointStamped.h>
00042
00043 #include "object_manipulator/tools/exceptions.h"
00044
00045 #include "configuration_loader.h"
00046
00047 namespace object_manipulator {
00048
00049 class CameraConfigurations : public ConfigurationLoader
00050 {
00051 private:
00052
00053 public:
00054 CameraConfigurations() {}
00055
00056 inline std::vector< double > get_camera_pose( const std::string &position_name )
00057 {
00058 ROS_WARN("This functionality is deprecated. Use get_camera_placement instead.");
00059 std::string name = "/im_camera_configurations/" + position_name;
00060 std::vector<double> values = getVectorDoubleParam(name);
00061 if (values.size() != 6) throw BadParamException(name);
00062 return values;
00063 }
00064
00065 inline bool get_camera_placement( const std::string &position_name,
00066 geometry_msgs::PointStamped &eye,
00067 geometry_msgs::PointStamped &focus,
00068 geometry_msgs::Vector3Stamped &up)
00069 {
00070 try
00071 {
00072 std::string name = "/im_camera_placements/" + position_name;
00073
00074 eye.header.stamp = ros::Time(0);
00075 eye.header.frame_id = getStringParam(name + "/eye/frame_id");
00076 focus.header.stamp = ros::Time(0);
00077 focus.header.frame_id = getStringParam(name + "/focus/frame_id");
00078 up.header.stamp = ros::Time(0);
00079 up.header.frame_id = getStringParam(name + "/up/frame_id");
00080
00081 std::vector<double> eye_values = getVectorDoubleParam(name + "/eye/point");
00082 if (eye_values.size() != 3) throw BadParamException(name + "/eye/point");
00083 eye.point.x = eye_values[0];
00084 eye.point.y = eye_values[1];
00085 eye.point.z = eye_values[2];
00086
00087 std::vector<double> focus_values = getVectorDoubleParam(name + "/focus/point");
00088 if (focus_values.size() != 3) throw BadParamException(name + "/focus/point");
00089 focus.point.x = focus_values[0];
00090 focus.point.y = focus_values[1];
00091 focus.point.z = focus_values[2];
00092
00093 std::vector<double> up_values = getVectorDoubleParam(name + "/up/vector");
00094 if (up_values.size() != 3) throw BadParamException(name + "/up/vector");
00095 up.vector.x = up_values[0];
00096 up.vector.y = up_values[1];
00097 up.vector.z = up_values[2];
00098
00099 return true;
00100 }
00101 catch(object_manipulator::BadParamException &ex)
00102 {
00103 ROS_ERROR("%s \n (Parameters that represent doubles or lists of doubles need to have decimal points.)", ex.what());
00104 }
00105 catch(object_manipulator::MissingParamException &ex)
00106 {
00107 ROS_ERROR("%s", ex.what());
00108 }
00109 return false;
00110 }
00111
00112 };
00113
00115 inline CameraConfigurations& cameraConfigurations()
00116 {
00117 static CameraConfigurations camera_configs;
00118 return camera_configs;
00119 }
00120
00121 }
00122
00123 #endif