50 staticDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::
OK),
51 tfListener_(tfBuffer_),
52 dynamicDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::
OK),
58 latestControlTime_(0),
59 tfTimeout_(
ros::Duration(0)),
62 predictToCurrentTime_(false),
63 printDiagnostics_(true),
64 gravitationalAcc_(9.80665),
65 publishTransform_(true),
66 publishAcceleration_(false),
69 smoothLaggedData_(false),
70 disabledAtStartup_(false),
166 robot_localization::ToggleFilterProcessing::Response& resp)
170 ROS_WARN_STREAM(
"Service was called to toggle filter processing but state was already as requested.");
175 ROS_INFO(
"Toggling filter measurement filtering to %s.", req.on ?
"On" :
"Off");
185 const std::string &targetFrame)
194 const std::string &topicName = callbackData.
topicName_;
196 RF_DEBUG(
"------ RosFilter::accelerationCallback (" << topicName <<
") ------\n" 197 "Twist message:\n" << *msg);
201 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
207 RF_DEBUG(
"Update vector for " << topicName <<
" is:\n" << topicName);
212 measurement.setZero();
213 measurementCovariance.setZero();
216 std::vector<int> updateVectorCorrected = callbackData.
updateVector_;
220 measurementCovariance))
226 measurementCovariance,
227 updateVectorCorrected,
231 RF_DEBUG(
"Enqueued new measurement for " << topicName <<
"_acceleration\n");
235 RF_DEBUG(
"Did *not* enqueue measurement for " << topicName <<
"_acceleration\n");
240 RF_DEBUG(
"Last message time for " << topicName <<
" is now " <<
249 std::stringstream stream;
250 stream <<
"The " << topicName <<
" message has a timestamp before that of the previous message received," <<
251 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
252 msg->header.stamp.toSec() <<
")";
254 topicName +
"_timestamp",
258 RF_DEBUG(
"Message is too old. Last message time for " << topicName <<
260 msg->header.stamp <<
".\n");
263 RF_DEBUG(
"\n----- /RosFilter::accelerationCallback (" << topicName <<
") ------\n");
269 geometry_msgs::TwistStampedPtr twistStampedPtr = geometry_msgs::TwistStampedPtr(
new geometry_msgs::TwistStamped());
270 twistStampedPtr->twist = *msg;
301 const Eigen::VectorXd &measurement,
302 const Eigen::MatrixXd &measurementCovariance,
303 const std::vector<int> &updateVector,
304 const double mahalanobisThresh,
309 meas->topicName_ = topicName;
310 meas->measurement_ = measurement;
311 meas->covariance_ = measurementCovariance;
312 meas->updateVector_ = updateVector;
313 meas->time_ = time.
toSec();
314 meas->mahalanobisThresh_ = mahalanobisThresh;
322 Eigen::MatrixXd &measurementCovariance,
323 std::vector<int> &updateVector)
354 if (
filter_.getInitializedStatus())
357 const Eigen::VectorXd &state =
filter_.getState();
358 const Eigen::MatrixXd &estimateErrorCovariance =
filter_.getEstimateErrorCovariance();
369 message.pose.pose.orientation.x = quat.x();
370 message.pose.pose.orientation.y = quat.y();
371 message.pose.pose.orientation.z = quat.z();
372 message.pose.pose.orientation.w = quat.w();
385 message.pose.covariance[POSE_SIZE * i + j] = estimateErrorCovariance(i, j);
396 message.twist.covariance[TWIST_SIZE * i + j] =
406 return filter_.getInitializedStatus();
413 if (
filter_.getInitializedStatus())
416 const Eigen::VectorXd &state =
filter_.getState();
417 const Eigen::MatrixXd &estimateErrorCovariance =
filter_.getEstimateErrorCovariance();
431 message.accel.covariance[
POSE_SIZE * i + j] =
441 return filter_.getInitializedStatus();
446 const std::string &topicName,
451 RF_DEBUG(
"------ RosFilter::imuCallback (" << topicName <<
") ------\n" <<
"IMU message:\n" << *msg);
457 std::stringstream stream;
458 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
459 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
460 msg->header.stamp.toSec() <<
")";
462 topicName +
"_timestamp",
465 RF_DEBUG(
"Received message that preceded the most recent pose reset. Ignoring...");
479 if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9)
481 RF_DEBUG(
"Received IMU message with -1 as its first covariance value for orientation. " 482 "Ignoring orientation...");
487 geometry_msgs::PoseWithCovarianceStamped *posPtr =
new geometry_msgs::PoseWithCovarianceStamped();
488 posPtr->header = msg->header;
489 posPtr->pose.pose.orientation = msg->orientation;
497 msg->orientation_covariance[ORIENTATION_SIZE * i + j];
505 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
513 if (::fabs(msg->angular_velocity_covariance[0] + 1) < 1e-9)
515 RF_DEBUG(
"Received IMU message with -1 as its first covariance value for angular " 516 "velocity. Ignoring angular velocity...");
521 geometry_msgs::TwistWithCovarianceStamped *twistPtr =
new geometry_msgs::TwistWithCovarianceStamped();
522 twistPtr->header = msg->header;
523 twistPtr->twist.twist.angular = msg->angular_velocity;
531 msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j];
535 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
543 if (::fabs(msg->linear_acceleration_covariance[0] + 1) < 1e-9)
545 RF_DEBUG(
"Received IMU message with -1 as its first covariance value for linear " 546 "acceleration. Ignoring linear acceleration...");
555 RF_DEBUG(
"\n----- /RosFilter::imuCallback (" << topicName <<
") ------\n");
561 const double currentTimeSec = currentTime.
toSec();
563 RF_DEBUG(
"------ RosFilter::integrateMeasurements ------\n\n" 564 "Integration time is " << std::setprecision(20) << currentTimeSec <<
"\n" 577 int restoredMeasurementCount = 0;
580 RF_DEBUG(
"Received a measurement that was " <<
filter_.getLastMeasurementTime() - firstMeasurement->time_ <<
581 " seconds in the past. Reverting filter state and measurement queue...");
584 const double firstMeasurementTime = firstMeasurement->time_;
585 const std::string firstMeasurementTopic = firstMeasurement->topicName_;
586 if (!
revertTo(firstMeasurementTime - 1e-9))
588 RF_DEBUG(
"ERROR: history interval is too small to revert to time " << firstMeasurementTime <<
"\n");
590 firstMeasurementTopic <<
", but history interval is insufficiently sized. Measurement time is " <<
591 std::setprecision(20) << firstMeasurementTime <<
", current time is " << currentTime.
toSec() <<
593 restoredMeasurementCount = 0;
596 restoredMeasurementCount =
static_cast<int>(
measurementQueue_.size()) - originalCount;
605 if (measurement->time_ > currentTime.
toSec())
620 filter_.setControl(measurement->latestControl_, measurement->latestControlTime_);
621 restoredMeasurementCount--;
625 filter_.processMeasurement(*(measurement.get()));
642 else if (
filter_.getInitializedStatus())
647 double lastUpdateDelta = currentTimeSec -
filter_.getLastMeasurementTime();
650 if (lastUpdateDelta >=
filter_.getSensorTimeout())
652 predictToCurrentTime =
true;
654 RF_DEBUG(
"Sensor timeout! Last measurement time was " <<
filter_.getLastMeasurementTime() <<
655 ", current time is " << currentTimeSec <<
656 ", delta is " << lastUpdateDelta <<
"\n");
661 RF_DEBUG(
"Filter not yet initialized.\n");
664 if (
filter_.getInitializedStatus() && predictToCurrentTime)
666 double lastUpdateDelta = currentTimeSec -
filter_.getLastMeasurementTime();
668 filter_.validateDelta(lastUpdateDelta);
669 filter_.predict(currentTimeSec, lastUpdateDelta);
672 filter_.setLastMeasurementTime(
filter_.getLastMeasurementTime() + lastUpdateDelta);
675 RF_DEBUG(
"\n----- /RosFilter::integrateMeasurements ------\n");
685 std::map<StateMembers, int> absPoseVarCounts;
694 std::map<StateMembers, int> twistVarCounts;
714 std::string debugOutFile;
718 nhLocal_.
param(
"debug_out_file", debugOutFile, std::string(
"robot_localization_debug.txt"));
728 ROS_WARN_STREAM(
"RosFilter::loadParams() - unable to create debug output file " << debugOutFile);
731 catch(
const std::exception &e)
733 ROS_WARN_STREAM(
"RosFilter::loadParams() - unable to create debug output file" << debugOutFile
734 <<
". Error was " << e.what() <<
"\n");
777 "Invalid frame configuration! The values for map_frame, odom_frame, " 778 "and base_link_frame must be unique. If using a base_link_frame_output values, it " 779 "must not match the map_frame or odom_frame.");
782 std::string tfPrefix =
"";
783 std::string tfPrefixPath =
"";
812 double sensorTimeout;
815 filter_.setSensorTimeout(sensorTimeout);
830 " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed.");
836 " specified. Absolute value will be assumed.");
844 bool stampedControl =
false;
845 double controlTimeout = sensorTimeout;
846 std::vector<int> controlUpdateVector(
TWIST_SIZE, 0);
847 std::vector<double> accelerationLimits(
TWIST_SIZE, 1.0);
848 std::vector<double> accelerationGains(
TWIST_SIZE, 1.0);
849 std::vector<double> decelerationLimits(
TWIST_SIZE, 1.0);
850 std::vector<double> decelerationGains(
TWIST_SIZE, 1.0);
854 nhLocal_.
param(
"control_timeout", controlTimeout, sensorTimeout);
863 "size " << controlUpdateVector.size() <<
". No control term will be used.");
869 ROS_ERROR_STREAM(
"use_control is set to true, but control_config is missing. No control term will be used.");
878 "size " << accelerationLimits.size() <<
". No control term will be used.");
884 ROS_WARN_STREAM(
"use_control is set to true, but acceleration_limits is missing. Will use default values.");
889 const int size = accelerationGains.size();
893 ". Provided config was of size " << size <<
". All gains will be assumed to be 1.");
894 std::fill_n(accelerationGains.begin(), std::min(size,
TWIST_SIZE), 1.0);
904 ". Provided config was of size " << decelerationLimits.size() <<
". No control term will be used.");
910 ROS_INFO_STREAM(
"use_control is set to true, but no deceleration_limits specified. Will use acceleration " 912 decelerationLimits = accelerationLimits;
917 const int size = decelerationGains.size();
921 ". Provided config was of size " << size <<
". All gains will be assumed to be 1.");
922 std::fill_n(decelerationGains.begin(), std::min(size,
TWIST_SIZE), 1.0);
928 ROS_INFO_STREAM(
"use_control is set to true, but no deceleration_gains specified. Will use acceleration " 930 decelerationGains = accelerationGains;
934 bool dynamicProcessNoiseCovariance =
false;
935 nhLocal_.
param(
"dynamic_process_noise_covariance", dynamicProcessNoiseCovariance,
false);
936 filter_.setUseDynamicProcessNoiseCovariance(dynamicProcessNoiseCovariance);
938 std::vector<double> initialState(
STATE_SIZE, 0.0);
944 initialState.size() <<
". The initial state will be ignored.");
948 Eigen::Map<Eigen::VectorXd> eigenState(initialState.data(), initialState.size());
959 RF_DEBUG(
"tf_prefix is " << tfPrefix <<
968 "\nsensor_timeout is " <<
filter_.getSensorTimeout() <<
969 "\ntwo_d_mode is " << (
twoDMode_ ?
"true" :
"false") <<
972 "\nuse_control is " << (
useControl_ ?
"true" :
"false") <<
973 "\nstamped_control is " << (stampedControl ?
"true" :
"false") <<
974 "\ncontrol_config is " << controlUpdateVector <<
975 "\ncontrol_timeout is " << controlTimeout <<
976 "\nacceleration_limits are " << accelerationLimits <<
977 "\nacceleration_gains are " << accelerationGains <<
978 "\ndeceleration_limits are " << decelerationLimits <<
979 "\ndeceleration_gains are " << decelerationGains <<
980 "\ninitial state is " <<
filter_.getState() <<
981 "\ndynamic_process_noise_covariance is " << (dynamicProcessNoiseCovariance ?
"true" :
"false") <<
1005 size_t topicInd = 0;
1006 bool moreParams =
false;
1012 std::stringstream ss;
1013 ss <<
"odom" << topicInd++;
1014 std::string odomTopicName = ss.str();
1021 nhLocal_.
param(odomTopicName + std::string(
"_differential"), differential,
false);
1025 nhLocal_.
param(odomTopicName + std::string(
"_relative"), relative,
false);
1027 if (relative && differential)
1029 ROS_WARN_STREAM(
"Both " << odomTopicName <<
"_differential" <<
" and " << odomTopicName <<
1030 "_relative were set to true. Using differential mode.");
1035 std::string odomTopic;
1039 double poseMahalanobisThresh;
1040 nhLocal_.
param(odomTopicName + std::string(
"_pose_rejection_threshold"),
1041 poseMahalanobisThresh,
1042 std::numeric_limits<double>::max());
1045 double twistMahalanobisThresh;
1046 nhLocal_.
param(odomTopicName + std::string(
"_twist_rejection_threshold"),
1047 twistMahalanobisThresh,
1048 std::numeric_limits<double>::max());
1054 std::vector<int> poseUpdateVec = updateVec;
1056 std::vector<int> twistUpdateVec = updateVec;
1059 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
1060 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
1061 int odomQueueSize = 1;
1062 nhLocal_.
param(odomTopicName +
"_queue_size", odomQueueSize, 1);
1064 const CallbackData poseCallbackData(odomTopicName +
"_pose", poseUpdateVec, poseUpdateSum, differential,
1065 relative, poseMahalanobisThresh);
1066 const CallbackData twistCallbackData(odomTopicName +
"_twist", twistUpdateVec, twistUpdateSum,
false,
false,
1067 twistMahalanobisThresh);
1069 bool nodelayOdom =
false;
1070 nhLocal_.
param(odomTopicName +
"_nodelay", nodelayOdom,
false);
1073 if (poseUpdateSum + twistUpdateSum > 0)
1076 nh_.
subscribe<nav_msgs::Odometry>(odomTopic, odomQueueSize,
1082 std::stringstream stream;
1083 stream << odomTopic <<
" is listed as an input topic, but all update variables are false";
1086 odomTopic +
"_configuration",
1091 if (poseUpdateSum > 0)
1113 if (twistUpdateSum > 0)
1123 RF_DEBUG(
"Subscribed to " << odomTopic <<
" (" << odomTopicName <<
")\n\t" <<
1124 odomTopicName <<
"_differential is " << (differential ?
"true" :
"false") <<
"\n\t" <<
1125 odomTopicName <<
"_pose_rejection_threshold is " << poseMahalanobisThresh <<
"\n\t" <<
1126 odomTopicName <<
"_twist_rejection_threshold is " << twistMahalanobisThresh <<
"\n\t" <<
1127 odomTopicName <<
"_queue_size is " << odomQueueSize <<
"\n\t" <<
1128 odomTopicName <<
" pose update vector is " << poseUpdateVec <<
"\t"<<
1129 odomTopicName <<
" twist update vector is " << twistUpdateVec);
1139 std::stringstream ss;
1140 ss <<
"pose" << topicInd++;
1141 std::string poseTopicName = ss.str();
1147 nhLocal_.
param(poseTopicName + std::string(
"_differential"), differential,
false);
1151 nhLocal_.
param(poseTopicName + std::string(
"_relative"), relative,
false);
1153 if (relative && differential)
1155 ROS_WARN_STREAM(
"Both " << poseTopicName <<
"_differential" <<
" and " << poseTopicName <<
1156 "_relative were set to true. Using differential mode.");
1161 std::string poseTopic;
1165 double poseMahalanobisThresh;
1166 nhLocal_.
param(poseTopicName + std::string(
"_rejection_threshold"),
1167 poseMahalanobisThresh,
1168 std::numeric_limits<double>::max());
1170 int poseQueueSize = 1;
1171 nhLocal_.
param(poseTopicName +
"_queue_size", poseQueueSize, 1);
1173 bool nodelayPose =
false;
1174 nhLocal_.
param(poseTopicName +
"_nodelay", nodelayPose,
false);
1185 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
1187 if (poseUpdateSum > 0)
1189 const CallbackData callbackData(poseTopicName, poseUpdateVec, poseUpdateSum, differential, relative,
1190 poseMahalanobisThresh);
1193 nh_.
subscribe<geometry_msgs::PoseWithCovarianceStamped>(poseTopic, poseQueueSize,
1218 ROS_WARN_STREAM(
"Warning: " << poseTopic <<
" is listed as an input topic, " 1219 "but all pose update variables are false");
1222 RF_DEBUG(
"Subscribed to " << poseTopic <<
" (" << poseTopicName <<
")\n\t" <<
1223 poseTopicName <<
"_differential is " << (differential ?
"true" :
"false") <<
"\n\t" <<
1224 poseTopicName <<
"_rejection_threshold is " << poseMahalanobisThresh <<
"\n\t" <<
1225 poseTopicName <<
"_queue_size is " << poseQueueSize <<
"\n\t" <<
1226 poseTopicName <<
" update vector is " << poseUpdateVec);
1236 std::stringstream ss;
1237 ss <<
"twist" << topicInd++;
1238 std::string twistTopicName = ss.str();
1243 std::string twistTopic;
1247 double twistMahalanobisThresh;
1248 nhLocal_.
param(twistTopicName + std::string(
"_rejection_threshold"),
1249 twistMahalanobisThresh,
1250 std::numeric_limits<double>::max());
1252 int twistQueueSize = 1;
1253 nhLocal_.
param(twistTopicName +
"_queue_size", twistQueueSize, 1);
1255 bool nodelayTwist =
false;
1256 nhLocal_.
param(twistTopicName +
"_nodelay", nodelayTwist,
false);
1262 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
1264 if (twistUpdateSum > 0)
1266 const CallbackData callbackData(twistTopicName, twistUpdateVec, twistUpdateSum,
false,
false,
1267 twistMahalanobisThresh);
1270 nh_.
subscribe<geometry_msgs::TwistWithCovarianceStamped>(twistTopic, twistQueueSize,
1283 ROS_WARN_STREAM(
"Warning: " << twistTopic <<
" is listed as an input topic, " 1284 "but all twist update variables are false");
1287 RF_DEBUG(
"Subscribed to " << twistTopic <<
" (" << twistTopicName <<
")\n\t" <<
1288 twistTopicName <<
"_rejection_threshold is " << twistMahalanobisThresh <<
"\n\t" <<
1289 twistTopicName <<
"_queue_size is " << twistQueueSize <<
"\n\t" <<
1290 twistTopicName <<
" update vector is " << twistUpdateVec);
1300 std::stringstream ss;
1301 ss <<
"imu" << topicInd++;
1302 std::string imuTopicName = ss.str();
1308 nhLocal_.
param(imuTopicName + std::string(
"_differential"), differential,
false);
1312 nhLocal_.
param(imuTopicName + std::string(
"_relative"), relative,
false);
1314 if (relative && differential)
1316 ROS_WARN_STREAM(
"Both " << imuTopicName <<
"_differential" <<
" and " << imuTopicName <<
1317 "_relative were set to true. Using differential mode.");
1322 std::string imuTopic;
1326 double poseMahalanobisThresh;
1327 nhLocal_.
param(imuTopicName + std::string(
"_pose_rejection_threshold"),
1328 poseMahalanobisThresh,
1329 std::numeric_limits<double>::max());
1332 double twistMahalanobisThresh;
1333 std::string imuTwistRejectionName =
1334 imuTopicName + std::string(
"_twist_rejection_threshold");
1335 nhLocal_.
param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits<double>::max());
1338 double accelMahalanobisThresh;
1339 nhLocal_.
param(imuTopicName + std::string(
"_linear_acceleration_rejection_threshold"),
1340 accelMahalanobisThresh,
1341 std::numeric_limits<double>::max());
1343 bool removeGravAcc =
false;
1344 nhLocal_.
param(imuTopicName +
"_remove_gravitational_acceleration", removeGravAcc,
false);
1351 std::vector<int> poseUpdateVec = updateVec;
1359 std::vector<int> twistUpdateVec = updateVec;
1367 std::vector<int> accelUpdateVec = updateVec;
1375 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
1376 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
1377 int accelUpdateSum = std::accumulate(accelUpdateVec.begin(), accelUpdateVec.end(), 0);
1382 ROS_WARN_STREAM(
"X acceleration is being measured from IMU; X velocity control input is disabled");
1387 ROS_WARN_STREAM(
"Y acceleration is being measured from IMU; Y velocity control input is disabled");
1392 ROS_WARN_STREAM(
"Z acceleration is being measured from IMU; Z velocity control input is disabled");
1396 int imuQueueSize = 1;
1397 nhLocal_.
param(imuTopicName +
"_queue_size", imuQueueSize, 1);
1399 bool nodelayImu =
false;
1400 nhLocal_.
param(imuTopicName +
"_nodelay", nodelayImu,
false);
1402 if (poseUpdateSum + twistUpdateSum + accelUpdateSum > 0)
1404 const CallbackData poseCallbackData(imuTopicName +
"_pose", poseUpdateVec, poseUpdateSum, differential,
1405 relative, poseMahalanobisThresh);
1406 const CallbackData twistCallbackData(imuTopicName +
"_twist", twistUpdateVec, twistUpdateSum, differential,
1407 relative, twistMahalanobisThresh);
1408 const CallbackData accelCallbackData(imuTopicName +
"_acceleration", accelUpdateVec, accelUpdateSum,
1409 differential, relative, accelMahalanobisThresh);
1412 nh_.
subscribe<sensor_msgs::Imu>(imuTopic, imuQueueSize,
1418 ROS_WARN_STREAM(
"Warning: " << imuTopic <<
" is listed as an input topic, " 1419 "but all its update variables are false");
1422 if (poseUpdateSum > 0)
1438 if (twistUpdateSum > 0)
1445 RF_DEBUG(
"Subscribed to " << imuTopic <<
" (" << imuTopicName <<
")\n\t" <<
1446 imuTopicName <<
"_differential is " << (differential ?
"true" :
"false") <<
"\n\t" <<
1447 imuTopicName <<
"_pose_rejection_threshold is " << poseMahalanobisThresh <<
"\n\t" <<
1448 imuTopicName <<
"_twist_rejection_threshold is " << twistMahalanobisThresh <<
"\n\t" <<
1449 imuTopicName <<
"_linear_acceleration_rejection_threshold is " << accelMahalanobisThresh <<
"\n\t" <<
1450 imuTopicName <<
"_remove_gravitational_acceleration is " <<
1451 (removeGravAcc ?
"true" :
"false") <<
"\n\t" <<
1452 imuTopicName <<
"_queue_size is " << imuQueueSize <<
"\n\t" <<
1453 imuTopicName <<
" pose update vector is " << poseUpdateVec <<
"\t"<<
1454 imuTopicName <<
" twist update vector is " << twistUpdateVec <<
"\t" <<
1455 imuTopicName <<
" acceleration update vector is " << accelUpdateVec);
1461 if (
useControl_ && std::accumulate(controlUpdateVector.begin(), controlUpdateVector.end(), 0) == 0)
1463 ROS_ERROR_STREAM(
"use_control is set to true, but control_config has only false values. No control term " 1474 filter_.setControlParams(controlUpdateVector, controlTimeout, accelerationLimits, accelerationGains,
1475 decelerationLimits, decelerationGains);
1495 if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] > 1)
1497 std::stringstream stream;
1498 stream << absPoseVarCounts[static_cast<StateMembers>(stateVar -
POSITION_OFFSET)] <<
1500 ". This may result in oscillations. Please ensure that your variances for each " 1501 "measured variable are set appropriately.";
1508 else if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] == 0)
1510 if ((static_cast<StateMembers>(stateVar) ==
StateMemberX &&
1511 twistVarCounts[static_cast<StateMembers>(
StateMemberVx)] == 0) ||
1513 twistVarCounts[static_cast<StateMembers>(
StateMemberVy)] == 0) ||
1514 (static_cast<StateMembers>(stateVar) ==
StateMemberZ &&
1526 std::stringstream stream;
1528 "velocity is being measured. This will result in unbounded " 1529 "error growth and erratic filter behavior.";
1532 stateVariableNames_[stateVar] +
"_configuration",
1542 processNoiseCovariance.setZero();
1553 int matSize = processNoiseCovariance.rows();
1555 for (
int i = 0; i < matSize; i++)
1557 for (
int j = 0; j < matSize; j++)
1564 std::ostringstream ostr;
1565 ostr << processNoiseCovarConfig[matSize * i + j];
1566 std::istringstream istr(ostr.str());
1567 istr >> processNoiseCovariance(i, j);
1580 RF_DEBUG(
"Process noise covariance is:\n" << processNoiseCovariance <<
"\n");
1586 " for process_noise_covariance (type: " <<
1587 processNoiseCovarConfig.
getType() <<
")");
1590 filter_.setProcessNoiseCovariance(processNoiseCovariance);
1595 initialEstimateErrorCovariance.setZero();
1602 nhLocal_.
getParam(
"initial_estimate_covariance", estimateErrorCovarConfig);
1606 int matSize = initialEstimateErrorCovariance.rows();
1608 for (
int i = 0; i < matSize; i++)
1610 for (
int j = 0; j < matSize; j++)
1617 std::ostringstream ostr;
1618 ostr << estimateErrorCovarConfig[matSize * i + j];
1619 std::istringstream istr(ostr.str());
1620 istr >> initialEstimateErrorCovariance(i, j);
1633 RF_DEBUG(
"Initial estimate error covariance is:\n" << initialEstimateErrorCovariance <<
"\n");
1638 estimateErrorCovarConfig.
getType() <<
1645 "ERROR reading initial_estimate_covariance (type: " << estimateErrorCovarConfig.
getType() <<
")");
1648 filter_.setEstimateErrorCovariance(initialEstimateErrorCovariance);
1652 template<
typename T>
1660 std::stringstream stream;
1661 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
1662 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
1663 msg->header.stamp.toSec() <<
")";
1665 topicName +
"_timestamp",
1668 RF_DEBUG(
"Received message that preceded the most recent pose reset. Ignoring...");
1673 RF_DEBUG(
"------ RosFilter::odometryCallback (" << topicName <<
") ------\n" <<
"Odometry message:\n" << *msg);
1678 geometry_msgs::PoseWithCovarianceStamped *posPtr =
new geometry_msgs::PoseWithCovarianceStamped();
1679 posPtr->header = msg->header;
1680 posPtr->pose = msg->pose;
1682 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
1689 geometry_msgs::TwistWithCovarianceStamped *twistPtr =
new geometry_msgs::TwistWithCovarianceStamped();
1690 twistPtr->header = msg->header;
1691 twistPtr->header.frame_id = msg->child_frame_id;
1692 twistPtr->twist = msg->twist;
1694 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
1698 RF_DEBUG(
"\n----- /RosFilter::odometryCallback (" << topicName <<
") ------\n");
1701 template<
typename T>
1704 const std::string &targetFrame,
1707 const std::string &topicName = callbackData.
topicName_;
1713 std::stringstream stream;
1714 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
1715 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
1716 msg->header.stamp.toSec() <<
")";
1718 topicName +
"_timestamp",
1724 RF_DEBUG(
"------ RosFilter::poseCallback (" << topicName <<
") ------\n" <<
1725 "Pose message:\n" << *msg);
1730 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
1741 measurement.setZero();
1742 measurementCovariance.setZero();
1745 std::vector<int> updateVectorCorrected = callbackData.
updateVector_;
1754 updateVectorCorrected,
1756 measurementCovariance))
1762 measurementCovariance,
1763 updateVectorCorrected,
1767 RF_DEBUG(
"Enqueued new measurement for " << topicName <<
"\n");
1771 RF_DEBUG(
"Did *not* enqueue measurement for " << topicName <<
"\n");
1776 RF_DEBUG(
"Last message time for " << topicName <<
" is now " <<
1785 std::stringstream stream;
1786 stream <<
"The " << topicName <<
" message has a timestamp before that of the previous message received," <<
1787 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
1788 msg->header.stamp.toSec() <<
")";
1790 topicName +
"_timestamp",
1794 RF_DEBUG(
"Message is too old. Last message time for " << topicName <<
" is " 1796 << msg->header.stamp <<
".\n");
1799 RF_DEBUG(
"\n----- /RosFilter::poseCallback (" << topicName <<
") ------\n");
1802 template<
typename T>
1806 const double loop_elapsed = (
event.current_real -
event.last_expected).toSec();
1809 ROS_WARN_STREAM(
"Failed to meet update rate! Took " << std::setprecision(20) << loop_elapsed);
1833 if (
filter_.getInitializedStatus())
1840 nav_msgs::Odometry filteredPosition;
1858 ROS_ERROR_STREAM(
"Critical Error, NaNs were detected in the output state of the filter." <<
1859 " This was likely due to poorly coniditioned process, noise, or sensor covariances.");
1870 else if (filteredPosition.header.frame_id ==
mapFrameId_)
1900 mapOdomTrans.
mult(worldBaseLinkTrans, baseLinkOdomTrans);
1902 geometry_msgs::TransformStamped mapOdomTransMsg;
1903 mapOdomTransMsg.transform =
tf2::toMsg(mapOdomTrans);
1904 mapOdomTransMsg.header.stamp = filteredPosition.header.stamp +
tfTimeOffset_;
1918 ROS_ERROR_STREAM(
"Odometry message frame_id was " << filteredPosition.header.frame_id <<
1933 geometry_msgs::AccelWithCovarianceStamped filteredAcceleration;
1957 template<
typename T>
1960 RF_DEBUG(
"------ RosFilter::setPoseCallback ------\nPose message:\n" << *msg);
1964 std::string topicName(
"setPose");
1983 std::vector<int> updateVector(
STATE_SIZE,
true);
1986 measurement.setZero();
1989 measurementCovariance.setIdentity();
1990 measurementCovariance *= 1e-6;
1997 filter_.setState(measurement);
1998 filter_.setEstimateErrorCovariance(measurementCovariance);
2002 RF_DEBUG(
"\n------ /RosFilter::setPoseCallback ------\n");
2005 template<
typename T>
2007 robot_localization::SetPose::Response&)
2009 geometry_msgs::PoseWithCovarianceStamped::Ptr
msg;
2010 msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>(request.pose);
2016 template<
typename T>
2018 std_srvs::Empty::Response&)
2024 ":] Asking for enabling filter service, but the filter was already enabled! Use param disabled_at_startup.");
2034 template<
typename T>
2037 const std::string &targetFrame)
2039 const std::string &topicName = callbackData.
topicName_;
2045 std::stringstream stream;
2046 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
2047 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
2048 msg->header.stamp.toSec() <<
")";
2050 topicName +
"_timestamp",
2056 RF_DEBUG(
"------ RosFilter::twistCallback (" << topicName <<
") ------\n" 2057 "Twist message:\n" << *msg);
2061 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
2072 measurement.setZero();
2073 measurementCovariance.setZero();
2076 std::vector<int> updateVectorCorrected = callbackData.
updateVector_;
2079 if (
prepareTwist(msg, topicName, targetFrame, updateVectorCorrected, measurement, measurementCovariance))
2085 measurementCovariance,
2086 updateVectorCorrected,
2090 RF_DEBUG(
"Enqueued new measurement for " << topicName <<
"_twist\n");
2094 RF_DEBUG(
"Did *not* enqueue measurement for " << topicName <<
"_twist\n");
2099 RF_DEBUG(
"Last message time for " << topicName <<
" is now " <<
2108 std::stringstream stream;
2109 stream <<
"The " << topicName <<
" message has a timestamp before that of the previous message received," <<
2110 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
2111 msg->header.stamp.toSec() <<
")";
2113 topicName +
"_timestamp",
2118 ", current message time is " << msg->header.stamp <<
".\n");
2121 RF_DEBUG(
"\n----- /RosFilter::twistCallback (" << topicName <<
") ------\n");
2124 template<
typename T>
2126 const std::string &topicAndClass,
2127 const std::string &message,
2128 const bool staticDiag)
2142 template<
typename T>
2151 switch (maxErrLevel)
2153 case diagnostic_msgs::DiagnosticStatus::ERROR:
2155 "Erroneous data or settings detected for a robot_localization state estimation node.");
2157 case diagnostic_msgs::DiagnosticStatus::WARN:
2159 "Potentially erroneous data or settings detected for " 2160 "a robot_localization state estimation node.");
2162 case diagnostic_msgs::DiagnosticStatus::STALE:
2164 "The state of the robot_localization state estimation node is stale.");
2166 case diagnostic_msgs::DiagnosticStatus::OK:
2168 "The robot_localization state estimation node appears to be functioning properly.");
2175 for (std::map<std::string, std::string>::iterator diagIt =
staticDiagnostics_.begin();
2179 wrapper.
add(diagIt->first, diagIt->second);
2187 wrapper.
add(diagIt->first, diagIt->second);
2195 template<
typename T>
2197 Eigen::MatrixXd &covariance,
2198 const std::string &topicName,
2199 const std::vector<int> &updateVector,
2200 const size_t offset,
2201 const size_t dimension)
2203 for (
size_t i = 0; i < dimension; i++)
2205 for (
size_t j = 0; j < dimension; j++)
2207 covariance(i, j) = arr[dimension * i + j];
2213 if (covariance(i, j) > 1e3 && (updateVector[offset + i] || updateVector[offset + j]))
2217 std::stringstream stream;
2218 stream <<
"The covariance at position (" << dimension * i + j <<
"), which corresponds to " <<
2219 (i == j ? iVar +
" variance" : iVar +
" and " + jVar +
" covariance") <<
2220 ", the value is extremely large (" << covariance(i, j) <<
"), but the update vector for " <<
2221 (i == j ? iVar : iVar +
" and/or " + jVar) <<
" is set to true. This may produce undesirable results.";
2224 topicName +
"_covariance",
2228 else if (updateVector[i] && i == j && covariance(i, j) == 0)
2230 std::stringstream stream;
2231 stream <<
"The covariance at position (" << dimension * i + j <<
"), which corresponds to " <<
2232 iVar <<
" variance, was zero. This will be replaced with a small value to maintain filter " 2233 "stability, but should be corrected at the message origin node.";
2236 topicName +
"_covariance",
2240 else if (updateVector[i] && i == j && covariance(i, j) < 0)
2242 std::stringstream stream;
2243 stream <<
"The covariance at position (" << dimension * i + j <<
2244 "), which corresponds to " << iVar <<
" variance, was negative. This will be replaced with a " 2245 "small positive value to maintain filter stability, but should be corrected at the message " 2249 topicName +
"_covariance",
2258 template<
typename T>
2261 const size_t dimension)
2263 for (
size_t i = 0; i < dimension; i++)
2265 for (
size_t j = 0; j < dimension; j++)
2267 arr[dimension * i + j] = covariance(i, j);
2272 template<
typename T>
2276 std::vector<int> updateVector(
STATE_SIZE, 0);
2277 std::string topicConfigName = topicName +
"_config";
2287 ROS_WARN_STREAM(
"Configuration vector for " << topicConfigName <<
" should have 15 entries.");
2290 for (
int i = 0; i < topicConfig.
size(); i++)
2295 updateVector[i] =
static_cast<int>(
static_cast<bool>(topicConfig[i]));
2300 ROS_FATAL_STREAM(
"Could not read sensor update configuration for topic " << topicName <<
2302 <<
"). Error was " << e.
getMessage() <<
"\n");
2305 return updateVector;
2308 template<
typename T>
2310 const std::string &topicName,
2311 const std::string &targetFrame,
2312 std::vector<int> &updateVector,
2313 Eigen::VectorXd &measurement,
2314 Eigen::MatrixXd &measurementCovariance)
2316 RF_DEBUG(
"------ RosFilter::prepareAcceleration (" << topicName <<
") ------\n");
2319 tf2::Vector3 accTmp(msg->linear_acceleration.x,
2320 msg->linear_acceleration.y,
2321 msg->linear_acceleration.z);
2324 std::string msgFrame = (msg->header.frame_id ==
"" ?
baseLinkFrameId_ : msg->header.frame_id);
2340 covarianceRotated.setZero();
2349 RF_DEBUG(
"Original measurement as tf object: " << accTmp <<
2350 "\nOriginal update vector:\n" << updateVector <<
2351 "\nOriginal covariance matrix:\n" << covarianceRotated <<
"\n");
2374 if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9)
2378 const Eigen::VectorXd &state =
filter_.getState();
2390 stateTmp = imuFrameTrans.
getBasis() * stateTmp;
2391 curAttitude.
setRPY(stateTmp.getX(), stateTmp.getY(), stateTmp.getZ());
2396 if (fabs(curAttitude.
length() - 1.0) > 0.01)
2398 ROS_WARN_ONCE(
"An input was not normalized, this should NOT happen, but will normalize.");
2404 accTmp.setX(accTmp.getX() - rotNorm.getX());
2405 accTmp.setY(accTmp.getY() - rotNorm.getY());
2406 accTmp.setZ(accTmp.getZ() - rotNorm.getZ());
2408 RF_DEBUG(
"Orientation is " << curAttitude <<
2409 "Acceleration due to gravity is " << rotNorm <<
2410 "After removing acceleration due to gravity, acceleration is " << accTmp <<
"\n");
2421 accTmp = targetFrameTrans.
getBasis() * accTmp;
2422 maskAcc = targetFrameTrans.
getBasis() * maskAcc;
2432 RF_DEBUG(msg->header.frame_id <<
"->" << targetFrame <<
" transform:\n" << targetFrameTrans <<
2433 "\nAfter applying transform to " << targetFrame <<
", update vector is:\n" << updateVector <<
2434 "\nAfter applying transform to " << targetFrame <<
", measurement is:\n" << accTmp <<
"\n");
2442 rot3d.setIdentity();
2446 rot3d(rInd, 0) = rot.getRow(rInd).getX();
2447 rot3d(rInd, 1) = rot.getRow(rInd).getY();
2448 rot3d(rInd, 2) = rot.getRow(rInd).getZ();
2452 covarianceRotated = rot3d * covarianceRotated.eval() * rot3d.transpose();
2454 RF_DEBUG(
"Transformed covariance is \n" << covarianceRotated <<
"\n");
2457 measurement(StateMemberAx) = accTmp.getX();
2458 measurement(StateMemberAy) = accTmp.getY();
2459 measurement(StateMemberAz) = accTmp.getZ();
2463 covarianceRotated.block(0, 0, ACCELERATION_SIZE, ACCELERATION_SIZE);
2468 forceTwoD(measurement, measurementCovariance, updateVector);
2473 RF_DEBUG(
"Could not transform measurement into " << targetFrame <<
". Ignoring...\n");
2476 RF_DEBUG(
"\n----- /RosFilter::prepareAcceleration(" << topicName <<
") ------\n");
2478 return canTransform;
2481 template<
typename T>
2483 const std::string &topicName,
2484 const std::string &targetFrame,
2485 const bool differential,
2486 const bool relative,
2488 std::vector<int> &updateVector,
2489 Eigen::VectorXd &measurement,
2490 Eigen::MatrixXd &measurementCovariance)
2492 bool retVal =
false;
2494 RF_DEBUG(
"------ RosFilter::preparePose (" << topicName <<
") ------\n");
2505 std::string finalTargetFrame;
2506 if (targetFrame ==
"" && msg->header.frame_id ==
"")
2513 else if (targetFrame ==
"")
2517 finalTargetFrame = msg->header.frame_id;
2523 finalTargetFrame = targetFrame;
2524 poseTmp.
frame_id_ = (differential ? finalTargetFrame : msg->header.frame_id);
2527 RF_DEBUG(
"Final target frame for " << topicName <<
" is " << finalTargetFrame <<
"\n");
2529 poseTmp.
stamp_ = msg->header.stamp;
2532 poseTmp.setOrigin(tf2::Vector3(msg->pose.pose.position.x,
2533 msg->pose.pose.position.y,
2534 msg->pose.pose.position.z));
2539 if (msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 &&
2540 msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0)
2542 orientation.setValue(0.0, 0.0, 0.0, 1.0);
2546 std::stringstream stream;
2547 stream <<
"The " << topicName <<
" message contains an invalid orientation quaternion, " <<
2548 "but its configuration is such that orientation data is being used. Correcting...";
2551 topicName +
"_orientation",
2559 if (fabs(orientation.
length() - 1.0) > 0.01)
2561 ROS_WARN_ONCE(
"An input was not normalized, this should NOT happen, but will normalize.");
2567 poseTmp.setRotation(orientation);
2612 transTmp.
setRPY(0.0, 0.0, yaw);
2614 maskPosition = transTmp * maskPosition;
2615 maskOrientation = transTmp * maskOrientation;
2619 maskPosition = targetFrameTrans.
getBasis() * maskPosition;
2620 maskOrientation = targetFrameTrans.
getBasis() * maskOrientation;
2641 covariance.setZero();
2649 rot6d.setIdentity();
2650 Eigen::MatrixXd covarianceRotated;
2657 rot.
setRPY(0.0, 0.0, yaw);
2666 rot6d(rInd, 0) = rot.
getRow(rInd).getX();
2667 rot6d(rInd, 1) = rot.
getRow(rInd).getY();
2668 rot6d(rInd, 2) = rot.
getRow(rInd).getZ();
2669 rot6d(rInd+POSITION_SIZE, 3) = rot.
getRow(rInd).getX();
2670 rot6d(rInd+POSITION_SIZE, 4) = rot.
getRow(rInd).getY();
2671 rot6d(rInd+POSITION_SIZE, 5) = rot.
getRow(rInd).getZ();
2675 covarianceRotated = rot6d * covariance * rot6d.transpose();
2677 RF_DEBUG(
"After rotating into the " << finalTargetFrame <<
2678 " frame, covariance is \n" << covarianceRotated <<
"\n");
2692 double rollOffset = 0;
2693 double pitchOffset = 0;
2694 double yawOffset = 0;
2710 mat.
setRPY(0.0, 0.0, yawOffset);
2711 rpyAngles = mat * rpyAngles;
2712 poseTmp.getBasis().setRPY(rpyAngles.getX(), rpyAngles.getY(), rpyAngles.getZ());
2725 bool success =
false;
2730 curMeasurement = poseTmp;
2746 "\nAfter removing previous measurement, measurement delta is:\n" << poseTmp <<
"\n");
2751 targetFrameTrans.
setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
2752 poseTmp.mult(targetFrameTrans, poseTmp);
2754 RF_DEBUG(
"After rotating to the target frame, measurement delta is:\n" << poseTmp <<
"\n");
2759 double xVel = poseTmp.getOrigin().getX() / dt;
2760 double yVel = poseTmp.getOrigin().getY() / dt;
2761 double zVel = poseTmp.getOrigin().getZ() / dt;
2764 double pitchVel = 0;
2773 ", current message time is " << msg->header.stamp.toSec() <<
", delta is " <<
2774 dt <<
", velocity is (vX, vY, vZ): (" << xVel <<
", " << yVel <<
", " << zVel <<
2775 ")\n" <<
"(vRoll, vPitch, vYaw): (" << rollVel <<
", " << pitchVel <<
", " <<
2779 geometry_msgs::TwistWithCovarianceStamped *twistPtr =
new geometry_msgs::TwistWithCovarianceStamped();
2780 twistPtr->header = msg->header;
2782 twistPtr->twist.twist.linear.x = xVel;
2783 twistPtr->twist.twist.linear.y = yVel;
2784 twistPtr->twist.twist.linear.z = zVel;
2785 twistPtr->twist.twist.angular.x = rollVel;
2786 twistPtr->twist.twist.angular.y = pitchVel;
2787 twistPtr->twist.twist.angular.z = yawVel;
2788 std::vector<int> twistUpdateVec(
STATE_SIZE,
false);
2792 std::copy(twistUpdateVec.begin(), twistUpdateVec.end(), updateVector.begin());
2793 geometry_msgs::TwistWithCovarianceStampedConstPtr ptr(twistPtr);
2799 covarianceRotated = (covarianceRotated.eval() + prevCovarRotated) * dt;
2803 "\nPrevious measurement covariance rotated:\n" << prevCovarRotated <<
2804 "\nFinal twist covariance:\n" << covarianceRotated <<
"\n");
2808 topicName +
"_twist",
2809 twistPtr->header.frame_id,
2812 measurementCovariance);
2836 poseTmp.mult(targetFrameTrans, poseTmp);
2840 measurement(StateMemberX) = poseTmp.getOrigin().x();
2841 measurement(StateMemberY) = poseTmp.getOrigin().y();
2842 measurement(StateMemberZ) = poseTmp.getOrigin().z();
2845 double roll, pitch, yaw;
2847 measurement(StateMemberRoll) = roll;
2848 measurement(StateMemberPitch) = pitch;
2849 measurement(StateMemberYaw) = yaw;
2856 forceTwoD(measurement, measurementCovariance, updateVector);
2866 RF_DEBUG(
"Could not transform measurement into " << finalTargetFrame <<
". Ignoring...");
2869 RF_DEBUG(
"\n----- /RosFilter::preparePose (" << topicName <<
") ------\n");
2874 template<
typename T>
2876 const std::string &topicName,
2877 const std::string &targetFrame,
2878 std::vector<int> &updateVector,
2879 Eigen::VectorXd &measurement,
2880 Eigen::MatrixXd &measurementCovariance)
2882 RF_DEBUG(
"------ RosFilter::prepareTwist (" << topicName <<
") ------\n");
2885 tf2::Vector3 twistLin(msg->twist.twist.linear.x,
2886 msg->twist.twist.linear.y,
2887 msg->twist.twist.linear.z);
2888 tf2::Vector3 measTwistRot(msg->twist.twist.angular.x,
2889 msg->twist.twist.angular.y,
2890 msg->twist.twist.angular.z);
2896 const Eigen::VectorXd &state =
filter_.getState();
2902 std::string msgFrame = (msg->header.frame_id ==
"" ? targetFrame : msg->header.frame_id);
2922 covarianceRotated.setZero();
2931 RF_DEBUG(
"Original measurement as tf object:\nLinear: " << twistLin <<
2932 "Rotational: " << measTwistRot <<
2933 "\nOriginal update vector:\n" << updateVector <<
2934 "\nOriginal covariance matrix:\n" << covarianceRotated <<
"\n");
2949 measTwistRot = targetFrameTrans.
getBasis() * measTwistRot;
2950 twistLin = targetFrameTrans.
getBasis() * twistLin + targetFrameTrans.
getOrigin().cross(stateTwistRot);
2951 maskLin = targetFrameTrans.
getBasis() * maskLin;
2952 maskRot = targetFrameTrans.
getBasis() * maskRot;
2968 RF_DEBUG(msg->header.frame_id <<
"->" << targetFrame <<
" transform:\n" << targetFrameTrans <<
2969 "\nAfter applying transform to " << targetFrame <<
", update vector is:\n" << updateVector <<
2970 "\nAfter applying transform to " << targetFrame <<
", measurement is:\n" <<
2971 "Linear: " << twistLin <<
"Rotational: " << measTwistRot <<
"\n");
2979 rot6d.setIdentity();
2983 rot6d(rInd, 0) = rot.getRow(rInd).getX();
2984 rot6d(rInd, 1) = rot.getRow(rInd).getY();
2985 rot6d(rInd, 2) = rot.getRow(rInd).getZ();
2986 rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
2987 rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
2988 rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
2992 covarianceRotated = rot6d * covarianceRotated.eval() * rot6d.transpose();
2994 RF_DEBUG(
"Transformed covariance is \n" << covarianceRotated <<
"\n");
2997 measurement(StateMemberVx) = twistLin.getX();
2998 measurement(StateMemberVy) = twistLin.getY();
2999 measurement(StateMemberVz) = twistLin.getZ();
3000 measurement(StateMemberVroll) = measTwistRot.getX();
3001 measurement(StateMemberVpitch) = measTwistRot.getY();
3002 measurement(StateMemberVyaw) = measTwistRot.getZ();
3011 forceTwoD(measurement, measurementCovariance, updateVector);
3016 RF_DEBUG(
"Could not transform measurement into " << targetFrame <<
". Ignoring...");
3019 RF_DEBUG(
"\n----- /RosFilter::prepareTwist (" << topicName <<
") ------\n");
3021 return canTransform;
3024 template<
typename T>
3028 state->state_ = Eigen::VectorXd(filter.
getState());
3031 state->latestControl_ = Eigen::VectorXd(filter.
getControl());
3034 RF_DEBUG(
"Saved state with timestamp " << std::setprecision(20) << state->lastMeasurementTime_ <<
3038 template<
typename T>
3041 RF_DEBUG(
"\n----- RosFilter::revertTo -----\n");
3042 RF_DEBUG(
"\nRequested time was " << std::setprecision(20) << time <<
"\n")
3058 bool retVal =
false;
3066 RF_DEBUG(
"Insufficient history to revert to time " << time <<
"\n");
3068 if (lastHistoryState)
3070 RF_DEBUG(
"Will revert to oldest state at " << lastHistoryState->latestControlTime_ <<
".\n");
3072 std::setprecision(20) << time <<
". Instead reverted to state with time " <<
3073 lastHistoryState->lastMeasurementTime_ <<
". History size was " << history_size);
3078 if (lastHistoryState)
3082 filter_.setState(state->state_);
3083 filter_.setEstimateErrorCovariance(state->estimateErrorCovariance_);
3084 filter_.setLastMeasurementTime(state->lastMeasurementTime_);
3086 RF_DEBUG(
"Reverted to state with time " << std::setprecision(20) << state->lastMeasurementTime_ <<
"\n");
3089 int restored_measurements = 0;
3096 restored_measurements++;
3102 RF_DEBUG(
"Restored " << restored_measurements <<
" to measurement queue.\n");
3105 RF_DEBUG(
"\n----- /RosFilter::revertTo\n");
3110 template<
typename T>
3113 return !std::isnan(message.pose.pose.position.x) && !std::isinf(message.pose.pose.position.x) &&
3114 !std::isnan(message.pose.pose.position.y) && !std::isinf(message.pose.pose.position.y) &&
3115 !std::isnan(message.pose.pose.position.z) && !std::isinf(message.pose.pose.position.z) &&
3116 !std::isnan(message.pose.pose.orientation.x) && !std::isinf(message.pose.pose.orientation.x) &&
3117 !std::isnan(message.pose.pose.orientation.y) && !std::isinf(message.pose.pose.orientation.y) &&
3118 !std::isnan(message.pose.pose.orientation.z) && !std::isinf(message.pose.pose.orientation.z) &&
3119 !std::isnan(message.pose.pose.orientation.w) && !std::isinf(message.pose.pose.orientation.w) &&
3120 !std::isnan(message.twist.twist.linear.x) && !std::isinf(message.twist.twist.linear.x) &&
3121 !std::isnan(message.twist.twist.linear.y) && !std::isinf(message.twist.twist.linear.y) &&
3122 !std::isnan(message.twist.twist.linear.z) && !std::isinf(message.twist.twist.linear.z) &&
3123 !std::isnan(message.twist.twist.angular.x) && !std::isinf(message.twist.twist.angular.x) &&
3124 !std::isnan(message.twist.twist.angular.y) && !std::isinf(message.twist.twist.angular.y) &&
3125 !std::isnan(message.twist.twist.angular.z) && !std::isinf(message.twist.twist.angular.z);
3128 template<
typename T>
3131 RF_DEBUG(
"\n----- RosFilter::clearExpiredHistory -----" <<
3132 "\nCutoff time is " << cutOffTime <<
"\n");
3134 int poppedMeasurements = 0;
3135 int poppedStates = 0;
3140 poppedMeasurements++;
3149 RF_DEBUG(
"\nPopped " << poppedMeasurements <<
" measurements and " <<
3150 poppedStates <<
" states from their respective queues." <<
3151 "\n---- /RosFilter::clearExpiredHistory ----\n");
3154 template<
typename T>
const std::string & getMessage() const
bool useControl_
Whether or not we use a control term.
ros::Time lastDiagTime_
last call of periodicUpdate
bool publishAcceleration_
Whether we publish the acceleration.
const int STATE_SIZE
Global constants that define our state vector size and offsets to groups of values within that state...
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
void integrateMeasurements(const ros::Time ¤tTime)
Processes all measurements in the measurement queue, in temporal order.
const int POSITION_A_OFFSET
bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message)
Retrieves the EKF's acceleration output for broadcasting.
void appendPrefix(std::string tfPrefix, std::string &frameId)
Utility method for appending tf2 prefixes cleanly.
void loadParams()
Loads all parameters from file.
void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName, const CallbackData &poseCallbackData, const CallbackData &twistCallbackData)
Callback method for receiving all odometry messages.
bool validateFilterOutput(const nav_msgs::Odometry &message)
Validates filter outputs for NaNs and Infinite values.
std::vector< int > updateVector_
ros::Subscriber controlSub_
Subscribes to the control input topic.
#define ROS_WARN_STREAM_THROTTLE(period, args)
void publish(const boost::shared_ptr< M > &message) const
void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const CallbackData &callbackData, const std::string &targetFrame)
Callback method for receiving all twist messages.
boost::shared_ptr< FilterState > FilterStatePtr
ros::NodeHandle nhLocal_
Local node handle (for params)
std::map< std::string, Eigen::MatrixXd > previousMeasurementCovariances_
We also need the previous covariance matrix for differential data.
const Eigen::VectorXd & getState()
Gets the filter state.
bool resetOnTimeJump_
Whether to reset the filters when backwards jump in time is detected.
MeasurementHistoryDeque measurementHistory_
A deque of previous measurements which is implicitly ordered from earliest to latest measurement...
void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
Callback method for manually setting/resetting the internal pose estimate.
void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper)
Aggregates all diagnostics so they can be published.
std::string baseLinkFrameId_
tf frame name for the robot's body frame
std::map< std::string, ros::Time > lastMessageTimes_
Store the last time a message from each topic was received.
ros::Subscriber setPoseSub_
Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWit...
void summary(unsigned char lvl, const std::string s)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg, const CallbackData &callbackData, const std::string &targetFrame)
Callback method for receiving all acceleration (IMU) messages.
bool enabled_
Whether the filter is enabled or not. See disabledAtStartup_.
ros::Timer periodicUpdateTimer_
timer calling periodicUpdate
ros::ServiceServer toggleFilterProcessingSrv_
Service that allows another node to toggle on/off filter processing while still publishing. Uses a robot_localization ToggleFilterProcessing service.
tf2_ros::TransformBroadcaster worldTransformBroadcaster_
broadcaster of worldTransform tfs
void setHardwareID(const std::string &hwid)
Eigen::VectorXd latestControl_
The most recent control input.
ros::Duration tfTimeout_
Parameter that specifies the how long we wait for a transform to become available.
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
Structure used for storing and comparing measurements (for priority queues)
void copyCovariance(const double *arr, Eigen::MatrixXd &covariance, const std::string &topicName, const std::vector< int > &updateVector, const size_t offset, const size_t dimension)
Utility method for copying covariances from ROS covariance arrays to Eigen matrices.
bool setPoseSrvCallback(robot_localization::SetPose::Request &request, robot_localization::SetPose::Response &)
Service callback for manually setting/resetting the internal pose estimate.
ros::Publisher accelPub_
optional acceleration publisher
ros::Time latestControlTime_
The time of the most recent control input.
TF2SIMD_FORCE_INLINE const Vector3 & getRow(int i) const
std::vector< int > loadUpdateConfig(const std::string &topicName)
Loads fusion settings from the config file.
std::vector< ros::Subscriber > topicSubs_
Vector to hold our subscribers until they go out of scope.
void add(const std::string &name, TaskFunction f)
double minFrequency_
minimal frequency
ROSCPP_DECL const std::string & getName()
bool printDiagnostics_
Whether or not we print diagnostic messages to the /diagnostics topic.
std::string worldFrameId_
tf frame name that is the parent frame of the transform that this node will calculate and broadcast...
int staticDiagErrorLevel_
The max (worst) static diagnostic level.
void forceTwoD(Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance, std::vector< int > &updateVector)
Method for zeroing out 3D variables within measurements.
void clearExpiredHistory(const double cutoffTime)
Removes measurements and filter states older than the given cutoff time.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
const int POSITION_V_OFFSET
ros::ServiceServer setPoseSrv_
Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service.
#define ROS_FATAL_COND(cond,...)
std::auto_ptr< diagnostic_updater::HeaderlessTopicDiagnostic > freqDiag_
optional signaling diagnostic frequency
int dynamicDiagErrorLevel_
The max (worst) dynamic diagnostic level.
void reset()
Resets the filter to its initial state.
ros::Time lastSetPoseTime_
Store the last time set pose message was received.
void periodicUpdate(const ros::TimerEvent &event)
callback function which is called for periodic updates
void controlCallback(const geometry_msgs::Twist::ConstPtr &msg)
Callback method for receiving non-stamped control input.
double getControlTime()
Returns the time at which the control term was issued.
void enqueueMeasurement(const std::string &topicName, const Eigen::VectorXd &measurement, const Eigen::MatrixXd &measurementCovariance, const std::vector< int > &updateVector, const double mahalanobisThresh, const ros::Time &time)
Adds a measurement to the queue of measurements to be processed.
bool publishTransform_
Whether we publish the transform from the world_frame to the base_link_frame.
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
const int ORIENTATION_SIZE
TransportHints & tcpNoDelay(bool nodelay=true)
RosFilter(ros::NodeHandle nh, ros::NodeHandle nh_priv, std::vector< double > args=std::vector< double >())
Constructor.
std::map< std::string, tf2::Transform > previousMeasurements_
Stores the last measurement from a given topic for differential integration.
double getLastMeasurementTime()
Gets the most recent measurement time.
std::string baseLinkOutputFrameId_
tf frame name for the robot's body frame
MeasurementQueue measurementQueue_
We process measurements by queueing them up in callbacks and processing them all at once within each ...
boost::shared_ptr< Measurement > MeasurementPtr
bool disabledAtStartup_
Start the Filter disabled at startup.
void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const CallbackData &poseCallbackData, const CallbackData &twistCallbackData, const CallbackData &accelCallbackData)
Callback method for receiving all IMU messages.
ros::Duration tfTimeOffset_
For future (or past) dating the world_frame->base_link_frame transform.
#define ROS_FATAL_STREAM(args)
Duration & fromSec(double t)
const int POSITION_OFFSET
ros::Publisher positionPub_
position publisher
ros::ServiceServer enableFilterSrv_
Service that allows another node to enable the filter. Uses a standard Empty service.
std::map< std::string, std::string > staticDiagnostics_
This object accumulates static diagnostics, e.g., diagnostics relating to the configuration parameter...
#define ROS_WARN_ONCE(...)
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const CallbackData &callbackData, const std::string &targetFrame, const bool imuData)
Callback method for receiving all pose messages.
bool smoothLaggedData_
Whether or not we use smoothing.
void addDiagnostic(const int errLevel, const std::string &topicAndClass, const std::string &message, const bool staticDiag)
Adds a diagnostic message to the accumulating map and updates the error level.
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void fromMsg(const A &, B &b)
bool toggledOn_
Whether the filter should process new measurements or not.
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
bool enableFilterSrvCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Service callback for manually enable the filter.
geometry_msgs::TransformStamped worldBaseLinkTransMsg_
Message that contains our latest transform (i.e., state)
std::string getService() const
double gravitationalAcc_
What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const int ACCELERATION_SIZE
std::string mapFrameId_
tf frame name for the robot's map (world-fixed) frame
#define ROS_WARN_STREAM(args)
bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance)
Prepares an IMU message's linear acceleration for integration into the filter.
double clampRotation(double rotation)
Utility method keeping RPY angles in the range [-pi, pi].
#define ROS_WARN_STREAM_DELAYED_THROTTLE(period, args)
void saveFilterState(FilterBase &filter)
Saves the current filter state in the queue of previous filter states.
void setRotation(const Quaternion &q)
const Eigen::VectorXd & getControl()
Returns the control vector currently being used.
#define ROS_INFO_STREAM_ONCE(args)
double rejectionThreshold_
void initialize()
Initialize filter.
bool toggleFilterProcessingCallback(robot_localization::ToggleFilterProcessing::Request &, robot_localization::ToggleFilterProcessing::Response &)
Service callback to toggle processing measurements for a standby mode but continuing to publish...
#define ROS_INFO_STREAM(args)
bool getFilteredOdometryMessage(nav_msgs::Odometry &message)
Retrieves the EKF's output for broadcasting.
std::map< std::string, tf2::Transform > initialMeasurements_
Stores the first measurement from each topic for relative measurements.
double historyLength_
The depth of the history we track for smoothing/delayed measurement processing.
const int POSE_SIZE
Pose and twist messages each contain six variables.
const int ORIENTATION_OFFSET
bool getParam(const std::string &key, std::string &s) const
bool predictToCurrentTime_
By default, the filter predicts and corrects up to the time of the latest measurement. If this is set to true, the filter does the same, but then also predicts up to the current time step.
Matrix3x3 inverse() const
std::map< std::string, std::string > dynamicDiagnostics_
This object accumulates dynamic diagnostics, e.g., diagnostics relating to sensor data...
tf2_ros::Buffer tfBuffer_
Transform buffer for managing coordinate transforms.
const Eigen::MatrixXd & getEstimateErrorCovariance()
Gets the estimate error covariance.
#define ROS_ERROR_STREAM_DELAYED_THROTTLE(period, args)
bool lookupTransformSafe(const tf2_ros::Buffer &buffer, const std::string &targetFrame, const std::string &sourceFrame, const ros::Time &time, const ros::Duration &timeout, tf2::Transform &targetFrameTrans, const bool silent=false)
Method for safely obtaining transforms.
diagnostic_updater::Updater diagnosticUpdater_
Used for updating the diagnostics.
Structure used for storing and comparing filter states.
bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const bool differential, const bool relative, const bool imuData, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance)
Prepares a pose message for integration into the filter.
bool twoDMode_
Whether or not we're in 2D mode.
void add(const std::string &key, const T &val)
bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance)
Prepares a twist message for integration into the filter.
void clearMeasurementQueue()
Clears measurement queue.
std::string odomFrameId_
tf frame name for the robot's odometry (world-fixed) frame
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
double frequency_
The frequency of the run loop.
std::ofstream debugStream_
Used for outputting debug messages.
double maxFrequency_
maximal frequency
std::vector< std::string > stateVariableNames_
Contains the state vector variable names in string format.
ros::NodeHandle nh_
Node handle.
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw)
Utility method for converting quaternion to RPY.
T filter_
Our filter (EKF, UKF, etc.)
void setData(const T &input)
bool revertTo(const double time)
Finds the latest filter state before the given timestamp and makes it the current state again...
FilterStateHistoryDeque filterStateHistory_
An implicitly time ordered queue of past filter states used for smoothing.
StateMembers
Enumeration that defines the state vector.
const int ORIENTATION_V_OFFSET
std::map< std::string, bool > removeGravitationalAcc_
If including acceleration for each IMU input, whether or not we remove acceleration due to gravity...