51 staticDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::
OK),
52 tfListener_(tfBuffer_),
53 dynamicDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::
OK),
59 latestControlTime_(0),
60 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),
131 robot_localization::ToggleFilterProcessing::Response& resp)
135 ROS_WARN_STREAM(
"Service was called to toggle filter processing but state was already as requested.");
140 ROS_INFO(
"Toggling filter measurement filtering to %s.", req.on ?
"On" :
"Off");
150 const std::string &targetFrame)
159 const std::string &topicName = callbackData.
topicName_;
161 RF_DEBUG(
"------ RosFilter::accelerationCallback (" << topicName <<
") ------\n" 162 "Twist message:\n" << *msg);
166 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
172 RF_DEBUG(
"Update vector for " << topicName <<
" is:\n" << topicName);
177 measurement.setZero();
178 measurementCovariance.setZero();
181 std::vector<int> updateVectorCorrected = callbackData.
updateVector_;
185 measurementCovariance))
191 measurementCovariance,
192 updateVectorCorrected,
196 RF_DEBUG(
"Enqueued new measurement for " << topicName <<
"_acceleration\n");
200 RF_DEBUG(
"Did *not* enqueue measurement for " << topicName <<
"_acceleration\n");
205 RF_DEBUG(
"Last message time for " << topicName <<
" is now " <<
214 std::stringstream stream;
215 stream <<
"The " << topicName <<
" message has a timestamp before that of the previous message received," <<
216 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
217 msg->header.stamp.toSec() <<
")";
219 topicName +
"_timestamp",
223 RF_DEBUG(
"Message is too old. Last message time for " << topicName <<
225 msg->header.stamp <<
".\n");
228 RF_DEBUG(
"\n----- /RosFilter::accelerationCallback (" << topicName <<
") ------\n");
234 geometry_msgs::TwistStampedPtr twistStampedPtr = geometry_msgs::TwistStampedPtr(
new geometry_msgs::TwistStamped());
235 twistStampedPtr->twist = *msg;
266 const Eigen::VectorXd &measurement,
267 const Eigen::MatrixXd &measurementCovariance,
268 const std::vector<int> &updateVector,
269 const double mahalanobisThresh,
274 meas->topicName_ = topicName;
275 meas->measurement_ = measurement;
276 meas->covariance_ = measurementCovariance;
277 meas->updateVector_ = updateVector;
278 meas->time_ = time.
toSec();
279 meas->mahalanobisThresh_ = mahalanobisThresh;
287 Eigen::MatrixXd &measurementCovariance,
288 std::vector<int> &updateVector)
319 if (
filter_.getInitializedStatus())
322 const Eigen::VectorXd &state =
filter_.getState();
323 const Eigen::MatrixXd &estimateErrorCovariance =
filter_.getEstimateErrorCovariance();
334 message.pose.pose.orientation.x = quat.x();
335 message.pose.pose.orientation.y = quat.y();
336 message.pose.pose.orientation.z = quat.z();
337 message.pose.pose.orientation.w = quat.w();
350 message.pose.covariance[POSE_SIZE * i + j] = estimateErrorCovariance(i, j);
361 message.twist.covariance[TWIST_SIZE * i + j] =
371 return filter_.getInitializedStatus();
378 if (
filter_.getInitializedStatus())
381 const Eigen::VectorXd &state =
filter_.getState();
382 const Eigen::MatrixXd &estimateErrorCovariance =
filter_.getEstimateErrorCovariance();
396 message.accel.covariance[
POSE_SIZE * i + j] =
406 return filter_.getInitializedStatus();
411 const std::string &topicName,
416 RF_DEBUG(
"------ RosFilter::imuCallback (" << topicName <<
") ------\n" <<
"IMU message:\n" << *msg);
422 std::stringstream stream;
423 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
424 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
425 msg->header.stamp.toSec() <<
")";
427 topicName +
"_timestamp",
430 RF_DEBUG(
"Received message that preceded the most recent pose reset. Ignoring...");
444 if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9)
446 RF_DEBUG(
"Received IMU message with -1 as its first covariance value for orientation. " 447 "Ignoring orientation...");
452 geometry_msgs::PoseWithCovarianceStamped *posPtr =
new geometry_msgs::PoseWithCovarianceStamped();
453 posPtr->header = msg->header;
454 posPtr->pose.pose.orientation = msg->orientation;
462 msg->orientation_covariance[ORIENTATION_SIZE * i + j];
470 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
478 if (::fabs(msg->angular_velocity_covariance[0] + 1) < 1e-9)
480 RF_DEBUG(
"Received IMU message with -1 as its first covariance value for angular " 481 "velocity. Ignoring angular velocity...");
486 geometry_msgs::TwistWithCovarianceStamped *twistPtr =
new geometry_msgs::TwistWithCovarianceStamped();
487 twistPtr->header = msg->header;
488 twistPtr->twist.twist.angular = msg->angular_velocity;
496 msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j];
500 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
508 if (::fabs(msg->linear_acceleration_covariance[0] + 1) < 1e-9)
510 RF_DEBUG(
"Received IMU message with -1 as its first covariance value for linear " 511 "acceleration. Ignoring linear acceleration...");
520 RF_DEBUG(
"\n----- /RosFilter::imuCallback (" << topicName <<
") ------\n");
526 const double currentTimeSec = currentTime.
toSec();
528 RF_DEBUG(
"------ RosFilter::integrateMeasurements ------\n\n" 529 "Integration time is " << std::setprecision(20) << currentTimeSec <<
"\n" 542 int restoredMeasurementCount = 0;
545 RF_DEBUG(
"Received a measurement that was " <<
filter_.getLastMeasurementTime() - firstMeasurement->time_ <<
546 " seconds in the past. Reverting filter state and measurement queue...");
549 const double firstMeasurementTime = firstMeasurement->time_;
550 const std::string firstMeasurementTopic = firstMeasurement->topicName_;
551 if (!
revertTo(firstMeasurementTime - 1e-9))
553 RF_DEBUG(
"ERROR: history interval is too small to revert to time " << firstMeasurementTime <<
"\n");
555 firstMeasurementTopic <<
", but history interval is insufficiently sized. Measurement time is " <<
556 std::setprecision(20) << firstMeasurementTime <<
", current time is " << currentTime.
toSec() <<
558 restoredMeasurementCount = 0;
561 restoredMeasurementCount =
static_cast<int>(
measurementQueue_.size()) - originalCount;
570 if (measurement->time_ > currentTime.
toSec())
585 filter_.setControl(measurement->latestControl_, measurement->latestControlTime_);
586 restoredMeasurementCount--;
590 filter_.processMeasurement(*(measurement.get()));
607 else if (
filter_.getInitializedStatus())
612 double lastUpdateDelta = currentTimeSec -
filter_.getLastMeasurementTime();
615 if (lastUpdateDelta >=
filter_.getSensorTimeout())
617 predictToCurrentTime =
true;
619 RF_DEBUG(
"Sensor timeout! Last measurement time was " <<
filter_.getLastMeasurementTime() <<
620 ", current time is " << currentTimeSec <<
621 ", delta is " << lastUpdateDelta <<
"\n");
626 RF_DEBUG(
"Filter not yet initialized.\n");
629 if (
filter_.getInitializedStatus() && predictToCurrentTime)
631 double lastUpdateDelta = currentTimeSec -
filter_.getLastMeasurementTime();
633 filter_.validateDelta(lastUpdateDelta);
634 filter_.predict(currentTimeSec, lastUpdateDelta);
637 filter_.setLastMeasurementTime(
filter_.getLastMeasurementTime() + lastUpdateDelta);
640 RF_DEBUG(
"\n----- /RosFilter::integrateMeasurements ------\n");
650 std::map<StateMembers, int> absPoseVarCounts;
659 std::map<StateMembers, int> twistVarCounts;
679 std::string debugOutFile;
683 nhLocal_.
param(
"debug_out_file", debugOutFile, std::string(
"robot_localization_debug.txt"));
693 ROS_WARN_STREAM(
"RosFilter::loadParams() - unable to create debug output file " << debugOutFile);
696 catch(
const std::exception &e)
698 ROS_WARN_STREAM(
"RosFilter::loadParams() - unable to create debug output file" << debugOutFile
699 <<
". Error was " << e.what() <<
"\n");
742 "Invalid frame configuration! The values for map_frame, odom_frame, " 743 "and base_link_frame must be unique. If using a base_link_frame_output values, it " 744 "must not match the map_frame or odom_frame.");
747 std::string tfPrefix =
"";
748 std::string tfPrefixPath =
"";
778 double sensorTimeout;
781 filter_.setSensorTimeout(sensorTimeout);
796 " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed.");
802 " specified. Absolute value will be assumed.");
810 bool stampedControl =
false;
811 double controlTimeout = sensorTimeout;
812 std::vector<int> controlUpdateVector(
TWIST_SIZE, 0);
813 std::vector<double> accelerationLimits(
TWIST_SIZE, 1.0);
814 std::vector<double> accelerationGains(
TWIST_SIZE, 1.0);
815 std::vector<double> decelerationLimits(
TWIST_SIZE, 1.0);
816 std::vector<double> decelerationGains(
TWIST_SIZE, 1.0);
820 nhLocal_.
param(
"control_timeout", controlTimeout, sensorTimeout);
829 "size " << controlUpdateVector.size() <<
". No control term will be used.");
835 ROS_ERROR_STREAM(
"use_control is set to true, but control_config is missing. No control term will be used.");
844 "size " << accelerationLimits.size() <<
". No control term will be used.");
850 ROS_WARN_STREAM(
"use_control is set to true, but acceleration_limits is missing. Will use default values.");
855 const int size = accelerationGains.size();
859 ". Provided config was of size " << size <<
". All gains will be assumed to be 1.");
860 std::fill_n(accelerationGains.begin(), std::min(size,
TWIST_SIZE), 1.0);
870 ". Provided config was of size " << decelerationLimits.size() <<
". No control term will be used.");
876 ROS_INFO_STREAM(
"use_control is set to true, but no deceleration_limits specified. Will use acceleration " 878 decelerationLimits = accelerationLimits;
883 const int size = decelerationGains.size();
887 ". Provided config was of size " << size <<
". All gains will be assumed to be 1.");
888 std::fill_n(decelerationGains.begin(), std::min(size,
TWIST_SIZE), 1.0);
894 ROS_INFO_STREAM(
"use_control is set to true, but no deceleration_gains specified. Will use acceleration " 896 decelerationGains = accelerationGains;
900 bool dynamicProcessNoiseCovariance =
false;
901 nhLocal_.
param(
"dynamic_process_noise_covariance", dynamicProcessNoiseCovariance,
false);
902 filter_.setUseDynamicProcessNoiseCovariance(dynamicProcessNoiseCovariance);
904 std::vector<double> initialState(
STATE_SIZE, 0.0);
910 initialState.size() <<
". The initial state will be ignored.");
914 Eigen::Map<Eigen::VectorXd> eigenState(initialState.data(), initialState.size());
925 RF_DEBUG(
"tf_prefix is " << tfPrefix <<
934 "\nsensor_timeout is " <<
filter_.getSensorTimeout() <<
935 "\ntwo_d_mode is " << (
twoDMode_ ?
"true" :
"false") <<
938 "\nuse_control is " << (
useControl_ ?
"true" :
"false") <<
939 "\nstamped_control is " << (stampedControl ?
"true" :
"false") <<
940 "\ncontrol_config is " << controlUpdateVector <<
941 "\ncontrol_timeout is " << controlTimeout <<
942 "\nacceleration_limits are " << accelerationLimits <<
943 "\nacceleration_gains are " << accelerationGains <<
944 "\ndeceleration_limits are " << decelerationLimits <<
945 "\ndeceleration_gains are " << decelerationGains <<
946 "\ninitial state is " <<
filter_.getState() <<
947 "\ndynamic_process_noise_covariance is " << (dynamicProcessNoiseCovariance ?
"true" :
"false") <<
972 bool moreParams =
false;
978 std::stringstream ss;
979 ss <<
"odom" << topicInd++;
980 std::string odomTopicName = ss.str();
987 nhLocal_.
param(odomTopicName + std::string(
"_differential"), differential,
false);
991 nhLocal_.
param(odomTopicName + std::string(
"_relative"), relative,
false);
993 if (relative && differential)
995 ROS_WARN_STREAM(
"Both " << odomTopicName <<
"_differential" <<
" and " << odomTopicName <<
996 "_relative were set to true. Using differential mode.");
1001 std::string odomTopic;
1005 double poseMahalanobisThresh;
1006 nhLocal_.
param(odomTopicName + std::string(
"_pose_rejection_threshold"),
1007 poseMahalanobisThresh,
1008 std::numeric_limits<double>::max());
1011 double twistMahalanobisThresh;
1012 nhLocal_.
param(odomTopicName + std::string(
"_twist_rejection_threshold"),
1013 twistMahalanobisThresh,
1014 std::numeric_limits<double>::max());
1020 std::vector<int> poseUpdateVec = updateVec;
1022 std::vector<int> twistUpdateVec = updateVec;
1025 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
1026 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
1027 int odomQueueSize = 1;
1028 nhLocal_.
param(odomTopicName +
"_queue_size", odomQueueSize, 1);
1030 const CallbackData poseCallbackData(odomTopicName +
"_pose", poseUpdateVec, poseUpdateSum, differential,
1031 relative, poseMahalanobisThresh);
1032 const CallbackData twistCallbackData(odomTopicName +
"_twist", twistUpdateVec, twistUpdateSum,
false,
false,
1033 twistMahalanobisThresh);
1035 bool nodelayOdom =
false;
1036 nhLocal_.
param(odomTopicName +
"_nodelay", nodelayOdom,
false);
1039 if (poseUpdateSum + twistUpdateSum > 0)
1042 nh_.
subscribe<nav_msgs::Odometry>(odomTopic, odomQueueSize,
1048 std::stringstream stream;
1049 stream << odomTopic <<
" is listed as an input topic, but all update variables are false";
1052 odomTopic +
"_configuration",
1057 if (poseUpdateSum > 0)
1079 if (twistUpdateSum > 0)
1089 RF_DEBUG(
"Subscribed to " << odomTopic <<
" (" << odomTopicName <<
")\n\t" <<
1090 odomTopicName <<
"_differential is " << (differential ?
"true" :
"false") <<
"\n\t" <<
1091 odomTopicName <<
"_pose_rejection_threshold is " << poseMahalanobisThresh <<
"\n\t" <<
1092 odomTopicName <<
"_twist_rejection_threshold is " << twistMahalanobisThresh <<
"\n\t" <<
1093 odomTopicName <<
"_queue_size is " << odomQueueSize <<
"\n\t" <<
1094 odomTopicName <<
" pose update vector is " << poseUpdateVec <<
"\t"<<
1095 odomTopicName <<
" twist update vector is " << twistUpdateVec);
1105 std::stringstream ss;
1106 ss <<
"pose" << topicInd++;
1107 std::string poseTopicName = ss.str();
1113 nhLocal_.
param(poseTopicName + std::string(
"_differential"), differential,
false);
1117 nhLocal_.
param(poseTopicName + std::string(
"_relative"), relative,
false);
1119 if (relative && differential)
1121 ROS_WARN_STREAM(
"Both " << poseTopicName <<
"_differential" <<
" and " << poseTopicName <<
1122 "_relative were set to true. Using differential mode.");
1127 std::string poseTopic;
1131 double poseMahalanobisThresh;
1132 nhLocal_.
param(poseTopicName + std::string(
"_rejection_threshold"),
1133 poseMahalanobisThresh,
1134 std::numeric_limits<double>::max());
1136 int poseQueueSize = 1;
1137 nhLocal_.
param(poseTopicName +
"_queue_size", poseQueueSize, 1);
1139 bool nodelayPose =
false;
1140 nhLocal_.
param(poseTopicName +
"_nodelay", nodelayPose,
false);
1151 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
1153 if (poseUpdateSum > 0)
1155 const CallbackData callbackData(poseTopicName, poseUpdateVec, poseUpdateSum, differential, relative,
1156 poseMahalanobisThresh);
1159 nh_.
subscribe<geometry_msgs::PoseWithCovarianceStamped>(poseTopic, poseQueueSize,
1184 ROS_WARN_STREAM(
"Warning: " << poseTopic <<
" is listed as an input topic, " 1185 "but all pose update variables are false");
1188 RF_DEBUG(
"Subscribed to " << poseTopic <<
" (" << poseTopicName <<
")\n\t" <<
1189 poseTopicName <<
"_differential is " << (differential ?
"true" :
"false") <<
"\n\t" <<
1190 poseTopicName <<
"_rejection_threshold is " << poseMahalanobisThresh <<
"\n\t" <<
1191 poseTopicName <<
"_queue_size is " << poseQueueSize <<
"\n\t" <<
1192 poseTopicName <<
" update vector is " << poseUpdateVec);
1202 std::stringstream ss;
1203 ss <<
"twist" << topicInd++;
1204 std::string twistTopicName = ss.str();
1209 std::string twistTopic;
1213 double twistMahalanobisThresh;
1214 nhLocal_.
param(twistTopicName + std::string(
"_rejection_threshold"),
1215 twistMahalanobisThresh,
1216 std::numeric_limits<double>::max());
1218 int twistQueueSize = 1;
1219 nhLocal_.
param(twistTopicName +
"_queue_size", twistQueueSize, 1);
1221 bool nodelayTwist =
false;
1222 nhLocal_.
param(twistTopicName +
"_nodelay", nodelayTwist,
false);
1228 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
1230 if (twistUpdateSum > 0)
1232 const CallbackData callbackData(twistTopicName, twistUpdateVec, twistUpdateSum,
false,
false,
1233 twistMahalanobisThresh);
1236 nh_.
subscribe<geometry_msgs::TwistWithCovarianceStamped>(twistTopic, twistQueueSize,
1249 ROS_WARN_STREAM(
"Warning: " << twistTopic <<
" is listed as an input topic, " 1250 "but all twist update variables are false");
1253 RF_DEBUG(
"Subscribed to " << twistTopic <<
" (" << twistTopicName <<
")\n\t" <<
1254 twistTopicName <<
"_rejection_threshold is " << twistMahalanobisThresh <<
"\n\t" <<
1255 twistTopicName <<
"_queue_size is " << twistQueueSize <<
"\n\t" <<
1256 twistTopicName <<
" update vector is " << twistUpdateVec);
1266 std::stringstream ss;
1267 ss <<
"imu" << topicInd++;
1268 std::string imuTopicName = ss.str();
1274 nhLocal_.
param(imuTopicName + std::string(
"_differential"), differential,
false);
1278 nhLocal_.
param(imuTopicName + std::string(
"_relative"), relative,
false);
1280 if (relative && differential)
1282 ROS_WARN_STREAM(
"Both " << imuTopicName <<
"_differential" <<
" and " << imuTopicName <<
1283 "_relative were set to true. Using differential mode.");
1288 std::string imuTopic;
1292 double poseMahalanobisThresh;
1293 nhLocal_.
param(imuTopicName + std::string(
"_pose_rejection_threshold"),
1294 poseMahalanobisThresh,
1295 std::numeric_limits<double>::max());
1298 double twistMahalanobisThresh;
1299 std::string imuTwistRejectionName =
1300 imuTopicName + std::string(
"_twist_rejection_threshold");
1301 nhLocal_.
param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits<double>::max());
1304 double accelMahalanobisThresh;
1305 nhLocal_.
param(imuTopicName + std::string(
"_linear_acceleration_rejection_threshold"),
1306 accelMahalanobisThresh,
1307 std::numeric_limits<double>::max());
1309 bool removeGravAcc =
false;
1310 nhLocal_.
param(imuTopicName +
"_remove_gravitational_acceleration", removeGravAcc,
false);
1317 std::vector<int> poseUpdateVec = updateVec;
1325 std::vector<int> twistUpdateVec = updateVec;
1333 std::vector<int> accelUpdateVec = updateVec;
1341 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
1342 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
1343 int accelUpdateSum = std::accumulate(accelUpdateVec.begin(), accelUpdateVec.end(), 0);
1348 ROS_WARN_STREAM(
"X acceleration is being measured from IMU; X velocity control input is disabled");
1353 ROS_WARN_STREAM(
"Y acceleration is being measured from IMU; Y velocity control input is disabled");
1358 ROS_WARN_STREAM(
"Z acceleration is being measured from IMU; Z velocity control input is disabled");
1362 int imuQueueSize = 1;
1363 nhLocal_.
param(imuTopicName +
"_queue_size", imuQueueSize, 1);
1365 bool nodelayImu =
false;
1366 nhLocal_.
param(imuTopicName +
"_nodelay", nodelayImu,
false);
1368 if (poseUpdateSum + twistUpdateSum + accelUpdateSum > 0)
1370 const CallbackData poseCallbackData(imuTopicName +
"_pose", poseUpdateVec, poseUpdateSum, differential,
1371 relative, poseMahalanobisThresh);
1372 const CallbackData twistCallbackData(imuTopicName +
"_twist", twistUpdateVec, twistUpdateSum, differential,
1373 relative, twistMahalanobisThresh);
1374 const CallbackData accelCallbackData(imuTopicName +
"_acceleration", accelUpdateVec, accelUpdateSum,
1375 differential, relative, accelMahalanobisThresh);
1378 nh_.
subscribe<sensor_msgs::Imu>(imuTopic, imuQueueSize,
1384 ROS_WARN_STREAM(
"Warning: " << imuTopic <<
" is listed as an input topic, " 1385 "but all its update variables are false");
1388 if (poseUpdateSum > 0)
1404 if (twistUpdateSum > 0)
1411 RF_DEBUG(
"Subscribed to " << imuTopic <<
" (" << imuTopicName <<
")\n\t" <<
1412 imuTopicName <<
"_differential is " << (differential ?
"true" :
"false") <<
"\n\t" <<
1413 imuTopicName <<
"_pose_rejection_threshold is " << poseMahalanobisThresh <<
"\n\t" <<
1414 imuTopicName <<
"_twist_rejection_threshold is " << twistMahalanobisThresh <<
"\n\t" <<
1415 imuTopicName <<
"_linear_acceleration_rejection_threshold is " << accelMahalanobisThresh <<
"\n\t" <<
1416 imuTopicName <<
"_remove_gravitational_acceleration is " <<
1417 (removeGravAcc ?
"true" :
"false") <<
"\n\t" <<
1418 imuTopicName <<
"_queue_size is " << imuQueueSize <<
"\n\t" <<
1419 imuTopicName <<
" pose update vector is " << poseUpdateVec <<
"\t"<<
1420 imuTopicName <<
" twist update vector is " << twistUpdateVec <<
"\t" <<
1421 imuTopicName <<
" acceleration update vector is " << accelUpdateVec);
1427 if (
useControl_ && std::accumulate(controlUpdateVector.begin(), controlUpdateVector.end(), 0) == 0)
1429 ROS_ERROR_STREAM(
"use_control is set to true, but control_config has only false values. No control term " 1440 filter_.setControlParams(controlUpdateVector, controlTimeout, accelerationLimits, accelerationGains,
1441 decelerationLimits, decelerationGains);
1461 if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] > 1)
1463 std::stringstream stream;
1464 stream << absPoseVarCounts[static_cast<StateMembers>(stateVar -
POSITION_OFFSET)] <<
1466 ". This may result in oscillations. Please ensure that your variances for each " 1467 "measured variable are set appropriately.";
1474 else if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] == 0)
1476 if ((static_cast<StateMembers>(stateVar) ==
StateMemberX &&
1477 twistVarCounts[static_cast<StateMembers>(
StateMemberVx)] == 0) ||
1479 twistVarCounts[static_cast<StateMembers>(
StateMemberVy)] == 0) ||
1480 (static_cast<StateMembers>(stateVar) ==
StateMemberZ &&
1492 std::stringstream stream;
1494 "velocity is being measured. This will result in unbounded " 1495 "error growth and erratic filter behavior.";
1498 stateVariableNames_[stateVar] +
"_configuration",
1508 processNoiseCovariance.setZero();
1519 int matSize = processNoiseCovariance.rows();
1521 for (
int i = 0; i < matSize; i++)
1523 for (
int j = 0; j < matSize; j++)
1530 std::ostringstream ostr;
1531 ostr << processNoiseCovarConfig[matSize * i + j];
1532 std::istringstream istr(ostr.str());
1533 istr >> processNoiseCovariance(i, j);
1546 RF_DEBUG(
"Process noise covariance is:\n" << processNoiseCovariance <<
"\n");
1552 " for process_noise_covariance (type: " <<
1553 processNoiseCovarConfig.
getType() <<
")");
1556 filter_.setProcessNoiseCovariance(processNoiseCovariance);
1561 initialEstimateErrorCovariance.setZero();
1568 nhLocal_.
getParam(
"initial_estimate_covariance", estimateErrorCovarConfig);
1572 int matSize = initialEstimateErrorCovariance.rows();
1574 for (
int i = 0; i < matSize; i++)
1576 for (
int j = 0; j < matSize; j++)
1583 std::ostringstream ostr;
1584 ostr << estimateErrorCovarConfig[matSize * i + j];
1585 std::istringstream istr(ostr.str());
1586 istr >> initialEstimateErrorCovariance(i, j);
1599 RF_DEBUG(
"Initial estimate error covariance is:\n" << initialEstimateErrorCovariance <<
"\n");
1604 estimateErrorCovarConfig.
getType() <<
1611 "ERROR reading initial_estimate_covariance (type: " << estimateErrorCovarConfig.
getType() <<
")");
1614 filter_.setEstimateErrorCovariance(initialEstimateErrorCovariance);
1618 template<
typename T>
1626 std::stringstream stream;
1627 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
1628 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
1629 msg->header.stamp.toSec() <<
")";
1631 topicName +
"_timestamp",
1634 RF_DEBUG(
"Received message that preceded the most recent pose reset. Ignoring...");
1638 RF_DEBUG(
"------ RosFilter::odometryCallback (" << topicName <<
") ------\n" <<
"Odometry message:\n" << *msg);
1643 geometry_msgs::PoseWithCovarianceStamped *posPtr =
new geometry_msgs::PoseWithCovarianceStamped();
1644 posPtr->header = msg->header;
1645 posPtr->pose = msg->pose;
1647 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
1654 geometry_msgs::TwistWithCovarianceStamped *twistPtr =
new geometry_msgs::TwistWithCovarianceStamped();
1655 twistPtr->header = msg->header;
1656 twistPtr->header.frame_id = msg->child_frame_id;
1657 twistPtr->twist = msg->twist;
1659 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
1663 RF_DEBUG(
"\n----- /RosFilter::odometryCallback (" << topicName <<
") ------\n");
1666 template<
typename T>
1669 const std::string &targetFrame,
1672 const std::string &topicName = callbackData.
topicName_;
1678 std::stringstream stream;
1679 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
1680 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
1681 msg->header.stamp.toSec() <<
")";
1683 topicName +
"_timestamp",
1689 RF_DEBUG(
"------ RosFilter::poseCallback (" << topicName <<
") ------\n" <<
1690 "Pose message:\n" << *msg);
1695 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
1706 measurement.setZero();
1707 measurementCovariance.setZero();
1710 std::vector<int> updateVectorCorrected = callbackData.
updateVector_;
1719 updateVectorCorrected,
1721 measurementCovariance))
1727 measurementCovariance,
1728 updateVectorCorrected,
1732 RF_DEBUG(
"Enqueued new measurement for " << topicName <<
"\n");
1736 RF_DEBUG(
"Did *not* enqueue measurement for " << topicName <<
"\n");
1741 RF_DEBUG(
"Last message time for " << topicName <<
" is now " <<
1750 std::stringstream stream;
1751 stream <<
"The " << topicName <<
" message has a timestamp before that of the previous message received," <<
1752 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
1753 msg->header.stamp.toSec() <<
")";
1755 topicName +
"_timestamp",
1759 RF_DEBUG(
"Message is too old. Last message time for " << topicName <<
" is " 1761 << msg->header.stamp <<
".\n");
1764 RF_DEBUG(
"\n----- /RosFilter::poseCallback (" << topicName <<
") ------\n");
1767 template<
typename T>
1770 ROS_INFO(
"Waiting for valid clock time...");
1772 ROS_INFO(
"Valid clock time received. Starting node.");
1794 geometry_msgs::TransformStamped mapOdomTransMsg;
1810 accelPub =
nh_.
advertise<geometry_msgs::AccelWithCovarianceStamped>(
"accel/filtered", 20);
1846 if (
filter_.getInitializedStatus())
1853 nav_msgs::Odometry filteredPosition;
1870 ROS_ERROR_STREAM(
"Critical Error, NaNs were detected in the output state of the filter." <<
1871 " This was likely due to poorly coniditioned process, noise, or sensor covariances.");
1882 else if (filteredPosition.header.frame_id ==
mapFrameId_)
1920 mapOdomTrans.
mult(worldBaseLinkTrans, baseLinkOdomTrans);
1922 mapOdomTransMsg.transform =
tf2::toMsg(mapOdomTrans);
1923 mapOdomTransMsg.header.stamp = filteredPosition.header.stamp +
tfTimeOffset_;
1927 worldTransformBroadcaster.sendTransform(mapOdomTransMsg);
1937 ROS_ERROR_STREAM(
"Odometry message frame_id was " << filteredPosition.header.frame_id <<
1943 positionPub.
publish(filteredPosition);
1952 geometry_msgs::AccelWithCovarianceStamped filteredAcceleration;
1955 accelPub.
publish(filteredAcceleration);
1962 double diagDuration = (curTime - lastDiagTime).toSec();
1966 lastDiagTime = curTime;
1977 if (loop_elapsed_time > loop_cycle_time)
1980 loop_elapsed_time.toSec() <<
" seconds. Try decreasing the rate, limiting sensor output frequency, or " 1981 "limiting the number of sensors.");
1985 ros::Duration sleep_time = loop_cycle_time - loop_elapsed_time;
1993 template<
typename T>
1996 RF_DEBUG(
"------ RosFilter::setPoseCallback ------\nPose message:\n" << *msg);
2000 std::string topicName(
"setPose");
2019 std::vector<int> updateVector(
STATE_SIZE,
true);
2022 measurement.setZero();
2025 measurementCovariance.setIdentity();
2026 measurementCovariance *= 1e-6;
2033 filter_.setState(measurement);
2034 filter_.setEstimateErrorCovariance(measurementCovariance);
2038 RF_DEBUG(
"\n------ /RosFilter::setPoseCallback ------\n");
2041 template<
typename T>
2043 robot_localization::SetPose::Response&)
2045 geometry_msgs::PoseWithCovarianceStamped::Ptr
msg;
2046 msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>(request.pose);
2052 template<
typename T>
2054 std_srvs::Empty::Response&)
2060 ":] Asking for enabling filter service, but the filter was already enabled! Use param disabled_at_startup.");
2070 template<
typename T>
2073 const std::string &targetFrame)
2075 const std::string &topicName = callbackData.
topicName_;
2081 std::stringstream stream;
2082 stream <<
"The " << topicName <<
" message has a timestamp equal to or before the last filter reset, " <<
2083 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
2084 msg->header.stamp.toSec() <<
")";
2086 topicName +
"_timestamp",
2092 RF_DEBUG(
"------ RosFilter::twistCallback (" << topicName <<
") ------\n" 2093 "Twist message:\n" << *msg);
2097 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
2108 measurement.setZero();
2109 measurementCovariance.setZero();
2112 std::vector<int> updateVectorCorrected = callbackData.
updateVector_;
2115 if (
prepareTwist(msg, topicName, targetFrame, updateVectorCorrected, measurement, measurementCovariance))
2121 measurementCovariance,
2122 updateVectorCorrected,
2126 RF_DEBUG(
"Enqueued new measurement for " << topicName <<
"_twist\n");
2130 RF_DEBUG(
"Did *not* enqueue measurement for " << topicName <<
"_twist\n");
2135 RF_DEBUG(
"Last message time for " << topicName <<
" is now " <<
2144 std::stringstream stream;
2145 stream <<
"The " << topicName <<
" message has a timestamp before that of the previous message received," <<
2146 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
2147 msg->header.stamp.toSec() <<
")";
2149 topicName +
"_timestamp",
2154 ", current message time is " << msg->header.stamp <<
".\n");
2157 RF_DEBUG(
"\n----- /RosFilter::twistCallback (" << topicName <<
") ------\n");
2160 template<
typename T>
2162 const std::string &topicAndClass,
2163 const std::string &message,
2164 const bool staticDiag)
2178 template<
typename T>
2187 switch (maxErrLevel)
2189 case diagnostic_msgs::DiagnosticStatus::ERROR:
2191 "Erroneous data or settings detected for a robot_localization state estimation node.");
2193 case diagnostic_msgs::DiagnosticStatus::WARN:
2195 "Potentially erroneous data or settings detected for " 2196 "a robot_localization state estimation node.");
2198 case diagnostic_msgs::DiagnosticStatus::STALE:
2200 "The state of the robot_localization state estimation node is stale.");
2202 case diagnostic_msgs::DiagnosticStatus::OK:
2204 "The robot_localization state estimation node appears to be functioning properly.");
2211 for (std::map<std::string, std::string>::iterator diagIt =
staticDiagnostics_.begin();
2215 wrapper.
add(diagIt->first, diagIt->second);
2223 wrapper.
add(diagIt->first, diagIt->second);
2231 template<
typename T>
2233 Eigen::MatrixXd &covariance,
2234 const std::string &topicName,
2235 const std::vector<int> &updateVector,
2236 const size_t offset,
2237 const size_t dimension)
2239 for (
size_t i = 0; i < dimension; i++)
2241 for (
size_t j = 0; j < dimension; j++)
2243 covariance(i, j) = arr[dimension * i + j];
2249 if (covariance(i, j) > 1e3 && (updateVector[offset + i] || updateVector[offset + j]))
2253 std::stringstream stream;
2254 stream <<
"The covariance at position (" << dimension * i + j <<
"), which corresponds to " <<
2255 (i == j ? iVar +
" variance" : iVar +
" and " + jVar +
" covariance") <<
2256 ", the value is extremely large (" << covariance(i, j) <<
"), but the update vector for " <<
2257 (i == j ? iVar : iVar +
" and/or " + jVar) <<
" is set to true. This may produce undesirable results.";
2260 topicName +
"_covariance",
2264 else if (updateVector[i] && i == j && covariance(i, j) == 0)
2266 std::stringstream stream;
2267 stream <<
"The covariance at position (" << dimension * i + j <<
"), which corresponds to " <<
2268 iVar <<
" variance, was zero. This will be replaced with a small value to maintain filter " 2269 "stability, but should be corrected at the message origin node.";
2272 topicName +
"_covariance",
2276 else if (updateVector[i] && i == j && covariance(i, j) < 0)
2278 std::stringstream stream;
2279 stream <<
"The covariance at position (" << dimension * i + j <<
2280 "), which corresponds to " << iVar <<
" variance, was negative. This will be replaced with a " 2281 "small positive value to maintain filter stability, but should be corrected at the message " 2285 topicName +
"_covariance",
2294 template<
typename T>
2297 const size_t dimension)
2299 for (
size_t i = 0; i < dimension; i++)
2301 for (
size_t j = 0; j < dimension; j++)
2303 arr[dimension * i + j] = covariance(i, j);
2308 template<
typename T>
2312 std::vector<int> updateVector(
STATE_SIZE, 0);
2313 std::string topicConfigName = topicName +
"_config";
2323 ROS_WARN_STREAM(
"Configuration vector for " << topicConfigName <<
" should have 15 entries.");
2326 for (
int i = 0; i < topicConfig.
size(); i++)
2331 updateVector[i] =
static_cast<int>(
static_cast<bool>(topicConfig[i]));
2336 ROS_FATAL_STREAM(
"Could not read sensor update configuration for topic " << topicName <<
2338 <<
"). Error was " << e.
getMessage() <<
"\n");
2341 return updateVector;
2344 template<
typename T>
2346 const std::string &topicName,
2347 const std::string &targetFrame,
2348 std::vector<int> &updateVector,
2349 Eigen::VectorXd &measurement,
2350 Eigen::MatrixXd &measurementCovariance)
2352 RF_DEBUG(
"------ RosFilter::prepareAcceleration (" << topicName <<
") ------\n");
2355 tf2::Vector3 accTmp(msg->linear_acceleration.x,
2356 msg->linear_acceleration.y,
2357 msg->linear_acceleration.z);
2360 std::string msgFrame = (msg->header.frame_id ==
"" ?
baseLinkFrameId_ : msg->header.frame_id);
2376 covarianceRotated.setZero();
2385 RF_DEBUG(
"Original measurement as tf object: " << accTmp <<
2386 "\nOriginal update vector:\n" << updateVector <<
2387 "\nOriginal covariance matrix:\n" << covarianceRotated <<
"\n");
2410 if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9)
2414 const Eigen::VectorXd &state =
filter_.getState();
2426 stateTmp = imuFrameTrans.
getBasis() * stateTmp;
2427 curAttitude.
setRPY(stateTmp.getX(), stateTmp.getY(), stateTmp.getZ());
2432 if (fabs(curAttitude.
length() - 1.0) > 0.01)
2434 ROS_WARN_ONCE(
"An input was not normalized, this should NOT happen, but will normalize.");
2440 accTmp.setX(accTmp.getX() - rotNorm.getX());
2441 accTmp.setY(accTmp.getY() - rotNorm.getY());
2442 accTmp.setZ(accTmp.getZ() - rotNorm.getZ());
2444 RF_DEBUG(
"Orientation is " << curAttitude <<
2445 "Acceleration due to gravity is " << rotNorm <<
2446 "After removing acceleration due to gravity, acceleration is " << accTmp <<
"\n");
2457 accTmp = targetFrameTrans.
getBasis() * accTmp;
2458 maskAcc = targetFrameTrans.
getBasis() * maskAcc;
2468 RF_DEBUG(msg->header.frame_id <<
"->" << targetFrame <<
" transform:\n" << targetFrameTrans <<
2469 "\nAfter applying transform to " << targetFrame <<
", update vector is:\n" << updateVector <<
2470 "\nAfter applying transform to " << targetFrame <<
", measurement is:\n" << accTmp <<
"\n");
2478 rot3d.setIdentity();
2482 rot3d(rInd, 0) = rot.getRow(rInd).getX();
2483 rot3d(rInd, 1) = rot.getRow(rInd).getY();
2484 rot3d(rInd, 2) = rot.getRow(rInd).getZ();
2488 covarianceRotated = rot3d * covarianceRotated.eval() * rot3d.transpose();
2490 RF_DEBUG(
"Transformed covariance is \n" << covarianceRotated <<
"\n");
2493 measurement(StateMemberAx) = accTmp.getX();
2494 measurement(StateMemberAy) = accTmp.getY();
2495 measurement(StateMemberAz) = accTmp.getZ();
2499 covarianceRotated.block(0, 0, ACCELERATION_SIZE, ACCELERATION_SIZE);
2504 forceTwoD(measurement, measurementCovariance, updateVector);
2509 RF_DEBUG(
"Could not transform measurement into " << targetFrame <<
". Ignoring...\n");
2512 RF_DEBUG(
"\n----- /RosFilter::prepareAcceleration(" << topicName <<
") ------\n");
2514 return canTransform;
2517 template<
typename T>
2519 const std::string &topicName,
2520 const std::string &targetFrame,
2521 const bool differential,
2522 const bool relative,
2524 std::vector<int> &updateVector,
2525 Eigen::VectorXd &measurement,
2526 Eigen::MatrixXd &measurementCovariance)
2528 bool retVal =
false;
2530 RF_DEBUG(
"------ RosFilter::preparePose (" << topicName <<
") ------\n");
2541 std::string finalTargetFrame;
2542 if (targetFrame ==
"" && msg->header.frame_id ==
"")
2549 else if (targetFrame ==
"")
2553 finalTargetFrame = msg->header.frame_id;
2559 finalTargetFrame = targetFrame;
2560 poseTmp.
frame_id_ = (differential ? finalTargetFrame : msg->header.frame_id);
2563 RF_DEBUG(
"Final target frame for " << topicName <<
" is " << finalTargetFrame <<
"\n");
2565 poseTmp.
stamp_ = msg->header.stamp;
2568 poseTmp.setOrigin(tf2::Vector3(msg->pose.pose.position.x,
2569 msg->pose.pose.position.y,
2570 msg->pose.pose.position.z));
2575 if (msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 &&
2576 msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0)
2578 orientation.setValue(0.0, 0.0, 0.0, 1.0);
2582 std::stringstream stream;
2583 stream <<
"The " << topicName <<
" message contains an invalid orientation quaternion, " <<
2584 "but its configuration is such that orientation data is being used. Correcting...";
2587 topicName +
"_orientation",
2595 if (fabs(orientation.
length() - 1.0) > 0.01)
2597 ROS_WARN_ONCE(
"An input was not normalized, this should NOT happen, but will normalize.");
2603 poseTmp.setRotation(orientation);
2648 transTmp.
setRPY(0.0, 0.0, yaw);
2650 maskPosition = transTmp * maskPosition;
2651 maskOrientation = transTmp * maskOrientation;
2655 maskPosition = targetFrameTrans.
getBasis() * maskPosition;
2656 maskOrientation = targetFrameTrans.
getBasis() * maskOrientation;
2677 covariance.setZero();
2685 rot6d.setIdentity();
2686 Eigen::MatrixXd covarianceRotated;
2693 rot.
setRPY(0.0, 0.0, yaw);
2702 rot6d(rInd, 0) = rot.
getRow(rInd).getX();
2703 rot6d(rInd, 1) = rot.
getRow(rInd).getY();
2704 rot6d(rInd, 2) = rot.
getRow(rInd).getZ();
2705 rot6d(rInd+POSITION_SIZE, 3) = rot.
getRow(rInd).getX();
2706 rot6d(rInd+POSITION_SIZE, 4) = rot.
getRow(rInd).getY();
2707 rot6d(rInd+POSITION_SIZE, 5) = rot.
getRow(rInd).getZ();
2711 covarianceRotated = rot6d * covariance * rot6d.transpose();
2713 RF_DEBUG(
"After rotating into the " << finalTargetFrame <<
2714 " frame, covariance is \n" << covarianceRotated <<
"\n");
2728 double rollOffset = 0;
2729 double pitchOffset = 0;
2730 double yawOffset = 0;
2746 mat.
setRPY(0.0, 0.0, yawOffset);
2747 rpyAngles = mat * rpyAngles;
2748 poseTmp.getBasis().setRPY(rpyAngles.getX(), rpyAngles.getY(), rpyAngles.getZ());
2761 bool success =
false;
2766 curMeasurement = poseTmp;
2782 "\nAfter removing previous measurement, measurement delta is:\n" << poseTmp <<
"\n");
2787 targetFrameTrans.
setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
2788 poseTmp.mult(targetFrameTrans, poseTmp);
2790 RF_DEBUG(
"After rotating to the target frame, measurement delta is:\n" << poseTmp <<
"\n");
2795 double xVel = poseTmp.getOrigin().getX() / dt;
2796 double yVel = poseTmp.getOrigin().getY() / dt;
2797 double zVel = poseTmp.getOrigin().getZ() / dt;
2800 double pitchVel = 0;
2809 ", current message time is " << msg->header.stamp.toSec() <<
", delta is " <<
2810 dt <<
", velocity is (vX, vY, vZ): (" << xVel <<
", " << yVel <<
", " << zVel <<
2811 ")\n" <<
"(vRoll, vPitch, vYaw): (" << rollVel <<
", " << pitchVel <<
", " <<
2815 geometry_msgs::TwistWithCovarianceStamped *twistPtr =
new geometry_msgs::TwistWithCovarianceStamped();
2816 twistPtr->header = msg->header;
2818 twistPtr->twist.twist.linear.x = xVel;
2819 twistPtr->twist.twist.linear.y = yVel;
2820 twistPtr->twist.twist.linear.z = zVel;
2821 twistPtr->twist.twist.angular.x = rollVel;
2822 twistPtr->twist.twist.angular.y = pitchVel;
2823 twistPtr->twist.twist.angular.z = yawVel;
2824 std::vector<int> twistUpdateVec(
STATE_SIZE,
false);
2828 std::copy(twistUpdateVec.begin(), twistUpdateVec.end(), updateVector.begin());
2829 geometry_msgs::TwistWithCovarianceStampedConstPtr ptr(twistPtr);
2835 covarianceRotated = (covarianceRotated.eval() + prevCovarRotated) * dt;
2839 "\nPrevious measurement covariance rotated:\n" << prevCovarRotated <<
2840 "\nFinal twist covariance:\n" << covarianceRotated <<
"\n");
2844 topicName +
"_twist",
2845 twistPtr->header.frame_id,
2848 measurementCovariance);
2872 poseTmp.mult(targetFrameTrans, poseTmp);
2876 measurement(StateMemberX) = poseTmp.getOrigin().x();
2877 measurement(StateMemberY) = poseTmp.getOrigin().y();
2878 measurement(StateMemberZ) = poseTmp.getOrigin().z();
2881 double roll, pitch, yaw;
2883 measurement(StateMemberRoll) = roll;
2884 measurement(StateMemberPitch) = pitch;
2885 measurement(StateMemberYaw) = yaw;
2892 forceTwoD(measurement, measurementCovariance, updateVector);
2902 RF_DEBUG(
"Could not transform measurement into " << finalTargetFrame <<
". Ignoring...");
2905 RF_DEBUG(
"\n----- /RosFilter::preparePose (" << topicName <<
") ------\n");
2910 template<
typename T>
2912 const std::string &topicName,
2913 const std::string &targetFrame,
2914 std::vector<int> &updateVector,
2915 Eigen::VectorXd &measurement,
2916 Eigen::MatrixXd &measurementCovariance)
2918 RF_DEBUG(
"------ RosFilter::prepareTwist (" << topicName <<
") ------\n");
2921 tf2::Vector3 twistLin(msg->twist.twist.linear.x,
2922 msg->twist.twist.linear.y,
2923 msg->twist.twist.linear.z);
2924 tf2::Vector3 measTwistRot(msg->twist.twist.angular.x,
2925 msg->twist.twist.angular.y,
2926 msg->twist.twist.angular.z);
2932 const Eigen::VectorXd &state =
filter_.getState();
2938 std::string msgFrame = (msg->header.frame_id ==
"" ? targetFrame : msg->header.frame_id);
2958 covarianceRotated.setZero();
2967 RF_DEBUG(
"Original measurement as tf object:\nLinear: " << twistLin <<
2968 "Rotational: " << measTwistRot <<
2969 "\nOriginal update vector:\n" << updateVector <<
2970 "\nOriginal covariance matrix:\n" << covarianceRotated <<
"\n");
2985 measTwistRot = targetFrameTrans.
getBasis() * measTwistRot;
2986 twistLin = targetFrameTrans.
getBasis() * twistLin + targetFrameTrans.
getOrigin().cross(stateTwistRot);
2987 maskLin = targetFrameTrans.
getBasis() * maskLin;
2988 maskRot = targetFrameTrans.
getBasis() * maskRot;
3004 RF_DEBUG(msg->header.frame_id <<
"->" << targetFrame <<
" transform:\n" << targetFrameTrans <<
3005 "\nAfter applying transform to " << targetFrame <<
", update vector is:\n" << updateVector <<
3006 "\nAfter applying transform to " << targetFrame <<
", measurement is:\n" <<
3007 "Linear: " << twistLin <<
"Rotational: " << measTwistRot <<
"\n");
3015 rot6d.setIdentity();
3019 rot6d(rInd, 0) = rot.getRow(rInd).getX();
3020 rot6d(rInd, 1) = rot.getRow(rInd).getY();
3021 rot6d(rInd, 2) = rot.getRow(rInd).getZ();
3022 rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
3023 rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
3024 rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
3028 covarianceRotated = rot6d * covarianceRotated.eval() * rot6d.transpose();
3030 RF_DEBUG(
"Transformed covariance is \n" << covarianceRotated <<
"\n");
3033 measurement(StateMemberVx) = twistLin.getX();
3034 measurement(StateMemberVy) = twistLin.getY();
3035 measurement(StateMemberVz) = twistLin.getZ();
3036 measurement(StateMemberVroll) = measTwistRot.getX();
3037 measurement(StateMemberVpitch) = measTwistRot.getY();
3038 measurement(StateMemberVyaw) = measTwistRot.getZ();
3047 forceTwoD(measurement, measurementCovariance, updateVector);
3052 RF_DEBUG(
"Could not transform measurement into " << targetFrame <<
". Ignoring...");
3055 RF_DEBUG(
"\n----- /RosFilter::prepareTwist (" << topicName <<
") ------\n");
3057 return canTransform;
3060 template<
typename T>
3064 state->state_ = Eigen::VectorXd(filter.
getState());
3067 state->latestControl_ = Eigen::VectorXd(filter.
getControl());
3070 RF_DEBUG(
"Saved state with timestamp " << std::setprecision(20) << state->lastMeasurementTime_ <<
3074 template<
typename T>
3077 RF_DEBUG(
"\n----- RosFilter::revertTo -----\n");
3078 RF_DEBUG(
"\nRequested time was " << std::setprecision(20) << time <<
"\n")
3094 bool retVal =
false;
3102 RF_DEBUG(
"Insufficient history to revert to time " << time <<
"\n");
3104 if (lastHistoryState)
3106 RF_DEBUG(
"Will revert to oldest state at " << lastHistoryState->latestControlTime_ <<
".\n");
3108 std::setprecision(20) << time <<
". Instead reverted to state with time " <<
3109 lastHistoryState->lastMeasurementTime_ <<
". History size was " << history_size);
3114 if (lastHistoryState)
3118 filter_.setState(state->state_);
3119 filter_.setEstimateErrorCovariance(state->estimateErrorCovariance_);
3120 filter_.setLastMeasurementTime(state->lastMeasurementTime_);
3122 RF_DEBUG(
"Reverted to state with time " << std::setprecision(20) << state->lastMeasurementTime_ <<
"\n");
3125 int restored_measurements = 0;
3132 restored_measurements++;
3138 RF_DEBUG(
"Restored " << restored_measurements <<
" to measurement queue.\n");
3141 RF_DEBUG(
"\n----- /RosFilter::revertTo\n");
3146 template<
typename T>
3149 return !std::isnan(message.pose.pose.position.x) && !std::isinf(message.pose.pose.position.x) &&
3150 !std::isnan(message.pose.pose.position.y) && !std::isinf(message.pose.pose.position.y) &&
3151 !std::isnan(message.pose.pose.position.z) && !std::isinf(message.pose.pose.position.z) &&
3152 !std::isnan(message.pose.pose.orientation.x) && !std::isinf(message.pose.pose.orientation.x) &&
3153 !std::isnan(message.pose.pose.orientation.y) && !std::isinf(message.pose.pose.orientation.y) &&
3154 !std::isnan(message.pose.pose.orientation.z) && !std::isinf(message.pose.pose.orientation.z) &&
3155 !std::isnan(message.pose.pose.orientation.w) && !std::isinf(message.pose.pose.orientation.w) &&
3156 !std::isnan(message.twist.twist.linear.x) && !std::isinf(message.twist.twist.linear.x) &&
3157 !std::isnan(message.twist.twist.linear.y) && !std::isinf(message.twist.twist.linear.y) &&
3158 !std::isnan(message.twist.twist.linear.z) && !std::isinf(message.twist.twist.linear.z) &&
3159 !std::isnan(message.twist.twist.angular.x) && !std::isinf(message.twist.twist.angular.x) &&
3160 !std::isnan(message.twist.twist.angular.y) && !std::isinf(message.twist.twist.angular.y) &&
3161 !std::isnan(message.twist.twist.angular.z) && !std::isinf(message.twist.twist.angular.z);
3164 template<
typename T>
3167 RF_DEBUG(
"\n----- RosFilter::clearExpiredHistory -----" <<
3168 "\nCutoff time is " << cutOffTime <<
"\n");
3170 int poppedMeasurements = 0;
3171 int poppedStates = 0;
3176 poppedMeasurements++;
3185 RF_DEBUG(
"\nPopped " << poppedMeasurements <<
" measurements and " <<
3186 poppedStates <<
" states from their respective queues." <<
3187 "\n---- /RosFilter::clearExpiredHistory ----\n");
3190 template<
typename T>
const std::string & getMessage() const
bool useControl_
Whether or not we use a control term.
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.
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::ServiceServer toggleFilterProcessingSrv_
Service that allows another node to toggle on/off filter processing while still publishing. Uses a robot_localization ToggleFilterProcessing service.
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.
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::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)
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,...)
int dynamicDiagErrorLevel_
The max (worst) dynamic diagnostic level.
#define ROS_ERROR_STREAM_DELAYED_THROTTLE(rate, args)
void reset()
Resets the filter to its initial state.
ros::Time lastSetPoseTime_
Store the last time set pose message was received.
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)
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::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_
$brief Whether the filter should process new measurements or not.
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)
double gravitationalAcc_
What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
#define ROS_WARN_STREAM_DELAYED_THROTTLE(rate, args)
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].
void saveFilterState(FilterBase &filter)
Saves the current filter state in the queue of previous filter states.
#define ROS_WARN_STREAM_THROTTLE(rate, args)
void setRotation(const Quaternion &q)
const Eigen::VectorXd & getControl()
Returns the control vector currently being used.
double rejectionThreshold_
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
#define ROS_WARN_STREAM_ONCE(args)
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.
RosFilter(std::vector< double > args=std::vector< double >())
Constructor.
const Eigen::MatrixXd & getEstimateErrorCovariance()
Gets the estimate error covariance.
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.
std::vector< std::string > stateVariableNames_
Contains the state vector variable names in string format.
ROSCPP_DECL void spinOnce()
static bool waitForValid()
ros::NodeHandle nh_
Node handle.
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw)
Utility method for converting quaternion to RPY.
void run()
Main run method.
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...