options.cpp
Go to the documentation of this file.
1 /*
2  * File: options.cpp
3  * Author: Vladislav Tananaev
4  */
5 
7 #include <set>
8 
9 namespace mrpt_rbpf_slam
10 {
11 namespace
12 {
13 using namespace mrpt::obs;
14 
15 bool loadThrunModelParameters(const ros::NodeHandle& nh,
16  CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel& thrunModel)
17 {
18  bool success = true;
19  ros::NodeHandle sub_nh(nh, "thrun_motion_model_options");
20  auto getParam = [&success, &sub_nh](const std::string param, auto& val) {
21  success = success && sub_nh.getParam(param, val);
22  };
23 
24  thrunModel.nParticlesCount = sub_nh.param<int>("particle_count", 100);
25  getParam("alfa1_rot_rot", thrunModel.alfa1_rot_rot);
26  getParam("alfa2_rot_trans", thrunModel.alfa2_rot_trans);
27  getParam("alfa3_trans_trans", thrunModel.alfa3_trans_trans);
28  getParam("alfa4_trans_rot", thrunModel.alfa4_trans_rot);
29  getParam("additional_std_XY", thrunModel.additional_std_XY);
30  getParam("additional_std_phi", thrunModel.additional_std_phi);
31  return success;
32 }
33 
34 bool loadGaussianModelParameters(const ros::NodeHandle& nh,
35  CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel& gaussian_model)
36 {
37  bool success = true;
38  ros::NodeHandle sub_nh(nh, "gaussian_motion_model_options");
39  auto getParam = [&success, &sub_nh](const std::string param, auto& val) {
40  success = success && sub_nh.getParam(param, val);
41  };
42 
43  getParam("a1", gaussian_model.a1);
44  getParam("a2", gaussian_model.a2);
45  getParam("a3", gaussian_model.a3);
46  getParam("a4", gaussian_model.a4);
47  getParam("minStdXY", gaussian_model.minStdXY);
48  getParam("minStdPHI", gaussian_model.minStdPHI);
49  return success;
50 }
51 
52 bool loadMotionModelParameters(const ros::NodeHandle& nh,
53  CActionRobotMovement2D::TMotionModelOptions& motion_model_options)
54 {
55  static const std::map<std::string, CActionRobotMovement2D::TDrawSampleMotionModel> motion_models = {
56  { "thrun", CActionRobotMovement2D::mmThrun }, { "gaussian", CActionRobotMovement2D::mmGaussian }
57  };
58 
59  bool success = true;
60  ros::NodeHandle sub_nh(nh, "motion_model");
61  auto getParam = [&success, &sub_nh](const std::string param, auto& val) {
62  success = success && sub_nh.getParam(param, val);
63  };
64 
65  std::string model_type;
66  getParam("type", model_type);
67  if (!motion_models.count(model_type))
68  {
69  ROS_ERROR_STREAM("Specified motion model " << model_type << " is not supported.");
70  return false;
71  }
72  motion_model_options.modelSelection = motion_models.at(model_type);
73  success = success && loadThrunModelParameters(sub_nh, motion_model_options.thrunModel);
74  success = success && loadGaussianModelParameters(sub_nh, motion_model_options.gaussianModel);
75  return success;
76 }
77 
78 bool loadVisualizationOptions(const ros::NodeHandle& nh, PFslam::Options& options)
79 {
80  bool success = true;
81  ros::NodeHandle sub_nh(nh, "mrpt_visualization_options");
82  success = success && sub_nh.getParam("width", options.PROGRESS_WINDOW_WIDTH_);
83  success = success && sub_nh.getParam("height", options.PROGRESS_WINDOW_HEIGHT_);
84  success = success && sub_nh.getParam("window_update_delay", options.SHOW_PROGRESS_IN_WINDOW_DELAY_MS_);
85  success = success && sub_nh.getParam("show_window", options.SHOW_PROGRESS_IN_WINDOW_);
86  success = success && sub_nh.getParam("camera_follow_robot", options.CAMERA_3DSCENE_FOLLOWS_ROBOT_);
87  return success;
88 }
89 } // namespace
90 
91 bool loadOptions(const ros::NodeHandle& nh, PFslam::Options& options)
92 {
93  bool success = true;
94  success = success && loadMotionModelParameters(nh, options.motion_model_options_);
95  success = success && loadVisualizationOptions(nh, options);
96  success = success && nh.getParam("simplemap_save_folder", options.simplemap_path_prefix);
97  return success;
98 }
99 
100 } // namespace mrpt_rbpf_slam
bool param(const std::string &param_name, T &param_val, const T &default_val)
bool loadOptions(const ros::NodeHandle &nh, PFslam::Options &options)
Definition: options.cpp:91
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_options_


mrpt_rbpf_slam
Author(s): Vladislav Tananaev
autogenerated on Thu Jun 6 2019 19:37:56