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 #include <hector_nav_msgs/GetNormal.h>
00037 
00038 namespace hector_object_tracker {
00039 
00040 std::map<std::string, bool>   _project_objects;
00041 std::map<std::string, bool>   _with_orientation;
00042 std::map<std::string, double> _default_distance;
00043 std::map<std::string, double> _distance_variance;
00044 std::map<std::string, double> _angle_variance;
00045 std::map<std::string, double> _min_height;
00046 std::map<std::string, double> _max_height;
00047 std::map<std::string, double> _pending_support;
00048 std::map<std::string, double> _pending_time;
00049 std::map<std::string, double> _active_support;
00050 std::map<std::string, double> _active_time;
00051 std::map<std::string, double> _inactive_support;
00052 std::map<std::string, double> _inactive_time;
00053 std::map<std::string, double> _min_distance_between_objects;
00054 std::map<std::string, std_msgs::ColorRGBA> _marker_color;
00055 std::map<std::string, ros::ServiceClientPtr> _distance_to_obstacle_service;
00056 std::map<std::string, ros::ServiceClientPtr> _get_normal_service;
00057 std::map<std::string, ServiceClientsWithProperties> _percept_verification_services;
00058 std::map<std::string, ServiceClientsWithProperties> _object_verification_services;
00059 
00060 namespace Parameters {
00061 
00062   void load(const std::string &class_id /* = std::string() */)
00063   {
00064     static std::set<std::string> _per_class_parameters_loaded;
00065 
00066     if (_per_class_parameters_loaded.count(class_id)) return;
00067     ros::NodeHandle priv_nh("~");
00068 
00069     std::string prefix = class_id + "/";
00070     if (prefix == "/") prefix.clear();
00071 
00072     if (priv_nh.hasParam(prefix + "project_objects"))
00073       priv_nh.getParam(prefix + "project_objects", _project_objects[class_id]);
00074     if (priv_nh.hasParam(prefix + "with_orientation"))
00075       priv_nh.getParam(prefix + "with_orientation", _with_orientation[class_id]);
00076     if (priv_nh.hasParam(prefix + "default_distance"))
00077       priv_nh.getParam(prefix + "default_distance", _default_distance[class_id]);
00078     if (priv_nh.hasParam(prefix + "distance_variance"))
00079       priv_nh.getParam(prefix + "distance_variance", _distance_variance[class_id]);
00080     if (priv_nh.hasParam(prefix + "angle_variance"))
00081       priv_nh.getParam(prefix + "angle_variance", _angle_variance[class_id]);
00082     if (priv_nh.hasParam(prefix + "min_height"))
00083       priv_nh.getParam(prefix + "min_height", _min_height[class_id]);
00084     if (priv_nh.hasParam(prefix + "max_height"))
00085       priv_nh.getParam(prefix + "max_height", _max_height[class_id]);
00086     if (priv_nh.hasParam(prefix + "pending_support"))
00087       priv_nh.getParam(prefix + "pending_support", _pending_support[class_id]);
00088     if (priv_nh.hasParam(prefix + "pending_time"))
00089       priv_nh.getParam(prefix + "pending_time", _pending_time[class_id]);
00090     if (priv_nh.hasParam(prefix + "active_support"))
00091       priv_nh.getParam(prefix + "active_support", _active_support[class_id]);
00092     if (priv_nh.hasParam(prefix + "active_time"))
00093       priv_nh.getParam(prefix + "active_time", _active_time[class_id]);
00094     if (priv_nh.hasParam(prefix + "inactive_support"))
00095       priv_nh.getParam(prefix + "inactive_support", _inactive_support[class_id]);
00096     if (priv_nh.hasParam(prefix + "inactive_time"))
00097       priv_nh.getParam(prefix + "inactive_time", _inactive_time[class_id]);
00098     if (priv_nh.hasParam(prefix + "min_distance_between_objects"))
00099       priv_nh.getParam(prefix + "min_distance_between_objects", _min_distance_between_objects[class_id]);
00100 
00101     if (priv_nh.hasParam(prefix + "marker_color")) {
00102       XmlRpc::XmlRpcValue color;
00103       priv_nh.getParam(prefix + "marker_color", color);
00104       if (color.getType() == XmlRpc::XmlRpcValue::TypeArray && color.size() >= 3) {
00105         _marker_color[class_id].r = (double) color[0];
00106         _marker_color[class_id].g = (double) color[1];
00107         _marker_color[class_id].b = (double) color[2];
00108         if (color.size() > 3)
00109           _marker_color[class_id].a = (double) color[3];
00110         else
00111           _marker_color[class_id].a = 1.0;
00112       } else {
00113         ROS_ERROR_STREAM("Illegal value for param marker_color: " << color << ". Must be an array of size 3 or 4.");
00114       }
00115     }
00116 
00117     std::string distance_to_obstacle_service;
00118     if (priv_nh.hasParam(prefix + "distance_to_obstacle_service")) {
00119       priv_nh.getParam(prefix + "distance_to_obstacle_service", distance_to_obstacle_service);
00120     } else if (class_id.empty()) {
00121       distance_to_obstacle_service = "get_distance_to_obstacle";
00122     }
00123 
00124     if (!distance_to_obstacle_service.empty()) {
00125       _distance_to_obstacle_service[class_id].reset(new ros::ServiceClient(ros::NodeHandle().serviceClient<hector_nav_msgs::GetDistanceToObstacle>(distance_to_obstacle_service)));
00126       bool project_objects = _project_objects.count(class_id) ? _project_objects.at(class_id) : _project_objects.at(std::string());
00127       if (project_objects && !_distance_to_obstacle_service[class_id]->waitForExistence(ros::Duration(5.0))) {
00128         ROS_WARN_STREAM("project_objects is true, but GetDistanceToObstacle service is not (yet) available" << (!class_id.empty() ? "for class " + class_id : ""));
00129       }
00130     }
00131 
00132     if (priv_nh.hasParam(prefix + "get_normal_service")) {
00133       std::string get_normal_service;
00134       priv_nh.getParam(prefix + "get_normal_service", get_normal_service);
00135       _get_normal_service[class_id].reset(new ros::ServiceClient(ros::NodeHandle().serviceClient<hector_nav_msgs::GetNormal>(get_normal_service)));
00136     }
00137 
00138     _per_class_parameters_loaded.insert(class_id);
00139   }
00140 
00141 } // namespace Parameters
00142 } // namespace hector_object_tracker


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Thu Jun 6 2019 20:20:14