mrpt_localization_node_parameters.cpp
Go to the documentation of this file.
00001 /***********************************************************************************
00002  * Revised BSD License                                                             *
00003  * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at>                    *
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 are met:     *
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 Vienna University of Technology nor the           *
00014  *       names of its contributors may be used to endorse or promote products      *
00015  *       derived from this software without 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 Markus Bader 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 "mrpt_localization_node.h"
00030 #include "mrpt_localization_node_defaults.h"
00031 
00032 PFLocalizationNode::Parameters::Parameters(PFLocalizationNode *p)
00033     : PFLocalization::Parameters(p), node("~") {
00034     node.param<double>("rate", rate, MRPT_LOCALIZATION_NODE_DEFAULT_RATE);
00035     ROS_INFO("rate: %f", rate);
00036     node.getParam("gui_mrpt", gui_mrpt);
00037     ROS_INFO("gui_mrpt:  %s", (gui_mrpt ? "true" : "false"));
00038     node.param<int>("parameter_update_skip", parameter_update_skip, MRPT_LOCALIZATION_NODE_DEFAULT_PARAMETER_UPDATE_SKIP);
00039     ROS_INFO("parameter_update_skip: %i", parameter_update_skip);
00040     node.getParam("ini_file", iniFile);
00041     ROS_INFO("ini_file: %s", iniFile.c_str());
00042     node.getParam("map_file", mapFile);
00043     ROS_INFO("map_file: %s", mapFile.c_str());
00044     node.param<std::string>("tf_prefix", tf_prefix, "");
00045     ROS_INFO("tf_prefix: %s", tf_prefix.c_str());
00046     node.param<std::string>("global_frame_id", global_frame_id, "map");
00047     ROS_INFO("global_frame_id: %s", global_frame_id.c_str());
00048     node.param<std::string>("odom_frame_id", odom_frame_id, "odom");
00049     ROS_INFO("odom_frame_id: %s", odom_frame_id.c_str());
00050     node.param<std::string>("base_frame_id", base_frame_id, "base_link");
00051     ROS_INFO("base_frame_id: %s", base_frame_id.c_str());
00052 
00053     reconfigureFnc_ = boost::bind(&PFLocalizationNode::Parameters::callbackParameters, this ,  _1, _2);
00054     reconfigureServer_.setCallback(reconfigureFnc_);
00055 }
00056 
00057 void PFLocalizationNode::Parameters::update(const unsigned long &loop_count) {
00058     if(loop_count % parameter_update_skip) return;
00059     node.getParam("debug", debug);
00060     if(loop_count == 0) ROS_INFO("debug:  %s", (debug ? "true" : "false"));
00061     {
00062         int v = particlecloud_update_skip;
00063         node.param<int>("particlecloud_update_skip", particlecloud_update_skip, MRPT_LOCALIZATION_NODE_DEFAULT_PARTICLECLOUD_UPDATE_SKIP);
00064         if(v != particlecloud_update_skip) ROS_INFO("particlecloud_update_skip: %i", particlecloud_update_skip);
00065     }
00066     {
00067         int v = map_update_skip;
00068         node.param<int>("map_update_skip", map_update_skip, MRPT_LOCALIZATION_NODE_DEFAULT_MAP_UPDATE_SKIP);
00069         if(v != map_update_skip) ROS_INFO("map_update_skip: %i", map_update_skip);
00070     }
00071 }
00072 
00073 void PFLocalizationNode::Parameters::callbackParameters (mrpt_localization::MotionConfig &config, uint32_t level ) {
00074     if(config.motion_noise_type == MOTION_MODEL_GAUSSIAN) {
00075         motion_model_options->modelSelection = mrpt::slam::CActionRobotMovement2D::mmGaussian;
00076         motion_model_options->gausianModel.a1 = config.gaussian_alpha_1;
00077         motion_model_options->gausianModel.a2 = config.gaussian_alpha_2;
00078         motion_model_options->gausianModel.a3 = config.gaussian_alpha_3;
00079         motion_model_options->gausianModel.a4 = config.gaussian_alpha_4;
00080         motion_model_options->gausianModel.minStdXY  = config.gaussian_alpha_xy;
00081         motion_model_options->gausianModel.minStdPHI = config.gaussian_alpha_phi;
00082         ROS_INFO("gausianModel.type: gaussian");
00083         ROS_INFO("gausianModel.a1: %f", motion_model_options->gausianModel.a1);
00084         ROS_INFO("gausianModel.a2: %f", motion_model_options->gausianModel.a2);
00085         ROS_INFO("gausianModel.a3: %f", motion_model_options->gausianModel.a3);
00086         ROS_INFO("gausianModel.a4: %f", motion_model_options->gausianModel.a4);
00087         ROS_INFO("gausianModel.minStdXY: %f", motion_model_options->gausianModel.minStdXY);
00088         ROS_INFO("gausianModel.minStdPHI: %f", motion_model_options->gausianModel.minStdPHI);
00089     } else {
00090         ROS_INFO("We support at the moment only gaussian motion models");
00091     }
00092     *use_motion_model_default_options = config.use_default_motion;
00093     ROS_INFO("use_motion_model_default_options:  %s", (use_motion_model_default_options ? "true" : "false"));
00094     motion_model_default_options->gausianModel.minStdXY = config.default_noise_xy;
00095     ROS_INFO("default_noise_xy: %f", motion_model_default_options->gausianModel.minStdXY);
00096     motion_model_default_options->gausianModel.minStdPHI = config.default_noise_phi;
00097     ROS_INFO("default_noise_phi: %f", motion_model_default_options->gausianModel.minStdPHI);
00098 }


mrpt_localization
Author(s): Markus Bader
autogenerated on Mon Aug 11 2014 11:23:29