mrpt_rbpf_slam: options.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * File: options.cpp
00003  * Author: Vladislav Tananaev
00004  */
00005 
00006 #include <mrpt_rbpf_slam/options.h>
00007 #include <set>
00008 
00009 namespace mrpt_rbpf_slam
00010 {
00011 namespace
00012 {
00013 using namespace mrpt::obs;
00014 
00015 bool loadThrunModelParameters(const ros::NodeHandle& nh,
00016                               CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel& thrunModel)
00017 {
00018   bool success = true;
00019   ros::NodeHandle sub_nh(nh, "thrun_motion_model_options");
00020   auto getParam = [&success, &sub_nh](const std::string param, auto& val) {
00021     success = success && sub_nh.getParam(param, val);
00022   };
00023 
00024   thrunModel.nParticlesCount = sub_nh.param<int>("particle_count", 100);
00025   getParam("alfa1_rot_rot", thrunModel.alfa1_rot_rot);
00026   getParam("alfa2_rot_trans", thrunModel.alfa2_rot_trans);
00027   getParam("alfa3_trans_trans", thrunModel.alfa3_trans_trans);
00028   getParam("alfa4_trans_rot", thrunModel.alfa4_trans_rot);
00029   getParam("additional_std_XY", thrunModel.additional_std_XY);
00030   getParam("additional_std_phi", thrunModel.additional_std_phi);
00031   return success;
00032 }
00033 
00034 bool loadGaussianModelParameters(const ros::NodeHandle& nh,
00035                                  CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel& gaussian_model)
00036 {
00037   bool success = true;
00038   ros::NodeHandle sub_nh(nh, "gaussian_motion_model_options");
00039   auto getParam = [&success, &sub_nh](const std::string param, auto& val) {
00040     success = success && sub_nh.getParam(param, val);
00041   };
00042 
00043   getParam("a1", gaussian_model.a1);
00044   getParam("a2", gaussian_model.a2);
00045   getParam("a3", gaussian_model.a3);
00046   getParam("a4", gaussian_model.a4);
00047   getParam("minStdXY", gaussian_model.minStdXY);
00048   getParam("minStdPHI", gaussian_model.minStdPHI);
00049   return success;
00050 }
00051 
00052 bool loadMotionModelParameters(const ros::NodeHandle& nh,
00053                                CActionRobotMovement2D::TMotionModelOptions& motion_model_options)
00054 {
00055   static const std::map<std::string, CActionRobotMovement2D::TDrawSampleMotionModel> motion_models = {
00056     { "thrun", CActionRobotMovement2D::mmThrun }, { "gaussian", CActionRobotMovement2D::mmGaussian }
00057   };
00058 
00059   bool success = true;
00060   ros::NodeHandle sub_nh(nh, "motion_model");
00061   auto getParam = [&success, &sub_nh](const std::string param, auto& val) {
00062     success = success && sub_nh.getParam(param, val);
00063   };
00064 
00065   std::string model_type;
00066   getParam("type", model_type);
00067   if (!motion_models.count(model_type))
00068   {
00069     ROS_ERROR_STREAM("Specified motion model " << model_type << " is not supported.");
00070     return false;
00071   }
00072   motion_model_options.modelSelection = motion_models.at(model_type);
00073   success = success && loadThrunModelParameters(sub_nh, motion_model_options.thrunModel);
00074   success = success && loadGaussianModelParameters(sub_nh, motion_model_options.gaussianModel);
00075   return success;
00076 }
00077 
00078 bool loadVisualizationOptions(const ros::NodeHandle& nh, PFslam::Options& options)
00079 {
00080   bool success = true;
00081   ros::NodeHandle sub_nh(nh, "mrpt_visualization_options");
00082   success = success && sub_nh.getParam("width", options.PROGRESS_WINDOW_WIDTH_);
00083   success = success && sub_nh.getParam("height", options.PROGRESS_WINDOW_HEIGHT_);
00084   success = success && sub_nh.getParam("window_update_delay", options.SHOW_PROGRESS_IN_WINDOW_DELAY_MS_);
00085   success = success && sub_nh.getParam("show_window", options.SHOW_PROGRESS_IN_WINDOW_);
00086   success = success && sub_nh.getParam("camera_follow_robot", options.CAMERA_3DSCENE_FOLLOWS_ROBOT_);
00087   return success;
00088 }
00089 }  // namespace
00090 
00091 bool loadOptions(const ros::NodeHandle& nh, PFslam::Options& options)
00092 {
00093   bool success = true;
00094   success = success && loadMotionModelParameters(nh, options.motion_model_options_);
00095   success = success && loadVisualizationOptions(nh, options);
00096   success = success && nh.getParam("simplemap_save_folder", options.simplemap_path_prefix);
00097   return success;
00098 }
00099 
00100 }  // namespace mrpt_rbpf_slam