VisualizationBase.cpp
Go to the documentation of this file.
00001 /*
00002  * VisualizationBase.cpp
00003  *
00004  *  Created on: Mar 20, 2015
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_visualization/visualizations/VisualizationBase.hpp"
00010 
00011 namespace grid_map_visualization {
00012 
00013 VisualizationBase::VisualizationBase(ros::NodeHandle& nodeHandle, const std::string& name)
00014     : nodeHandle_(nodeHandle),
00015       name_(name)
00016 {
00017 }
00018 
00019 VisualizationBase::~VisualizationBase()
00020 {
00021 }
00022 
00023 bool VisualizationBase::isActive() const
00024 {
00025   if (publisher_.getNumSubscribers() > 0) return true;
00026   return false;
00027 }
00028 
00029 bool VisualizationBase::readParameters(XmlRpc::XmlRpcValue& config)
00030 {
00031   if (config.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00032     ROS_ERROR("A filter configuration must be a map with fields name, type, and params.");
00033     return false;
00034   }
00035 
00036   // Check to see if we have parameters in our list.
00037   if (config.hasMember("params")) {
00038     XmlRpc::XmlRpcValue params = config["params"];
00039     if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00040       ROS_ERROR("Params must be a map.");
00041       return false;
00042     } else {
00043       for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) {
00044         ROS_DEBUG("Loading param %s\n", it->first.c_str());
00045         parameters_[it->first] = it->second;
00046       }
00047     }
00048   }
00049 
00050   return true;
00051 }
00052 
00053 bool VisualizationBase::getParam(const std::string& name, std::string& value)
00054 {
00055   StringMap::iterator it = parameters_.find(name);
00056   if (it == parameters_.end()) return false;
00057   if (it->second.getType() != XmlRpc::XmlRpcValue::TypeString) return false;
00058   value = std::string(it->second);
00059   return true;
00060 }
00061 
00062 bool VisualizationBase::getParam(const std::string& name, double& value)
00063 {
00064   StringMap::iterator it = parameters_.find(name);
00065   if (it == parameters_.end()) return false;
00066   if (it->second.getType() != XmlRpc::XmlRpcValue::TypeDouble
00067       && it->second.getType() != XmlRpc::XmlRpcValue::TypeInt) return false;
00068   value = it->second.getType() == XmlRpc::XmlRpcValue::TypeInt ?
00069           (int) (it->second) : (double) (it->second);
00070   return true;
00071 }
00072 
00073 bool VisualizationBase::getParam(const std::string& name, float& value)
00074 {
00075   double valueDouble;
00076   bool isSuccess = getParam(name, valueDouble);
00077   if (isSuccess) value = static_cast<float>(valueDouble);
00078   return isSuccess;
00079 }
00080 
00081 bool VisualizationBase::getParam(const std::string& name, bool& value)
00082 {
00083   StringMap::iterator it = parameters_.find(name);
00084   if (it == parameters_.end()) return false;
00085   if (it->second.getType() != XmlRpc::XmlRpcValue::TypeBoolean) return false;
00086   value = it->second;
00087   return true;
00088 }
00089 
00090 bool VisualizationBase::getParam(const std::string&name, int& value)
00091 {
00092   StringMap::iterator it = parameters_.find(name);
00093   if (it == parameters_.end()) return false;
00094   if(it->second.getType() != XmlRpc::XmlRpcValue::TypeInt) return false;
00095   value = it->second;
00096   return true;
00097 }
00098 
00099 } /* namespace */


grid_map_visualization
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:50