Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "rawlog_record_node.h"
00035 #include "rawlog_record_node_defaults.h"
00036
00037 #include <mrpt/version.h>
00038
00039 RawlogRecordNode::ParametersNode::ParametersNode() : Parameters(), node("~")
00040 {
00041 node.param<double>("rate", rate, RAWLOG_RECORD_NODE_DEFAULT_RATE);
00042 ROS_INFO("rate: %f", rate);
00043 node.param<int>(
00044 "parameter_update_skip", parameter_update_skip,
00045 RAWLOG_RECORD_NODE_DEFAULT_PARAMETER_UPDATE_SKIP);
00046 ROS_INFO("parameter_update_skip: %i", parameter_update_skip);
00047 node.param<std::string>("tf_prefix", tf_prefix, "");
00048 ROS_INFO("tf_prefix: %s", tf_prefix.c_str());
00049 node.param<std::string>("odom_frame_id", odom_frame_id, "odom");
00050 ROS_INFO("odom_frame_id: %s", odom_frame_id.c_str());
00051 node.param<std::string>("base_frame_id", base_frame_id, "base_link");
00052 ROS_INFO("base_frame_id: %s", base_frame_id.c_str());
00053 node.getParam("raw_log_folder", raw_log_folder);
00054 ROS_INFO("raw_log_folder: %s", raw_log_folder.c_str());
00055 reconfigureFnc_ = boost::bind(
00056 &RawlogRecordNode::ParametersNode::callbackParameters, this, _1, _2);
00057 reconfigureServer_.setCallback(reconfigureFnc_);
00058
00059 node.param<bool>("record_range_scan", record_range_scan, true);
00060 ROS_INFO("record_range_scan: %s", (record_range_scan?"ture":"flase"));
00061 node.param<bool>("record_bearing_range", record_bearing_range, false);
00062 ROS_INFO("record_bearing_range: %s", (record_bearing_range?"ture":"flase"));
00063 node.param<bool>("record_beacon_range", record_beacon_range, false);
00064 ROS_INFO("record_beacon_range: %s", (record_beacon_range?"ture":"flase"));
00065
00066 }
00067
00068 void RawlogRecordNode::ParametersNode::update(const unsigned long& loop_count)
00069 {
00070 if (loop_count % parameter_update_skip) return;
00071 node.getParam("debug", debug);
00072 if (loop_count == 0) ROS_INFO("debug: %s", (debug ? "true" : "false"));
00073 }
00074
00075 void RawlogRecordNode::ParametersNode::callbackParameters(
00076 mrpt_rawlog::RawLogRecordConfig& config, uint32_t level)
00077 {
00078 using namespace mrpt::maps;
00079 using namespace mrpt::obs;
00080
00081 if (config.motion_noise_type == MOTION_MODEL_GAUSSIAN)
00082 {
00083 motionModelOptions.modelSelection = CActionRobotMovement2D::mmGaussian;
00084 motionModelOptions.gaussianModel.a1 = config.motion_gaussian_alpha_1;
00085 motionModelOptions.gaussianModel.a2 = config.motion_gaussian_alpha_2;
00086 motionModelOptions.gaussianModel.a3 = config.motion_gaussian_alpha_3;
00087 motionModelOptions.gaussianModel.a4 = config.motion_gaussian_alpha_4;
00088 motionModelOptions.gaussianModel.minStdXY = config.motion_gaussian_alpha_xy;
00089 motionModelOptions.gaussianModel.minStdPHI = config.motion_gaussian_alpha_phi;
00090 ROS_INFO("gaussianModel.a1: %f", motionModelOptions.gaussianModel.a1);
00091 ROS_INFO("gaussianModel.a2: %f", motionModelOptions.gaussianModel.a2);
00092 ROS_INFO("gaussianModel.a3: %f", motionModelOptions.gaussianModel.a3);
00093 ROS_INFO("gaussianModel.a4: %f", motionModelOptions.gaussianModel.a4);
00094 ROS_INFO(
00095 "gaussianModel.minStdXY: %f",
00096 motionModelOptions.gaussianModel.minStdXY);
00097 ROS_INFO(
00098 "gaussianModel.minStdPHI: %f",
00099 motionModelOptions.gaussianModel.minStdPHI);
00100
00101
00102 bearing_range_std_range = config.bearing_range_std_range;
00103 bearing_range_std_yaw = config.bearing_range_std_yaw;
00104 bearing_range_std_pitch = config.bearing_range_std_pitch;
00105 ROS_INFO("bearing_range_std_range: %f", bearing_range_std_range);
00106 ROS_INFO("bearing_range_std_yaw: %f", bearing_range_std_yaw);
00107 ROS_INFO("bearing_range_std_pitch: %f", bearing_range_std_pitch);
00108
00109 sensor_frame_sync_threshold = config.sensor_frame_sync_threshold;
00110 ROS_INFO("sensor_frame_sync_threshold: %f", sensor_frame_sync_threshold);
00111 }
00112 }