Ethzasl MSF 소스 읽기 (1): 프로그램 포털 및 주제 구독
Ethz의 Stephen Weiss 작업은 IMU 파인 결합 방법입니다.
1. 프로그램 입구:ethzaslmsf\msf_updates\src\pose_msf\main.cpp
1 #include "pose_sensormanager.h"
2
3 int main(int argc, char** argv)
4 {
5 ros::init(argc, argv, "msf_pose_sensor");
6 msf_pose_sensor::PoseSensorManager manager;
7 ros::spin();
8 return 0;
9 }
PoseSensorManager 클래스, 구조 함수를 봅니다.PoseSensorManager는 msf 에서 상속됨core::MSF_SensorManagerROS, msf 상속core::MSF_SensorManager
1 PoseSensorManager(ros::NodeHandle pnh = ros::NodeHandle("~/pose_sensor"))
2 {
3 bool distortmeas = false; //< Distort the pose measurements.
4 imu_handler_.reset(new msf_core::IMUHandler_ROS<:ekfstate>(*this, "msf_core", "imu_handler"));
5 pose_handler_.reset(new PoseSensorHandler_T(*this, "", "pose_sensor", distortmeas)); AddHandler(pose_handler_);
6
7 reconf_server_.reset(new ReconfigureServer(pnh));
8 ReconfigureServer::CallbackType f = boost::bind(&PoseSensorManager::Config, this, _1, _2);//
9 reconf_server_->setCallback(f);
10
11 init_scale_srv_ = pnh.advertiseService("initialize_msf_scale",
12 &PoseSensorManager::InitScale, this);
13 init_height_srv_ = pnh.advertiseService("initialize_msf_height",
14 &PoseSensorManager::InitHeight, this);
15 }
2. 그중의 imu 주의handler_및 posehandler_,각각 IMUHandlerROS 객체 및 PoseSensorHandler 템플릿 클래스 객체IMUHandler_ROS는 IMUHandler에서 상속됩니다.
typedef PoseSensorHandler<:pose_measurement::posemeasurement>,
PoseSensorManager> PoseSensorHandler_T;
(msf updates::pose measurement::PoseMeasurement<>는 모델 클래스 PoseSensorHandler의 클래스 매개 변수이며 이것은 관측량 매개 변수입니다.)
IMUHandler와 PoseSensorHandler는 최종적으로 msf 에서 계승됩니다.core::SensorHandler 클래스.
이 두 종류의 대상이 구성될 때 *this 포인터, 즉 PoseSensorManager 대상 자체로 전송됩니다.이것은 매우 중요합니다. 두 클래스 모두 PoseSensorManager의 구성원 함수를 호출했습니다.
이 두 가지 클래스에 들어가서 구조 함수를 볼 수 있는데 이 두 가지 구조 함수는 모두 일부 주제의 구독을 진행했다.
IMUHandler_ROS 구성, IMUHandler 보기ROS::IMUcallback 콜백 함수.
1 IMUHandler_ROS(MSF_SensorManager& mng,
2 const std::string& topic_namespace, const std::string& parameternamespace)
3 : IMUHandler(mng, topic_namespace, parameternamespace)
4 {
5 ros::NodeHandle nh(topic_namespace);
6 subImu_ = nh.subscribe("imu_state_input", 100, &IMUHandler_ROS::IMUCallback, this);
7 subState_ = nh.subscribe("hl_state_input", 10, &IMUHandler_ROS::StateCallback, this);
8 }
PoseSensorHandler 구조를 보고 PoseSensorHandler::Measurement Callback 리셋 함수를 보십시오.
1 template
2 PoseSensorHandler::PoseSensorHandler(
3 MANAGER_TYPE& meas, std::string topic_namespace,
4 std::string parameternamespace, bool distortmeas)
5 : SensorHandler<:ekfstate>(meas, topic_namespace, parameternamespace),
6 n_zp_(1e-6),
7 n_zq_(1e-6),
8 delay_(0),
9 timestamp_previous_pose_(0)
10 {
11 ros::NodeHandle pnh("~/" + parameternamespace);
12
13 MSF_INFO_STREAM(
14 "Loading parameters for pose sensor from namespace: "
15 << pnh.getNamespace());
16
17 pnh.param("pose_absolute_measurements", provides_absolute_measurements_,
18 true);
19 pnh.param("pose_measurement_world_sensor", measurement_world_sensor_, true);
20 pnh.param("pose_use_fixed_covariance", use_fixed_covariance_, false);
21 pnh.param("pose_measurement_minimum_dt", pose_measurement_minimum_dt_, 0.05);
22 pnh.param("enable_mah_outlier_rejection", enable_mah_outlier_rejection_, false);
23 pnh.param("mah_threshold", mah_threshold_, msf_core::kDefaultMahThreshold_);
24
25 MSF_INFO_STREAM_COND(measurement_world_sensor_, "Pose sensor is interpreting "
26 "measurement as sensor w.r.t. world");
27 MSF_INFO_STREAM_COND(
28 !measurement_world_sensor_,
29 "Pose sensor is interpreting measurement as world w.r.t. "
30 "sensor (e.g. ethzasl_ptam)");
31
32 MSF_INFO_STREAM_COND(use_fixed_covariance_, "Pose sensor is using fixed "
33 "covariance");
34 MSF_INFO_STREAM_COND(!use_fixed_covariance_,
35 "Pose sensor is using covariance "
36 "from sensor");
37
38 MSF_INFO_STREAM_COND(provides_absolute_measurements_,
39 "Pose sensor is handling "
40 "measurements as absolute values");
41 MSF_INFO_STREAM_COND(!provides_absolute_measurements_, "Pose sensor is "
42 "handling measurements as relative values");
43
44 ros::NodeHandle nh("msf_updates/" + topic_namespace);
45 subPoseWithCovarianceStamped_ =
46 nh.subscribe < geometry_msgs::PoseWithCovarianceStamped
47 > ("pose_with_covariance_input", 20, &PoseSensorHandler::MeasurementCallback, this);
48 subTransformStamped_ = nh.subscribe < geometry_msgs::TransformStamped
49 > ("transform_input", 20, &PoseSensorHandler::MeasurementCallback, this);
50 subPoseStamped_ = nh.subscribe < geometry_msgs::PoseStamped
51 > ("pose_input", 20, &PoseSensorHandler::MeasurementCallback, this);
52
53 z_p_.setZero();
54 z_q_.setIdentity();
55
56 if (distortmeas)
57 {
58 Eigen::Vector3d meanpos;
59 double distortpos_mean;
60 pnh.param("distortpos_mean", distortpos_mean, 0.0);
61 meanpos.setConstant(distortpos_mean);
62
63 Eigen::Vector3d stddevpos;
64 double distortpos_stddev;
65 pnh.param("distortpos_stddev", distortpos_stddev, 0.0);
66 stddevpos.setConstant(distortpos_stddev);
67
68 Eigen::Vector3d meanatt;
69 double distortatt_mean;
70 pnh.param("distortatt_mean", distortatt_mean, 0.0);
71 meanatt.setConstant(distortatt_mean);
72
73 Eigen::Vector3d stddevatt;
74 double distortatt_stddev;
75 pnh.param("distortatt_stddev", distortatt_stddev, 0.0);
76 stddevatt.setConstant(distortatt_stddev);
77
78 double distortscale_mean;
79 pnh.param("distortscale_mean", distortscale_mean, 0.0);
80 double distortscale_stddev;
81 pnh.param("distortscale_stddev", distortscale_stddev, 0.0);
82
83 distorter_.reset( new msf_updates::PoseDistorter(meanpos, stddevpos, meanatt, stddevatt, distortscale_mean, distortscale_stddev));
84 }
85 }
3.reconf_server_->setCallback(f)가 리셋을 바인딩했습니다. 리셋 함수PoseSensorManager::Config(xx).
1 virtual void Config(Config_T &config, uint32_t level)
2 {
3 config_ = config;
4 pose_handler_->SetNoises(config.pose_noise_meas_p,
5 config.pose_noise_meas_q);
6 pose_handler_->SetDelay(config.pose_delay);
7 if ((level & msf_updates::SinglePoseSensor_INIT_FILTER)
8 && config.core_init_filter == true)
9 {
10 Init(config.pose_initial_scale);
11 config.core_init_filter = false;
12 }
13 // Init call with "set height" checkbox.
14 if ((level & msf_updates::SinglePoseSensor_SET_HEIGHT)
15 && config.core_set_height == true)
16 {
17 Eigen::Matrix<double, 3, 1> p = pose_handler_->GetPositionMeasurement();
18 if (p.norm() == 0)
19 {
20 MSF_WARN_STREAM(
21 "No measurements received yet to initialize position. Height init "
22 "not allowed.");
23 return;
24 }
25 double scale = p[2] / config.core_height;
26 Init(scale);
27 config.core_set_height = false;
28 }
29 }
참조 자료:
다중 센서 칼만 융합 프레임워크 Ethzasl MSF Framework 컴파일 및 사용
이 내용에 흥미가 있습니까?
현재 기사가 여러분의 문제를 해결하지 못하는 경우 AI 엔진은 머신러닝 분석(스마트 모델이 방금 만들어져 부정확한 경우가 있을 수 있음)을 통해 가장 유사한 기사를 추천합니다:
다양한 언어의 JSONJSON은 Javascript 표기법을 사용하여 데이터 구조를 레이아웃하는 데이터 형식입니다. 그러나 Javascript가 코드에서 이러한 구조를 나타낼 수 있는 유일한 언어는 아닙니다. 저는 일반적으로 '객체'{}...
텍스트를 자유롭게 공유하거나 복사할 수 있습니다.하지만 이 문서의 URL은 참조 URL로 남겨 두십시오.
CC BY-SA 2.5, CC BY-SA 3.0 및 CC BY-SA 4.0에 따라 라이센스가 부여됩니다.