parameters.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include "parameters.h"
00030 #include <set>
00031 
00032 #include <ros/node_handle.h>
00033 #include <ros/console.h>
00034 
00035 #include <hector_nav_msgs/GetDistanceToObstacle.h>
00036 
00037 namespace hector_object_tracker {
00038 
00039 std::map<std::string, bool>   _project_objects;
00040 std::map<std::string, double> _default_distance;
00041 std::map<std::string, double> _distance_variance;
00042 std::map<std::string, double> _angle_variance;
00043 std::map<std::string, double> _min_height;
00044 std::map<std::string, double> _max_height;
00045 std::map<std::string, double> _pending_support;
00046 std::map<std::string, double> _pending_time;
00047 std::map<std::string, double> _active_support;
00048 std::map<std::string, double> _active_time;
00049 std::map<std::string, double> _inactive_support;
00050 std::map<std::string, double> _inactive_time;
00051 std::map<std::string, std_msgs::ColorRGBA> _marker_color;
00052 std::map<std::string, ros::ServiceClient> _distance_to_obstacle_service;
00053 
00054 namespace Parameters {
00055 
00056   void load(const std::string &class_id /* = std::string() */)
00057   {
00058     static std::set<std::string> _per_class_parameters_loaded;
00059 
00060     if (_per_class_parameters_loaded.count(class_id)) return;
00061     ros::NodeHandle priv_nh("~");
00062 
00063     std::string prefix = class_id + "/";
00064     if (prefix == "/") prefix.clear();
00065 
00066     if (priv_nh.hasParam(prefix + "project_objects"))
00067       priv_nh.getParam(prefix + "project_objects", _project_objects[class_id]);
00068     if (priv_nh.hasParam(prefix + "default_distance"))
00069       priv_nh.getParam(prefix + "default_distance", _default_distance[class_id]);
00070     if (priv_nh.hasParam(prefix + "distance_variance"))
00071       priv_nh.getParam(prefix + "distance_variance", _distance_variance[class_id]);
00072     if (priv_nh.hasParam(prefix + "angle_variance"))
00073       priv_nh.getParam(prefix + "angle_variance", _angle_variance[class_id]);
00074     if (priv_nh.hasParam(prefix + "min_height"))
00075       priv_nh.getParam(prefix + "min_height", _min_height[class_id]);
00076     if (priv_nh.hasParam(prefix + "max_height"))
00077       priv_nh.getParam(prefix + "max_height", _max_height[class_id]);
00078     if (priv_nh.hasParam(prefix + "pending_support"))
00079       priv_nh.getParam(prefix + "pending_support", _pending_support[class_id]);
00080     if (priv_nh.hasParam(prefix + "pending_time"))
00081       priv_nh.getParam(prefix + "pending_time", _pending_time[class_id]);
00082     if (priv_nh.hasParam(prefix + "active_support"))
00083       priv_nh.getParam(prefix + "active_support", _active_support[class_id]);
00084     if (priv_nh.hasParam(prefix + "active_time"))
00085       priv_nh.getParam(prefix + "active_time", _active_time[class_id]);
00086     if (priv_nh.hasParam(prefix + "inactive_support"))
00087       priv_nh.getParam(prefix + "inactive_support", _inactive_support[class_id]);
00088     if (priv_nh.hasParam(prefix + "inactive_time"))
00089       priv_nh.getParam(prefix + "inactive_time", _inactive_time[class_id]);
00090 
00091     if (priv_nh.hasParam(prefix + "marker_color")) {
00092       XmlRpc::XmlRpcValue color;
00093       priv_nh.getParam(prefix + "marker_color", color);
00094       if (color.getType() == XmlRpc::XmlRpcValue::TypeArray && color.size() >= 3) {
00095         _marker_color[class_id].r = (double) color[0];
00096         _marker_color[class_id].g = (double) color[1];
00097         _marker_color[class_id].b = (double) color[2];
00098         if (color.size() > 3)
00099           _marker_color[class_id].a = (double) color[3];
00100         else
00101           _marker_color[class_id].a = 1.0;
00102       } else {
00103         ROS_ERROR_STREAM("Illegal value for param marker_color: " << color << ". Must be an array of size 3 or 4.");
00104       }
00105     }
00106 
00107     std::string distance_to_obstacle_service;
00108     if (priv_nh.hasParam(prefix + "distance_to_obstacle_service")) {
00109       priv_nh.getParam(prefix + "distance_to_obstacle_service", distance_to_obstacle_service);
00110     } else if (class_id.empty()) {
00111       distance_to_obstacle_service = "get_distance_to_obstacle";
00112     }
00113 
00114     if (!distance_to_obstacle_service.empty()) {
00115       _distance_to_obstacle_service[class_id] = ros::NodeHandle().serviceClient<hector_nav_msgs::GetDistanceToObstacle>(distance_to_obstacle_service);
00116       bool project_objects = _project_objects.count(class_id) ? _project_objects.at(class_id) : _project_objects.at(std::string());
00117       if (project_objects && !_distance_to_obstacle_service[class_id].waitForExistence(ros::Duration(5.0))) {
00118         ROS_WARN_STREAM("project_objects is true, but GetDistanceToObstacle service is not (yet) available" << (!class_id.empty() ? "for class " + class_id : ""));
00119       }
00120     }
00121 
00122     _per_class_parameters_loaded.insert(class_id);
00123   }
00124 
00125 } // namespace Parameters
00126 } // namespace hector_object_tracker
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:50:57