GraftParameterManager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
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  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Chad Rockey
00032  */
00033 
00034 #include <graft/GraftParameterManager.h>
00035 
00036 
00037 // Commonly used parameter loading function
00038 template <std::size_t SIZE>
00039 bool load_rosparam_array(ros::NodeHandle& nh,
00040                          const std::string& param_name,
00041                          boost::array<double, SIZE>& values)
00042 {
00043   XmlRpc::XmlRpcValue xml_array;
00044   if (nh.getParam(param_name, xml_array))
00045   {
00046     if (xml_array.size() == SIZE)
00047     {
00048       for (size_t i = 0; i < xml_array.size(); i++)
00049       {
00050         std::stringstream ss; // Convert the list element into doubles
00051         ss << xml_array[i];
00052         ss >> values[i] ? values[i] : 0;
00053       }
00054       return true;  // loaded params into array
00055     }
00056     else
00057     {
00058       ROS_WARN("%s/%s parameter requires %d elements, skipping.",
00059                nh.getNamespace().c_str(), param_name.c_str(), static_cast<int>(SIZE));
00060       return false;
00061     }
00062   }
00063   return false;
00064 }
00065 
00066 
00067 GraftParameterManager::GraftParameterManager(ros::NodeHandle n, ros::NodeHandle pnh):
00068   n_(n), pnh_(pnh), include_pose_(false)
00069 {
00070 }
00071 
00072 
00073 GraftParameterManager::~GraftParameterManager()
00074 {
00075 }
00076 
00077 
00078 void GraftParameterManager::parseNavMsgsOdometryParameters(
00079     ros::NodeHandle& tnh,
00080     boost::shared_ptr<GraftOdometryTopic>& odom)
00081 {
00082   // Check how to use this sensor
00083   bool absolute_pose, delta_pose, use_velocities;
00084   double timeout;
00085   tnh.param<bool>("absolute_pose", absolute_pose, false);
00086   tnh.param<bool>("delta_pose", delta_pose, false);
00087   tnh.param<bool>("use_velocities", use_velocities, false);
00088   tnh.param<double>("timeout", timeout, 1.0);
00089 
00090   // Check for incompatible usage
00091   if (absolute_pose == true)
00092   {
00093     delta_pose = false;  // Should not use for both absolute position and velocity
00094   }
00095   if (delta_pose == true)
00096   {
00097     use_velocities = false;  // Should not use both estimated and reported velocities
00098   }
00099 
00100   // Apply to global state
00101   include_pose_ = include_pose_ || absolute_pose;
00102 
00103   // Apply to sensor
00104   odom->useAbsolutePose(absolute_pose);
00105   odom->useDeltaPose(delta_pose);
00106   odom->useVelocities(use_velocities);
00107   odom->setTimeout(timeout);
00108 
00109   //ROS_INFO("Abs pose: %d\nDelta pose: %d\nUse Vel: %d\nTimeout: %.3f", absolute_pose, delta_pose, use_velocities, timeout);
00110 
00111   // Set covariances
00112   boost::array<double, 36> covariance;
00113   if (load_rosparam_array(tnh, "override_pose_covariance", covariance))
00114     odom->setPoseCovariance(covariance);
00115   if (load_rosparam_array(tnh, "override_twist_covariance", covariance))
00116     odom->setTwistCovariance(covariance);
00117 }
00118 
00119 void GraftParameterManager::parseSensorMsgsIMUParameters(
00120     ros::NodeHandle& tnh,
00121     boost::shared_ptr<GraftImuTopic>& imu)
00122 {
00123   // Check how to use this sensor
00124   bool absolute_orientation, delta_orientation, use_velocities, use_accelerations;
00125   double timeout;
00126   tnh.param<bool>("absolute_orientation", absolute_orientation, false);
00127   tnh.param<bool>("delta_orientation", delta_orientation, false);
00128   tnh.param<bool>("use_velocities", use_velocities, false);
00129   tnh.param<bool>("use_accelerations", use_accelerations, false);
00130   tnh.param<double>("timeout", timeout, 1.0);
00131 
00132   // Check for incompatible usage
00133   if (absolute_orientation == true){
00134     delta_orientation = false;  // Should not use for both absolute position and velocity
00135   }
00136   if (delta_orientation == true){
00137     use_velocities = false;  // Should not use both estimated and reported velocities
00138   }
00139 
00140   // Apply to global state
00141   include_pose_ = include_pose_ || absolute_orientation;
00142 
00143   // Apply to sensor
00144   //imu->useAbsoluteOrientation(absolute_orientation);
00145   imu->useDeltaOrientation(delta_orientation);
00146   //imu->useVelocities(use_velocities);
00147   //imu->useAccelerations(use_accelerations);
00148   imu->setTimeout(timeout);
00149 
00150   ROS_INFO("Abs orientation: %d\nDelta orientation: %d\nUse Vel: %d\nTimeout: %.3f",
00151     absolute_orientation, delta_orientation, use_velocities, timeout);
00152 
00153   // Set covariances
00154   boost::array<double, 9> covariance;
00155   if (load_rosparam_array(tnh, "override_orientation_covariance", covariance))
00156     imu->setOrientationCovariance(covariance);
00157   if (load_rosparam_array(tnh, "override_angular_velocity_covariance", covariance))
00158     imu->setAngularVelocityCovariance(covariance);
00159   if (load_rosparam_array(tnh, "override_linear_acceleration_covariance", covariance))
00160     imu->setLinearAccelerationCovariance(covariance);
00161 }
00162 
00163 void GraftParameterManager::loadParameters(
00164     std::vector<boost::shared_ptr<GraftSensor> >& topics,
00165     std::vector<ros::Subscriber>& subs)
00166 {
00167   // Filter behavior parameters
00168   pnh_.param<std::string>("filter_type", filter_type_, "EKF");
00169   pnh_.param<bool>("planar_output", planar_output_, true);
00170 
00171   pnh_.param<std::string>("parent_frame_id", parent_frame_id_, "odom");
00172   pnh_.param<std::string>("child_frame_id", child_frame_id_, "base_link");
00173 
00174   pnh_.param<double>("freq", update_rate_, 50.0);
00175   pnh_.param<double>("update_rate", update_rate_, update_rate_); // Overrides 'freq'
00176   pnh_.param<double>("dt_override", dt_override_, 0.0);
00177 
00178   pnh_.param<bool>("publish_tf", publish_tf_, false);
00179 
00180   pnh_.param<int>("queue_size", queue_size_, 1);
00181 
00182   pnh_.param<double>("alpha", alpha_, 0.001);
00183   pnh_.param<double>("kappa", kappa_, 0.0);
00184   pnh_.param<double>("beta", beta_, 2.0);
00185 
00186   // Initial covariance
00187   XmlRpc::XmlRpcValue xml_initial_covariance;
00188   if (pnh_.getParam("initial_covariance", xml_initial_covariance))
00189   {
00190     initial_covariance_.resize(xml_initial_covariance.size());
00191     for (size_t i = 0; i < xml_initial_covariance.size(); i++)
00192     {
00193       std::stringstream ss;  // Convert the list element into doubles
00194       ss << xml_initial_covariance[i];
00195       ss >> initial_covariance_[i] ? initial_covariance_[i] : 0;
00196     }
00197   }
00198 
00199   // Process noise covariance
00200   XmlRpc::XmlRpcValue xml_process_noise;
00201   if (pnh_.getParam("process_noise", xml_process_noise))
00202   {
00203     process_noise_.resize(xml_process_noise.size());
00204     for (size_t i = 0; i < xml_process_noise.size(); i++)
00205     {
00206       std::stringstream ss;  // Convert the list element into doubles
00207       ss << xml_process_noise[i];
00208       ss >> process_noise_[i] ? process_noise_[i] : 0;
00209     }
00210   }
00211 
00212   // Read each topic config
00213   try
00214   {
00215     XmlRpc::XmlRpcValue topic_list;
00216     if (!pnh_.getParam("topics", topic_list))
00217     {
00218       ROS_FATAL("XmlRpc Error parsing parameters.  Make sure parameters were loaded into this node's namespace.");
00219       ros::shutdown();
00220     }
00221 
00222     ROS_INFO("Number of topics: %i", topic_list.size());
00223 
00224     // Iterate over the map of each joint and its parameters
00225     std::map<std::string, XmlRpc::XmlRpcValue>::iterator i;
00226     for (i = topic_list.begin(); i != topic_list.end(); i++)
00227     {
00228       // Get the name of this topic.  ex: base_odometry
00229       std::string topic_name = i->first;
00230 
00231       // Set up a nodehandle in this namespace (so we don't have to deal with the XmlRpc object)
00232       ros::NodeHandle tnh("~/topics/" + topic_name);
00233 
00234       ROS_INFO("Topic name: %s", topic_name.c_str());
00235 
00236       // Parse parameters according to topic type
00237       std::string type;
00238       if (!tnh.getParam("type", type))
00239       {
00240         ROS_ERROR("Could not get topic type for %s, skipping.", topic_name.c_str());
00241         continue;
00242       }
00243 
00244       ROS_INFO("Type: %s", type.c_str());
00245 
00246       if (type == "nav_msgs/Odometry")
00247       {
00248         std::string full_topic;
00249         if (!tnh.getParam("topic", full_topic))
00250         {
00251           ROS_ERROR("Could not get full topic for %s, skipping.", topic_name.c_str());
00252           continue;
00253         }
00254 
00255         // Create Odometry object
00256         boost::shared_ptr<GraftOdometryTopic> odom(new GraftOdometryTopic());
00257         odom->setName(topic_name);
00258         topics.push_back(odom);
00259 
00260         // Should we specify nodelay so that packets arrive without delay
00261         ros::TransportHints hints;
00262         bool tcp_nodelay;
00263         if (tnh.getParam("no_delay", tcp_nodelay))
00264         {
00265           if (tcp_nodelay)
00266             hints.tcpNoDelay();
00267         }
00268 
00269         // Subscribe to topic
00270         ros::Subscriber sub = n_.subscribe(full_topic, queue_size_, &GraftOdometryTopic::callback, odom, hints);
00271         subs.push_back(sub);
00272 
00273         // Parse rest of parameters
00274         parseNavMsgsOdometryParameters(tnh, odom);
00275       }
00276       else if (type == "sensor_msgs/Imu")
00277       {
00278         std::string full_topic;
00279         if (!tnh.getParam("topic", full_topic))
00280         {
00281           ROS_ERROR("Could not get full topic for %s, skipping.", topic_name.c_str());
00282           continue;
00283         }
00284 
00285         // Create Imu object
00286         boost::shared_ptr<GraftImuTopic> imu(new GraftImuTopic());
00287         imu->setName(topic_name);
00288         topics.push_back(imu);
00289 
00290         // Should we specify nodelay so that packets arrive without delay
00291         ros::TransportHints hints;
00292         bool tcp_nodelay;
00293         if (tnh.getParam("no_delay", tcp_nodelay))
00294         {
00295           if (tcp_nodelay)
00296             hints.tcpNoDelay();
00297         }
00298 
00299         // Subscribe to topic
00300         ros::Subscriber sub = n_.subscribe(full_topic, queue_size_, &GraftImuTopic::callback, imu, hints);
00301         subs.push_back(sub);
00302 
00303         // Parse rest of parameters
00304         parseSensorMsgsIMUParameters(tnh, imu);
00305       }
00306       else
00307       {
00308         ROS_WARN("Unknown type: %s  Not parsing configuration.", type.c_str());
00309       }
00310     }
00311   }
00312   catch(...)
00313   {
00314     ROS_FATAL("XmlRpc Error parsing parameters.  Cannot start.");
00315     ros::shutdown();
00316   }
00317 }
00318 
00319 
00320 std::string GraftParameterManager::getFilterType()
00321 {
00322   return filter_type_;
00323 }
00324 
00325 
00326 bool GraftParameterManager::getPlanarOutput()
00327 {
00328   return planar_output_;
00329 }
00330 
00331 
00332 std::string GraftParameterManager::getParentFrameID()
00333 {
00334   return parent_frame_id_;
00335 }
00336 
00337 
00338 std::string GraftParameterManager::getChildFrameID()
00339 {
00340   return child_frame_id_;
00341 }
00342 
00343 
00344 double GraftParameterManager::getUpdateRate()
00345 {
00346   return update_rate_;
00347 }
00348 
00349 
00350 std::string GraftParameterManager::getUpdateTopic()
00351 {
00352   return update_topic_;
00353 }
00354 
00355 
00356 double GraftParameterManager::getdtOveride()
00357 {
00358   return dt_override_;
00359 }
00360 
00361 
00362 int GraftParameterManager::getQueueSize()
00363 {
00364   return queue_size_;
00365 }
00366 
00367 
00368 bool GraftParameterManager::getIncludePose()
00369 {
00370   return include_pose_;
00371 }
00372 
00373 
00374 bool GraftParameterManager::getPublishTF()
00375 {
00376   return publish_tf_;
00377 }
00378 
00379 
00380 std::vector<double> GraftParameterManager::getInitialCovariance()
00381 {
00382   return initial_covariance_;
00383 }
00384 
00385 
00386 std::vector<double> GraftParameterManager::getProcessNoise()
00387 {
00388   return process_noise_;
00389 }
00390 
00391 
00392 double GraftParameterManager::getAlpha()
00393 {
00394   return alpha_;
00395 }
00396 
00397 
00398 double GraftParameterManager::getKappa()
00399 {
00400   return kappa_;
00401 }
00402 
00403 
00404 double GraftParameterManager::getBeta()
00405 {
00406   return beta_;
00407 }


graft
Author(s): Chad Rockey
autogenerated on Fri Aug 28 2015 10:53:16