diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index e4273687e7..d1861ca181 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -54,6 +54,7 @@ PositionEstimator::PositionEstimator() : _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), _startup_time(1) { + _n.getParam("vehicle_model", _model_name); } void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) @@ -68,7 +69,7 @@ void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when //gazebo rearranges indexes. for (std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { - if (*it == "iris" || *it == "ardrone") { + if (*it == _model_name) { index = it - msg->name.begin(); break; } diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h index da1fc2c5a8..ee46cdf77d 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.h +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -57,6 +57,6 @@ protected: ros::Publisher _vehicle_position_pub; uint64_t _startup_time; - + std::string _model_name; };