camera_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 _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       //std::string frame_id = "";
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 }; // end of class definition
00113 
00115 inline CameraConfigurations& cameraConfigurations()
00116 {
00117   static CameraConfigurations camera_configs;
00118   return camera_configs;
00119 }
00120 
00121 } //namespace object_manipulator
00122 
00123 #endif


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