00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "robot_localization/ros_filter.h"
00034 #include "robot_localization/filter_utilities.h"
00035 #include "robot_localization/ekf.h"
00036 #include "robot_localization/ukf.h"
00037
00038 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00039
00040 #include <algorithm>
00041 #include <map>
00042 #include <string>
00043 #include <utility>
00044 #include <vector>
00045 #include <limits>
00046
00047 namespace RobotLocalization
00048 {
00049 template<typename T>
00050 RosFilter<T>::RosFilter(std::vector<double> args) :
00051 staticDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK),
00052 tfListener_(tfBuffer_),
00053 dynamicDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK),
00054 filter_(args),
00055 frequency_(30.0),
00056 historyLength_(0),
00057 lastSetPoseTime_(0),
00058 latestControl_(),
00059 latestControlTime_(0),
00060 tfTimeout_(ros::Duration(0)),
00061 nhLocal_("~"),
00062 printDiagnostics_(true),
00063 gravitationalAcc_(9.80665),
00064 publishTransform_(true),
00065 publishAcceleration_(false),
00066 twoDMode_(false),
00067 useControl_(false),
00068 smoothLaggedData_(false)
00069 {
00070 stateVariableNames_.push_back("X");
00071 stateVariableNames_.push_back("Y");
00072 stateVariableNames_.push_back("Z");
00073 stateVariableNames_.push_back("ROLL");
00074 stateVariableNames_.push_back("PITCH");
00075 stateVariableNames_.push_back("YAW");
00076 stateVariableNames_.push_back("X_VELOCITY");
00077 stateVariableNames_.push_back("Y_VELOCITY");
00078 stateVariableNames_.push_back("Z_VELOCITY");
00079 stateVariableNames_.push_back("ROLL_VELOCITY");
00080 stateVariableNames_.push_back("PITCH_VELOCITY");
00081 stateVariableNames_.push_back("YAW_VELOCITY");
00082 stateVariableNames_.push_back("X_ACCELERATION");
00083 stateVariableNames_.push_back("Y_ACCELERATION");
00084 stateVariableNames_.push_back("Z_ACCELERATION");
00085
00086 diagnosticUpdater_.setHardwareID("none");
00087 }
00088
00089 template<typename T>
00090 RosFilter<T>::~RosFilter()
00091 {
00092 topicSubs_.clear();
00093 }
00094
00095 template<typename T>
00096 void RosFilter<T>::reset()
00097 {
00098
00099 initialMeasurements_.clear();
00100 previousMeasurements_.clear();
00101 previousMeasurementCovariances_.clear();
00102
00103
00104
00105 while (!measurementQueue_.empty() && ros::ok())
00106 {
00107 measurementQueue_.pop();
00108 }
00109
00110 filterStateHistory_.clear();
00111 measurementHistory_.clear();
00112
00113
00114
00115 lastSetPoseTime_ = ros::Time(0);
00116
00117
00118 tfBuffer_.clear();
00119
00120
00121 lastMessageTimes_.clear();
00122
00123
00124 filter_.reset();
00125
00126
00127 ros::getGlobalCallbackQueue()->clear();
00128 }
00129
00130
00131 template<typename T>
00132 void RosFilter<T>::accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg, const CallbackData &callbackData,
00133 const std::string &targetFrame)
00134 {
00135
00136
00137 if (msg->header.stamp <= lastSetPoseTime_)
00138 {
00139 return;
00140 }
00141
00142 const std::string &topicName = callbackData.topicName_;
00143
00144 RF_DEBUG("------ RosFilter::accelerationCallback (" << topicName << ") ------\n"
00145 "Twist message:\n" << *msg);
00146
00147 if (lastMessageTimes_.count(topicName) == 0)
00148 {
00149 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
00150 }
00151
00152
00153 if (msg->header.stamp >= lastMessageTimes_[topicName])
00154 {
00155 RF_DEBUG("Update vector for " << topicName << " is:\n" << topicName);
00156
00157 Eigen::VectorXd measurement(STATE_SIZE);
00158 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
00159
00160 measurement.setZero();
00161 measurementCovariance.setZero();
00162
00163
00164 std::vector<int> updateVectorCorrected = callbackData.updateVector_;
00165
00166
00167 if (prepareAcceleration(msg, topicName, targetFrame, updateVectorCorrected, measurement,
00168 measurementCovariance))
00169 {
00170
00171
00172 enqueueMeasurement(topicName,
00173 measurement,
00174 measurementCovariance,
00175 updateVectorCorrected,
00176 callbackData.rejectionThreshold_,
00177 msg->header.stamp);
00178
00179 RF_DEBUG("Enqueued new measurement for " << topicName << "_acceleration\n");
00180 }
00181 else
00182 {
00183 RF_DEBUG("Did *not* enqueue measurement for " << topicName << "_acceleration\n");
00184 }
00185
00186 lastMessageTimes_[topicName] = msg->header.stamp;
00187
00188 RF_DEBUG("Last message time for " << topicName << " is now " <<
00189 lastMessageTimes_[topicName] << "\n");
00190 }
00191 else if (resetOnTimeJump_ && ros::Time::isSimTime())
00192 {
00193 reset();
00194 }
00195 else
00196 {
00197 std::stringstream stream;
00198 stream << "The " << topicName << " message has a timestamp before that of the previous message received," <<
00199 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
00200 msg->header.stamp.toSec() << ")";
00201 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
00202 topicName + "_timestamp",
00203 stream.str(),
00204 false);
00205
00206 RF_DEBUG("Message is too old. Last message time for " << topicName <<
00207 " is " << lastMessageTimes_[topicName] << ", current message time is " <<
00208 msg->header.stamp << ".\n");
00209 }
00210
00211 RF_DEBUG("\n----- /RosFilter::accelerationCallback (" << topicName << ") ------\n");
00212 }
00213
00214 template<typename T>
00215 void RosFilter<T>::controlCallback(const geometry_msgs::Twist::ConstPtr &msg)
00216 {
00217 geometry_msgs::TwistStampedPtr twistStampedPtr = geometry_msgs::TwistStampedPtr(new geometry_msgs::TwistStamped());
00218 twistStampedPtr->twist = *msg;
00219 twistStampedPtr->header.frame_id = baseLinkFrameId_;
00220 twistStampedPtr->header.stamp = ros::Time::now();
00221 controlCallback(twistStampedPtr);
00222 }
00223
00224 template<typename T>
00225 void RosFilter<T>::controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg)
00226 {
00227 if (msg->header.frame_id == baseLinkFrameId_ || msg->header.frame_id == "")
00228 {
00229 latestControl_(ControlMemberVx) = msg->twist.linear.x;
00230 latestControl_(ControlMemberVy) = msg->twist.linear.y;
00231 latestControl_(ControlMemberVz) = msg->twist.linear.z;
00232 latestControl_(ControlMemberVroll) = msg->twist.angular.x;
00233 latestControl_(ControlMemberVpitch) = msg->twist.angular.y;
00234 latestControl_(ControlMemberVyaw) = msg->twist.angular.z;
00235 latestControlTime_ = msg->header.stamp;
00236
00237
00238 filter_.setControl(latestControl_, msg->header.stamp.toSec());
00239 }
00240 else
00241 {
00242 ROS_WARN_STREAM_THROTTLE(5.0, "Commanded velocities must be given in the robot's body frame (" <<
00243 baseLinkFrameId_ << "). Message frame was " << msg->header.frame_id);
00244 }
00245 }
00246
00247 template<typename T>
00248 void RosFilter<T>::enqueueMeasurement(const std::string &topicName,
00249 const Eigen::VectorXd &measurement,
00250 const Eigen::MatrixXd &measurementCovariance,
00251 const std::vector<int> &updateVector,
00252 const double mahalanobisThresh,
00253 const ros::Time &time)
00254 {
00255 MeasurementPtr meas = MeasurementPtr(new Measurement());
00256
00257 meas->topicName_ = topicName;
00258 meas->measurement_ = measurement;
00259 meas->covariance_ = measurementCovariance;
00260 meas->updateVector_ = updateVector;
00261 meas->time_ = time.toSec();
00262 meas->mahalanobisThresh_ = mahalanobisThresh;
00263 meas->latestControl_ = latestControl_;
00264 meas->latestControlTime_ = latestControlTime_.toSec();
00265 measurementQueue_.push(meas);
00266 }
00267
00268 template<typename T>
00269 void RosFilter<T>::forceTwoD(Eigen::VectorXd &measurement,
00270 Eigen::MatrixXd &measurementCovariance,
00271 std::vector<int> &updateVector)
00272 {
00273 measurement(StateMemberZ) = 0.0;
00274 measurement(StateMemberRoll) = 0.0;
00275 measurement(StateMemberPitch) = 0.0;
00276 measurement(StateMemberVz) = 0.0;
00277 measurement(StateMemberVroll) = 0.0;
00278 measurement(StateMemberVpitch) = 0.0;
00279 measurement(StateMemberAz) = 0.0;
00280
00281 measurementCovariance(StateMemberZ, StateMemberZ) = 1e-6;
00282 measurementCovariance(StateMemberRoll, StateMemberRoll) = 1e-6;
00283 measurementCovariance(StateMemberPitch, StateMemberPitch) = 1e-6;
00284 measurementCovariance(StateMemberVz, StateMemberVz) = 1e-6;
00285 measurementCovariance(StateMemberVroll, StateMemberVroll) = 1e-6;
00286 measurementCovariance(StateMemberVpitch, StateMemberVpitch) = 1e-6;
00287 measurementCovariance(StateMemberAz, StateMemberAz) = 1e-6;
00288
00289 updateVector[StateMemberZ] = 1;
00290 updateVector[StateMemberRoll] = 1;
00291 updateVector[StateMemberPitch] = 1;
00292 updateVector[StateMemberVz] = 1;
00293 updateVector[StateMemberVroll] = 1;
00294 updateVector[StateMemberVpitch] = 1;
00295 updateVector[StateMemberAz] = 1;
00296 }
00297
00298 template<typename T>
00299 bool RosFilter<T>::getFilteredOdometryMessage(nav_msgs::Odometry &message)
00300 {
00301
00302 if (filter_.getInitializedStatus())
00303 {
00304
00305 const Eigen::VectorXd &state = filter_.getState();
00306 const Eigen::MatrixXd &estimateErrorCovariance = filter_.getEstimateErrorCovariance();
00307
00308
00309
00310 tf2::Quaternion quat;
00311 quat.setRPY(state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw));
00312
00313
00314 message.pose.pose.position.x = state(StateMemberX);
00315 message.pose.pose.position.y = state(StateMemberY);
00316 message.pose.pose.position.z = state(StateMemberZ);
00317 message.pose.pose.orientation.x = quat.x();
00318 message.pose.pose.orientation.y = quat.y();
00319 message.pose.pose.orientation.z = quat.z();
00320 message.pose.pose.orientation.w = quat.w();
00321 message.twist.twist.linear.x = state(StateMemberVx);
00322 message.twist.twist.linear.y = state(StateMemberVy);
00323 message.twist.twist.linear.z = state(StateMemberVz);
00324 message.twist.twist.angular.x = state(StateMemberVroll);
00325 message.twist.twist.angular.y = state(StateMemberVpitch);
00326 message.twist.twist.angular.z = state(StateMemberVyaw);
00327
00328
00329 for (size_t i = 0; i < POSE_SIZE; i++)
00330 {
00331 for (size_t j = 0; j < POSE_SIZE; j++)
00332 {
00333 message.pose.covariance[POSE_SIZE * i + j] = estimateErrorCovariance(i, j);
00334 }
00335 }
00336
00337
00338
00339
00340 for (size_t i = 0; i < TWIST_SIZE; i++)
00341 {
00342 for (size_t j = 0; j < TWIST_SIZE; j++)
00343 {
00344 message.twist.covariance[TWIST_SIZE * i + j] =
00345 estimateErrorCovariance(i + POSITION_V_OFFSET, j + POSITION_V_OFFSET);
00346 }
00347 }
00348
00349 message.header.stamp = ros::Time(filter_.getLastMeasurementTime());
00350 message.header.frame_id = worldFrameId_;
00351 message.child_frame_id = baseLinkFrameId_;
00352 }
00353
00354 return filter_.getInitializedStatus();
00355 }
00356
00357 template<typename T>
00358 bool RosFilter<T>::getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message)
00359 {
00360
00361 if (filter_.getInitializedStatus())
00362 {
00363
00364 const Eigen::VectorXd &state = filter_.getState();
00365 const Eigen::MatrixXd &estimateErrorCovariance = filter_.getEstimateErrorCovariance();
00366
00368 message.accel.accel.linear.x = state(StateMemberAx);
00369 message.accel.accel.linear.y = state(StateMemberAy);
00370 message.accel.accel.linear.z = state(StateMemberAz);
00371
00372
00373
00374 for (size_t i = 0; i < ACCELERATION_SIZE; i++)
00375 {
00376 for (size_t j = 0; j < ACCELERATION_SIZE; j++)
00377 {
00378
00379 message.accel.covariance[POSE_SIZE * i + j] =
00380 estimateErrorCovariance(i + POSITION_A_OFFSET, j + POSITION_A_OFFSET);
00381 }
00382 }
00383
00384
00385 message.header.stamp = ros::Time(filter_.getLastMeasurementTime());
00386 message.header.frame_id = baseLinkFrameId_;
00387 }
00388
00389 return filter_.getInitializedStatus();
00390 }
00391
00392 template<typename T>
00393 void RosFilter<T>::imuCallback(const sensor_msgs::Imu::ConstPtr &msg,
00394 const std::string &topicName,
00395 const CallbackData &poseCallbackData,
00396 const CallbackData &twistCallbackData,
00397 const CallbackData &accelCallbackData)
00398 {
00399 RF_DEBUG("------ RosFilter::imuCallback (" << topicName << ") ------\n" << "IMU message:\n" << *msg);
00400
00401
00402
00403 if (msg->header.stamp <= lastSetPoseTime_)
00404 {
00405 std::stringstream stream;
00406 stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " <<
00407 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
00408 msg->header.stamp.toSec() << ")";
00409 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
00410 topicName + "_timestamp",
00411 stream.str(),
00412 false);
00413 RF_DEBUG("Received message that preceded the most recent pose reset. Ignoring...");
00414
00415 return;
00416 }
00417
00418
00419
00420 if (poseCallbackData.updateSum_ > 0)
00421 {
00422
00423
00424
00425
00426
00427 if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9)
00428 {
00429 RF_DEBUG("Received IMU message with -1 as its first covariance value for orientation. "
00430 "Ignoring orientation...");
00431 }
00432 else
00433 {
00434
00435 geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
00436 posPtr->header = msg->header;
00437 posPtr->pose.pose.orientation = msg->orientation;
00438
00439
00440 for (size_t i = 0; i < ORIENTATION_SIZE; i++)
00441 {
00442 for (size_t j = 0; j < ORIENTATION_SIZE; j++)
00443 {
00444 posPtr->pose.covariance[POSE_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] =
00445 msg->orientation_covariance[ORIENTATION_SIZE * i + j];
00446 }
00447 }
00448
00449
00450
00451
00452
00453 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
00454 poseCallback(pptr, poseCallbackData, baseLinkFrameId_, true);
00455 }
00456 }
00457
00458 if (twistCallbackData.updateSum_ > 0)
00459 {
00460
00461 if (::fabs(msg->angular_velocity_covariance[0] + 1) < 1e-9)
00462 {
00463 RF_DEBUG("Received IMU message with -1 as its first covariance value for angular "
00464 "velocity. Ignoring angular velocity...");
00465 }
00466 else
00467 {
00468
00469 geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
00470 twistPtr->header = msg->header;
00471 twistPtr->twist.twist.angular = msg->angular_velocity;
00472
00473
00474 for (size_t i = 0; i < ORIENTATION_SIZE; i++)
00475 {
00476 for (size_t j = 0; j < ORIENTATION_SIZE; j++)
00477 {
00478 twistPtr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] =
00479 msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j];
00480 }
00481 }
00482
00483 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
00484 twistCallback(tptr, twistCallbackData, baseLinkFrameId_);
00485 }
00486 }
00487
00488 if (accelCallbackData.updateSum_ > 0)
00489 {
00490
00491 if (::fabs(msg->linear_acceleration_covariance[0] + 1) < 1e-9)
00492 {
00493 RF_DEBUG("Received IMU message with -1 as its first covariance value for linear "
00494 "acceleration. Ignoring linear acceleration...");
00495 }
00496 else
00497 {
00498
00499 accelerationCallback(msg, accelCallbackData, baseLinkFrameId_);
00500 }
00501 }
00502
00503 RF_DEBUG("\n----- /RosFilter::imuCallback (" << topicName << ") ------\n");
00504 }
00505
00506 template<typename T>
00507 void RosFilter<T>::integrateMeasurements(const ros::Time ¤tTime)
00508 {
00509 const double currentTimeSec = currentTime.toSec();
00510
00511 RF_DEBUG("------ RosFilter::integrateMeasurements ------\n\n"
00512 "Integration time is " << std::setprecision(20) << currentTimeSec << "\n"
00513 << measurementQueue_.size() << " measurements in queue.\n");
00514
00515
00516 if (!measurementQueue_.empty())
00517 {
00518
00519
00520
00521
00522 const MeasurementPtr& firstMeasurement = measurementQueue_.top();
00523 int restoredMeasurementCount = 0;
00524 if (smoothLaggedData_ && firstMeasurement->time_ < filter_.getLastMeasurementTime())
00525 {
00526 RF_DEBUG("Received a measurement that was " << filter_.getLastMeasurementTime() - firstMeasurement->time_ <<
00527 " seconds in the past. Reverting filter state and measurement queue...");
00528
00529 int originalCount = static_cast<int>(measurementQueue_.size());
00530 const double firstMeasurementTime = firstMeasurement->time_;
00531 const std::string firstMeasurementTopic = firstMeasurement->topicName_;
00532 if (!revertTo(firstMeasurement->time_ - 1e-9))
00533 {
00534 RF_DEBUG("ERROR: history interval is too small to revert to time " << firstMeasurementTime << "\n");
00535 ROS_WARN_STREAM_DELAYED_THROTTLE(historyLength_, "Received old measurement for topic " <<
00536 firstMeasurementTopic << ", but history interval is insufficiently sized to revert state and "
00537 "measurement queue.");
00538 restoredMeasurementCount = 0;
00539 }
00540
00541 restoredMeasurementCount = static_cast<int>(measurementQueue_.size()) - originalCount;
00542 }
00543
00544 while (!measurementQueue_.empty() && ros::ok())
00545 {
00546 MeasurementPtr measurement = measurementQueue_.top();
00547
00548
00549
00550 if (measurement->time_ > currentTime.toSec())
00551 {
00552 break;
00553 }
00554
00555 measurementQueue_.pop();
00556
00557
00558
00559
00560
00561
00562
00563 if (useControl_ && restoredMeasurementCount > 0)
00564 {
00565 filter_.setControl(measurement->latestControl_, measurement->latestControlTime_);
00566 restoredMeasurementCount--;
00567 }
00568
00569
00570 filter_.processMeasurement(*(measurement.get()));
00571
00572
00573 if (smoothLaggedData_)
00574 {
00575
00576 measurementHistory_.push_back(measurement);
00577
00578
00579 if (measurementQueue_.empty() ||
00580 ::fabs(measurementQueue_.top()->time_ - filter_.getLastMeasurementTime()) > 1e-9)
00581 {
00582 saveFilterState(filter_);
00583 }
00584 }
00585 }
00586
00587 filter_.setLastUpdateTime(currentTimeSec);
00588 }
00589 else if (filter_.getInitializedStatus())
00590 {
00591
00592
00593
00594 double lastUpdateDelta = currentTimeSec - filter_.getLastUpdateTime();
00595
00596
00597 if (lastUpdateDelta >= filter_.getSensorTimeout())
00598 {
00599 RF_DEBUG("Sensor timeout! Last update time was " << filter_.getLastUpdateTime() <<
00600 ", current time is " << currentTimeSec <<
00601 ", delta is " << lastUpdateDelta << "\n");
00602
00603 filter_.validateDelta(lastUpdateDelta);
00604 filter_.predict(currentTimeSec, lastUpdateDelta);
00605
00606
00607 filter_.setLastMeasurementTime(filter_.getLastMeasurementTime() + lastUpdateDelta);
00608 filter_.setLastUpdateTime(filter_.getLastUpdateTime() + lastUpdateDelta);
00609 }
00610 }
00611 else
00612 {
00613 RF_DEBUG("Filter not yet initialized.\n");
00614 }
00615
00616 RF_DEBUG("\n----- /RosFilter::integrateMeasurements ------\n");
00617 }
00618
00619 template<typename T>
00620 void RosFilter<T>::loadParams()
00621 {
00622
00623
00624
00625
00626 std::map<StateMembers, int> absPoseVarCounts;
00627 absPoseVarCounts[StateMemberX] = 0;
00628 absPoseVarCounts[StateMemberY] = 0;
00629 absPoseVarCounts[StateMemberZ] = 0;
00630 absPoseVarCounts[StateMemberRoll] = 0;
00631 absPoseVarCounts[StateMemberPitch] = 0;
00632 absPoseVarCounts[StateMemberYaw] = 0;
00633
00634
00635 std::map<StateMembers, int> twistVarCounts;
00636 twistVarCounts[StateMemberVx] = 0;
00637 twistVarCounts[StateMemberVy] = 0;
00638 twistVarCounts[StateMemberVz] = 0;
00639 twistVarCounts[StateMemberVroll] = 0;
00640 twistVarCounts[StateMemberVpitch] = 0;
00641 twistVarCounts[StateMemberVyaw] = 0;
00642
00643
00644 nhLocal_.param("print_diagnostics", printDiagnostics_, true);
00645
00646
00647 nhLocal_.param("gravitational_acceleration", gravitationalAcc_, 9.80665);
00648
00649
00650 bool debug;
00651 nhLocal_.param("debug", debug, false);
00652
00653 if (debug)
00654 {
00655 std::string debugOutFile;
00656
00657 try
00658 {
00659 nhLocal_.param("debug_out_file", debugOutFile, std::string("robot_localization_debug.txt"));
00660 debugStream_.open(debugOutFile.c_str());
00661
00662
00663 if (debugStream_.is_open())
00664 {
00665 filter_.setDebug(debug, &debugStream_);
00666 }
00667 else
00668 {
00669 ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file " << debugOutFile);
00670 }
00671 }
00672 catch(const std::exception &e)
00673 {
00674 ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file" << debugOutFile
00675 << ". Error was " << e.what() << "\n");
00676 }
00677 }
00678
00679
00680
00681 nhLocal_.param("map_frame", mapFrameId_, std::string("map"));
00682 nhLocal_.param("odom_frame", odomFrameId_, std::string("odom"));
00683 nhLocal_.param("base_link_frame", baseLinkFrameId_, std::string("base_link"));
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710 nhLocal_.param("world_frame", worldFrameId_, odomFrameId_);
00711
00712 ROS_FATAL_COND(mapFrameId_ == odomFrameId_ ||
00713 odomFrameId_ == baseLinkFrameId_ ||
00714 mapFrameId_ == baseLinkFrameId_,
00715 "Invalid frame configuration! The values for map_frame, odom_frame, "
00716 "and base_link_frame must be unique");
00717
00718
00719 std::string tfPrefix = "";
00720 std::string tfPrefixPath = "";
00721 if (nhLocal_.searchParam("tf_prefix", tfPrefixPath))
00722 {
00723 nhLocal_.getParam(tfPrefixPath, tfPrefix);
00724 }
00725
00726
00727 FilterUtilities::appendPrefix(tfPrefix, mapFrameId_);
00728 FilterUtilities::appendPrefix(tfPrefix, odomFrameId_);
00729 FilterUtilities::appendPrefix(tfPrefix, baseLinkFrameId_);
00730 FilterUtilities::appendPrefix(tfPrefix, worldFrameId_);
00731
00732
00733 nhLocal_.param("publish_tf", publishTransform_, true);
00734
00735
00736 nhLocal_.param("publish_acceleration", publishAcceleration_, false);
00737
00738
00739 double offsetTmp;
00740 nhLocal_.param("transform_time_offset", offsetTmp, 0.0);
00741 tfTimeOffset_.fromSec(offsetTmp);
00742
00743
00744 double timeoutTmp;
00745 nhLocal_.param("transform_timeout", timeoutTmp, 0.0);
00746 tfTimeout_.fromSec(timeoutTmp);
00747
00748
00749 double sensorTimeout;
00750 nhLocal_.param("frequency", frequency_, 30.0);
00751 nhLocal_.param("sensor_timeout", sensorTimeout, 1.0 / frequency_);
00752 filter_.setSensorTimeout(sensorTimeout);
00753
00754
00755 nhLocal_.param("two_d_mode", twoDMode_, false);
00756
00757
00758 nhLocal_.param("smooth_lagged_data", smoothLaggedData_, false);
00759 nhLocal_.param("history_length", historyLength_, 0.0);
00760
00761
00762 nhLocal_.param("reset_on_time_jump", resetOnTimeJump_, false);
00763
00764 if (!smoothLaggedData_ && ::fabs(historyLength_) > 1e-9)
00765 {
00766 ROS_WARN_STREAM("Filter history interval of " << historyLength_ <<
00767 " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed.");
00768 }
00769
00770 if (smoothLaggedData_ && historyLength_ < -1e9)
00771 {
00772 ROS_WARN_STREAM("Negative history interval of " << historyLength_ <<
00773 " specified. Absolute value will be assumed.");
00774 }
00775
00776 historyLength_ = ::fabs(historyLength_);
00777
00778
00779 bool stampedControl = false;
00780 double controlTimeout = sensorTimeout;
00781 std::vector<int> controlUpdateVector(TWIST_SIZE, 0);
00782 std::vector<double> accelerationLimits(TWIST_SIZE, 1.0);
00783 std::vector<double> accelerationGains(TWIST_SIZE, 1.0);
00784 std::vector<double> decelerationLimits(TWIST_SIZE, 1.0);
00785 std::vector<double> decelerationGains(TWIST_SIZE, 1.0);
00786
00787 nhLocal_.param("use_control", useControl_, false);
00788 nhLocal_.param("stamped_control", stampedControl, false);
00789 nhLocal_.param("control_timeout", controlTimeout, sensorTimeout);
00790
00791 if (useControl_)
00792 {
00793 if (nhLocal_.getParam("control_config", controlUpdateVector))
00794 {
00795 if (controlUpdateVector.size() != TWIST_SIZE)
00796 {
00797 ROS_ERROR_STREAM("Control configuration must be of size " << TWIST_SIZE << ". Provided config was of "
00798 "size " << controlUpdateVector.size() << ". No control term will be used.");
00799 useControl_ = false;
00800 }
00801 }
00802 else
00803 {
00804 ROS_ERROR_STREAM("use_control is set to true, but control_config is missing. No control term will be used.");
00805 useControl_ = false;
00806 }
00807
00808 if (nhLocal_.getParam("acceleration_limits", accelerationLimits))
00809 {
00810 if (accelerationLimits.size() != TWIST_SIZE)
00811 {
00812 ROS_ERROR_STREAM("Acceleration configuration must be of size " << TWIST_SIZE << ". Provided config was of "
00813 "size " << accelerationLimits.size() << ". No control term will be used.");
00814 useControl_ = false;
00815 }
00816 }
00817 else
00818 {
00819 ROS_WARN_STREAM("use_control is set to true, but acceleration_limits is missing. Will use default values.");
00820 }
00821
00822 if (nhLocal_.getParam("acceleration_gains", accelerationGains))
00823 {
00824 const int size = accelerationGains.size();
00825 if (size != TWIST_SIZE)
00826 {
00827 ROS_ERROR_STREAM("Acceleration gain configuration must be of size " << TWIST_SIZE <<
00828 ". Provided config was of size " << size << ". All gains will be assumed to be 1.");
00829 std::fill_n(accelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0);
00830 accelerationGains.resize(TWIST_SIZE, 1.0);
00831 }
00832 }
00833
00834 if (nhLocal_.getParam("deceleration_limits", decelerationLimits))
00835 {
00836 if (decelerationLimits.size() != TWIST_SIZE)
00837 {
00838 ROS_ERROR_STREAM("Deceleration configuration must be of size " << TWIST_SIZE <<
00839 ". Provided config was of size " << decelerationLimits.size() << ". No control term will be used.");
00840 useControl_ = false;
00841 }
00842 }
00843 else
00844 {
00845 ROS_INFO_STREAM("use_control is set to true, but no deceleration_limits specified. Will use acceleration "
00846 "limits.");
00847 decelerationLimits = accelerationLimits;
00848 }
00849
00850 if (nhLocal_.getParam("deceleration_gains", decelerationGains))
00851 {
00852 const int size = decelerationGains.size();
00853 if (size != TWIST_SIZE)
00854 {
00855 ROS_ERROR_STREAM("Deceleration gain configuration must be of size " << TWIST_SIZE <<
00856 ". Provided config was of size " << size << ". All gains will be assumed to be 1.");
00857 std::fill_n(decelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0);
00858 decelerationGains.resize(TWIST_SIZE, 1.0);
00859 }
00860 }
00861 else
00862 {
00863 ROS_INFO_STREAM("use_control is set to true, but no deceleration_gains specified. Will use acceleration "
00864 "gains.");
00865 decelerationGains = accelerationGains;
00866 }
00867 }
00868
00869 bool dynamicProcessNoiseCovariance = false;
00870 nhLocal_.param("dynamic_process_noise_covariance", dynamicProcessNoiseCovariance, false);
00871 filter_.setUseDynamicProcessNoiseCovariance(dynamicProcessNoiseCovariance);
00872
00873 std::vector<double> initialState(STATE_SIZE, 0.0);
00874 if (nhLocal_.getParam("initial_state", initialState))
00875 {
00876 if (initialState.size() != STATE_SIZE)
00877 {
00878 ROS_ERROR_STREAM("Initial state must be of size " << STATE_SIZE << ". Provided config was of size " <<
00879 initialState.size() << ". The initial state will be ignored.");
00880 }
00881 else
00882 {
00883 Eigen::Map<Eigen::VectorXd> eigenState(initialState.data(), initialState.size());
00884 filter_.setState(eigenState);
00885 }
00886 }
00887
00888
00889 RF_DEBUG("tf_prefix is " << tfPrefix <<
00890 "\nmap_frame is " << mapFrameId_ <<
00891 "\nodom_frame is " << odomFrameId_ <<
00892 "\nbase_link_frame is " << baseLinkFrameId_ <<
00893 "\nworld_frame is " << worldFrameId_ <<
00894 "\ntransform_time_offset is " << tfTimeOffset_.toSec() <<
00895 "\ntransform_timeout is " << tfTimeout_.toSec() <<
00896 "\nfrequency is " << frequency_ <<
00897 "\nsensor_timeout is " << filter_.getSensorTimeout() <<
00898 "\ntwo_d_mode is " << (twoDMode_ ? "true" : "false") <<
00899 "\nsmooth_lagged_data is " << (smoothLaggedData_ ? "true" : "false") <<
00900 "\nhistory_length is " << historyLength_ <<
00901 "\nuse_control is " << (useControl_ ? "true" : "false") <<
00902 "\nstamped_control is " << (stampedControl ? "true" : "false") <<
00903 "\ncontrol_config is " << controlUpdateVector <<
00904 "\ncontrol_timeout is " << controlTimeout <<
00905 "\nacceleration_limits are " << accelerationLimits <<
00906 "\nacceleration_gains are " << accelerationGains <<
00907 "\ndeceleration_limits are " << decelerationLimits <<
00908 "\ndeceleration_gains are " << decelerationGains <<
00909 "\ninitial state is " << filter_.getState() <<
00910 "\ndynamic_process_noise_covariance is " << (dynamicProcessNoiseCovariance ? "true" : "false") <<
00911 "\nprint_diagnostics is " << (printDiagnostics_ ? "true" : "false") << "\n");
00912
00913
00914 setPoseSub_ = nh_.subscribe("set_pose",
00915 1,
00916 &RosFilter<T>::setPoseCallback,
00917 this, ros::TransportHints().tcpNoDelay(false));
00918
00919
00920 setPoseSrv_ = nh_.advertiseService("set_pose", &RosFilter<T>::setPoseSrvCallback, this);
00921
00922
00923 filter_.setLastMeasurementTime(ros::Time::now().toSec());
00924 filter_.setLastUpdateTime(ros::Time::now().toSec());
00925
00926
00927
00928 size_t topicInd = 0;
00929 bool moreParams = false;
00930 do
00931 {
00932
00933
00934
00935 std::stringstream ss;
00936 ss << "odom" << topicInd++;
00937 std::string odomTopicName = ss.str();
00938 moreParams = nhLocal_.hasParam(odomTopicName);
00939
00940 if (moreParams)
00941 {
00942
00943 bool differential;
00944 nhLocal_.param(odomTopicName + std::string("_differential"), differential, false);
00945
00946
00947 bool relative;
00948 nhLocal_.param(odomTopicName + std::string("_relative"), relative, false);
00949
00950 if (relative && differential)
00951 {
00952 ROS_WARN_STREAM("Both " << odomTopicName << "_differential" << " and " << odomTopicName <<
00953 "_relative were set to true. Using differential mode.");
00954
00955 relative = false;
00956 }
00957
00958 std::string odomTopic;
00959 nhLocal_.getParam(odomTopicName, odomTopic);
00960
00961
00962 double poseMahalanobisThresh;
00963 nhLocal_.param(odomTopicName + std::string("_pose_rejection_threshold"),
00964 poseMahalanobisThresh,
00965 std::numeric_limits<double>::max());
00966
00967
00968 double twistMahalanobisThresh;
00969 nhLocal_.param(odomTopicName + std::string("_twist_rejection_threshold"),
00970 twistMahalanobisThresh,
00971 std::numeric_limits<double>::max());
00972
00973
00974
00975
00976 std::vector<int> updateVec = loadUpdateConfig(odomTopicName);
00977 std::vector<int> poseUpdateVec = updateVec;
00978 std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0);
00979 std::vector<int> twistUpdateVec = updateVec;
00980 std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);
00981
00982 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
00983 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
00984 int odomQueueSize = 1;
00985 nhLocal_.param(odomTopicName + "_queue_size", odomQueueSize, 1);
00986
00987 const CallbackData poseCallbackData(odomTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential,
00988 relative, poseMahalanobisThresh);
00989 const CallbackData twistCallbackData(odomTopicName + "_twist", twistUpdateVec, twistUpdateSum, false, false,
00990 twistMahalanobisThresh);
00991
00992 bool nodelayOdom = false;
00993 nhLocal_.param(odomTopicName + "_nodelay", nodelayOdom, false);
00994
00995
00996 if (poseUpdateSum + twistUpdateSum > 0)
00997 {
00998 topicSubs_.push_back(
00999 nh_.subscribe<nav_msgs::Odometry>(odomTopic, odomQueueSize,
01000 boost::bind(&RosFilter::odometryCallback, this, _1, odomTopicName, poseCallbackData, twistCallbackData),
01001 ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayOdom)));
01002 }
01003 else
01004 {
01005 std::stringstream stream;
01006 stream << odomTopic << " is listed as an input topic, but all update variables are false";
01007
01008 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
01009 odomTopic + "_configuration",
01010 stream.str(),
01011 true);
01012 }
01013
01014 if (poseUpdateSum > 0)
01015 {
01016 if (differential)
01017 {
01018 twistVarCounts[StateMemberVx] += poseUpdateVec[StateMemberX];
01019 twistVarCounts[StateMemberVy] += poseUpdateVec[StateMemberY];
01020 twistVarCounts[StateMemberVz] += poseUpdateVec[StateMemberZ];
01021 twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll];
01022 twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch];
01023 twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw];
01024 }
01025 else
01026 {
01027 absPoseVarCounts[StateMemberX] += poseUpdateVec[StateMemberX];
01028 absPoseVarCounts[StateMemberY] += poseUpdateVec[StateMemberY];
01029 absPoseVarCounts[StateMemberZ] += poseUpdateVec[StateMemberZ];
01030 absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll];
01031 absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch];
01032 absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw];
01033 }
01034 }
01035
01036 if (twistUpdateSum > 0)
01037 {
01038 twistVarCounts[StateMemberVx] += twistUpdateVec[StateMemberVx];
01039 twistVarCounts[StateMemberVy] += twistUpdateVec[StateMemberVx];
01040 twistVarCounts[StateMemberVz] += twistUpdateVec[StateMemberVz];
01041 twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll];
01042 twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch];
01043 twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw];
01044 }
01045
01046 RF_DEBUG("Subscribed to " << odomTopic << " (" << odomTopicName << ")\n\t" <<
01047 odomTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" <<
01048 odomTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "\n\t" <<
01049 odomTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "\n\t" <<
01050 odomTopicName << "_queue_size is " << odomQueueSize << "\n\t" <<
01051 odomTopicName << " pose update vector is " << poseUpdateVec << "\t"<<
01052 odomTopicName << " twist update vector is " << twistUpdateVec);
01053 }
01054 }
01055 while (moreParams);
01056
01057
01058 topicInd = 0;
01059 moreParams = false;
01060 do
01061 {
01062 std::stringstream ss;
01063 ss << "pose" << topicInd++;
01064 std::string poseTopicName = ss.str();
01065 moreParams = nhLocal_.hasParam(poseTopicName);
01066
01067 if (moreParams)
01068 {
01069 bool differential;
01070 nhLocal_.param(poseTopicName + std::string("_differential"), differential, false);
01071
01072
01073 bool relative;
01074 nhLocal_.param(poseTopicName + std::string("_relative"), relative, false);
01075
01076 if (relative && differential)
01077 {
01078 ROS_WARN_STREAM("Both " << poseTopicName << "_differential" << " and " << poseTopicName <<
01079 "_relative were set to true. Using differential mode.");
01080
01081 relative = false;
01082 }
01083
01084 std::string poseTopic;
01085 nhLocal_.getParam(poseTopicName, poseTopic);
01086
01087
01088 double poseMahalanobisThresh;
01089 nhLocal_.param(poseTopicName + std::string("_rejection_threshold"),
01090 poseMahalanobisThresh,
01091 std::numeric_limits<double>::max());
01092
01093 int poseQueueSize = 1;
01094 nhLocal_.param(poseTopicName + "_queue_size", poseQueueSize, 1);
01095
01096 bool nodelayPose = false;
01097 nhLocal_.param(poseTopicName + "_nodelay", nodelayPose, false);
01098
01099
01100 std::vector<int> poseUpdateVec = loadUpdateConfig(poseTopicName);
01101 std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET,
01102 poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE,
01103 0);
01104 std::fill(poseUpdateVec.begin() + POSITION_A_OFFSET,
01105 poseUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE,
01106 0);
01107
01108 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
01109
01110 if (poseUpdateSum > 0)
01111 {
01112 const CallbackData callbackData(poseTopicName, poseUpdateVec, poseUpdateSum, differential, relative,
01113 poseMahalanobisThresh);
01114
01115 topicSubs_.push_back(
01116 nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>(poseTopic, poseQueueSize,
01117 boost::bind(&RosFilter::poseCallback, this, _1, callbackData, worldFrameId_, false),
01118 ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayPose)));
01119
01120 if (differential)
01121 {
01122 twistVarCounts[StateMemberVx] += poseUpdateVec[StateMemberX];
01123 twistVarCounts[StateMemberVy] += poseUpdateVec[StateMemberY];
01124 twistVarCounts[StateMemberVz] += poseUpdateVec[StateMemberZ];
01125 twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll];
01126 twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch];
01127 twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw];
01128 }
01129 else
01130 {
01131 absPoseVarCounts[StateMemberX] += poseUpdateVec[StateMemberX];
01132 absPoseVarCounts[StateMemberY] += poseUpdateVec[StateMemberY];
01133 absPoseVarCounts[StateMemberZ] += poseUpdateVec[StateMemberZ];
01134 absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll];
01135 absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch];
01136 absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw];
01137 }
01138 }
01139 else
01140 {
01141 ROS_WARN_STREAM("Warning: " << poseTopic << " is listed as an input topic, "
01142 "but all pose update variables are false");
01143 }
01144
01145 RF_DEBUG("Subscribed to " << poseTopic << " (" << poseTopicName << ")\n\t" <<
01146 poseTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" <<
01147 poseTopicName << "_rejection_threshold is " << poseMahalanobisThresh << "\n\t" <<
01148 poseTopicName << "_queue_size is " << poseQueueSize << "\n\t" <<
01149 poseTopicName << " update vector is " << poseUpdateVec);
01150 }
01151 }
01152 while (moreParams);
01153
01154
01155 topicInd = 0;
01156 moreParams = false;
01157 do
01158 {
01159 std::stringstream ss;
01160 ss << "twist" << topicInd++;
01161 std::string twistTopicName = ss.str();
01162 moreParams = nhLocal_.hasParam(twistTopicName);
01163
01164 if (moreParams)
01165 {
01166 std::string twistTopic;
01167 nhLocal_.getParam(twistTopicName, twistTopic);
01168
01169
01170 double twistMahalanobisThresh;
01171 nhLocal_.param(twistTopicName + std::string("_rejection_threshold"),
01172 twistMahalanobisThresh,
01173 std::numeric_limits<double>::max());
01174
01175 int twistQueueSize = 1;
01176 nhLocal_.param(twistTopicName + "_queue_size", twistQueueSize, 1);
01177
01178 bool nodelayTwist = false;
01179 nhLocal_.param(twistTopicName + "_nodelay", nodelayTwist, false);
01180
01181
01182 std::vector<int> twistUpdateVec = loadUpdateConfig(twistTopicName);
01183 std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);
01184
01185 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
01186
01187 if (twistUpdateSum > 0)
01188 {
01189 const CallbackData callbackData(twistTopicName, twistUpdateVec, twistUpdateSum, false, false,
01190 twistMahalanobisThresh);
01191
01192 topicSubs_.push_back(
01193 nh_.subscribe<geometry_msgs::TwistWithCovarianceStamped>(twistTopic, twistQueueSize,
01194 boost::bind(&RosFilter<T>::twistCallback, this, _1, callbackData, baseLinkFrameId_),
01195 ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayTwist)));
01196
01197 twistVarCounts[StateMemberVx] += twistUpdateVec[StateMemberVx];
01198 twistVarCounts[StateMemberVy] += twistUpdateVec[StateMemberVy];
01199 twistVarCounts[StateMemberVz] += twistUpdateVec[StateMemberVz];
01200 twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll];
01201 twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch];
01202 twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw];
01203 }
01204 else
01205 {
01206 ROS_WARN_STREAM("Warning: " << twistTopic << " is listed as an input topic, "
01207 "but all twist update variables are false");
01208 }
01209
01210 RF_DEBUG("Subscribed to " << twistTopic << " (" << twistTopicName << ")\n\t" <<
01211 twistTopicName << "_rejection_threshold is " << twistMahalanobisThresh << "\n\t" <<
01212 twistTopicName << "_queue_size is " << twistQueueSize << "\n\t" <<
01213 twistTopicName << " update vector is " << twistUpdateVec);
01214 }
01215 }
01216 while (moreParams);
01217
01218
01219 topicInd = 0;
01220 moreParams = false;
01221 do
01222 {
01223 std::stringstream ss;
01224 ss << "imu" << topicInd++;
01225 std::string imuTopicName = ss.str();
01226 moreParams = nhLocal_.hasParam(imuTopicName);
01227
01228 if (moreParams)
01229 {
01230 bool differential;
01231 nhLocal_.param(imuTopicName + std::string("_differential"), differential, false);
01232
01233
01234 bool relative;
01235 nhLocal_.param(imuTopicName + std::string("_relative"), relative, false);
01236
01237 if (relative && differential)
01238 {
01239 ROS_WARN_STREAM("Both " << imuTopicName << "_differential" << " and " << imuTopicName <<
01240 "_relative were set to true. Using differential mode.");
01241
01242 relative = false;
01243 }
01244
01245 std::string imuTopic;
01246 nhLocal_.getParam(imuTopicName, imuTopic);
01247
01248
01249 double poseMahalanobisThresh;
01250 nhLocal_.param(imuTopicName + std::string("_pose_rejection_threshold"),
01251 poseMahalanobisThresh,
01252 std::numeric_limits<double>::max());
01253
01254
01255 double twistMahalanobisThresh;
01256 std::string imuTwistRejectionName =
01257 imuTopicName + std::string("_twist_rejection_threshold");
01258 nhLocal_.param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits<double>::max());
01259
01260
01261 double accelMahalanobisThresh;
01262 nhLocal_.param(imuTopicName + std::string("_linear_acceleration_rejection_threshold"),
01263 accelMahalanobisThresh,
01264 std::numeric_limits<double>::max());
01265
01266 bool removeGravAcc = false;
01267 nhLocal_.param(imuTopicName + "_remove_gravitational_acceleration", removeGravAcc, false);
01268 removeGravitationalAcc_[imuTopicName + "_acceleration"] = removeGravAcc;
01269
01270
01271
01272 std::vector<int> updateVec = loadUpdateConfig(imuTopicName);
01273
01274 std::vector<int> poseUpdateVec = updateVec;
01275 std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET,
01276 poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE,
01277 0);
01278 std::fill(poseUpdateVec.begin() + POSITION_A_OFFSET,
01279 poseUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE,
01280 0);
01281
01282 std::vector<int> twistUpdateVec = updateVec;
01283 std::fill(twistUpdateVec.begin() + POSITION_OFFSET,
01284 twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE,
01285 0);
01286 std::fill(twistUpdateVec.begin() + POSITION_A_OFFSET,
01287 twistUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE,
01288 0);
01289
01290 std::vector<int> accelUpdateVec = updateVec;
01291 std::fill(accelUpdateVec.begin() + POSITION_OFFSET,
01292 accelUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE,
01293 0);
01294 std::fill(accelUpdateVec.begin() + POSITION_V_OFFSET,
01295 accelUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE,
01296 0);
01297
01298 int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
01299 int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
01300 int accelUpdateSum = std::accumulate(accelUpdateVec.begin(), accelUpdateVec.end(), 0);
01301
01302
01303 if (static_cast<bool>(controlUpdateVector[ControlMemberVx]) && static_cast<bool>(accelUpdateVec[StateMemberAx]))
01304 {
01305 ROS_WARN_STREAM("X acceleration is being measured from IMU; X velocity control input is disabled");
01306 controlUpdateVector[ControlMemberVx] = 0;
01307 }
01308 if (static_cast<bool>(controlUpdateVector[ControlMemberVy]) && static_cast<bool>(accelUpdateVec[StateMemberAy]))
01309 {
01310 ROS_WARN_STREAM("Y acceleration is being measured from IMU; Y velocity control input is disabled");
01311 controlUpdateVector[ControlMemberVy] = 0;
01312 }
01313 if (static_cast<bool>(controlUpdateVector[ControlMemberVz]) && static_cast<bool>(accelUpdateVec[StateMemberAz]))
01314 {
01315 ROS_WARN_STREAM("Z acceleration is being measured from IMU; Z velocity control input is disabled");
01316 controlUpdateVector[ControlMemberVz] = 0;
01317 }
01318
01319 int imuQueueSize = 1;
01320 nhLocal_.param(imuTopicName + "_queue_size", imuQueueSize, 1);
01321
01322 bool nodelayImu = false;
01323 nhLocal_.param(imuTopicName + "_nodelay", nodelayImu, false);
01324
01325 if (poseUpdateSum + twistUpdateSum + accelUpdateSum > 0)
01326 {
01327 const CallbackData poseCallbackData(imuTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential,
01328 relative, poseMahalanobisThresh);
01329 const CallbackData twistCallbackData(imuTopicName + "_twist", twistUpdateVec, twistUpdateSum, differential,
01330 relative, twistMahalanobisThresh);
01331 const CallbackData accelCallbackData(imuTopicName + "_acceleration", accelUpdateVec, accelUpdateSum,
01332 differential, relative, accelMahalanobisThresh);
01333
01334 topicSubs_.push_back(
01335 nh_.subscribe<sensor_msgs::Imu>(imuTopic, imuQueueSize,
01336 boost::bind(&RosFilter<T>::imuCallback, this, _1, imuTopicName, poseCallbackData, twistCallbackData,
01337 accelCallbackData), ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayImu)));
01338 }
01339 else
01340 {
01341 ROS_WARN_STREAM("Warning: " << imuTopic << " is listed as an input topic, "
01342 "but all its update variables are false");
01343 }
01344
01345 if (poseUpdateSum > 0)
01346 {
01347 if (differential)
01348 {
01349 twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll];
01350 twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch];
01351 twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw];
01352 }
01353 else
01354 {
01355 absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll];
01356 absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch];
01357 absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw];
01358 }
01359 }
01360
01361 if (twistUpdateSum > 0)
01362 {
01363 twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll];
01364 twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch];
01365 twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw];
01366 }
01367
01368 RF_DEBUG("Subscribed to " << imuTopic << " (" << imuTopicName << ")\n\t" <<
01369 imuTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" <<
01370 imuTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "\n\t" <<
01371 imuTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "\n\t" <<
01372 imuTopicName << "_linear_acceleration_rejection_threshold is " << accelMahalanobisThresh << "\n\t" <<
01373 imuTopicName << "_remove_gravitational_acceleration is " <<
01374 (removeGravAcc ? "true" : "false") << "\n\t" <<
01375 imuTopicName << "_queue_size is " << imuQueueSize << "\n\t" <<
01376 imuTopicName << " pose update vector is " << poseUpdateVec << "\t"<<
01377 imuTopicName << " twist update vector is " << twistUpdateVec << "\t" <<
01378 imuTopicName << " acceleration update vector is " << accelUpdateVec);
01379 }
01380 }
01381 while (moreParams);
01382
01383
01384 if (useControl_ && std::accumulate(controlUpdateVector.begin(), controlUpdateVector.end(), 0) == 0)
01385 {
01386 ROS_ERROR_STREAM("use_control is set to true, but control_config has only false values. No control term "
01387 "will be used.");
01388 useControl_ = false;
01389 }
01390
01391
01392 if (useControl_)
01393 {
01394 latestControl_.resize(TWIST_SIZE);
01395 latestControl_.setZero();
01396
01397 filter_.setControlParams(controlUpdateVector, controlTimeout, accelerationLimits, accelerationGains,
01398 decelerationLimits, decelerationGains);
01399
01400 if (stampedControl)
01401 {
01402 controlSub_ = nh_.subscribe<geometry_msgs::TwistStamped>("cmd_vel", 1, &RosFilter<T>::controlCallback, this);
01403 }
01404 else
01405 {
01406 controlSub_ = nh_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &RosFilter<T>::controlCallback, this);
01407 }
01408 }
01409
01410
01411
01412
01413
01414 if (printDiagnostics_)
01415 {
01416 for (int stateVar = StateMemberX; stateVar <= StateMemberYaw; ++stateVar)
01417 {
01418 if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] > 1)
01419 {
01420 std::stringstream stream;
01421 stream << absPoseVarCounts[static_cast<StateMembers>(stateVar - POSITION_OFFSET)] <<
01422 " absolute pose inputs detected for " << stateVariableNames_[stateVar] <<
01423 ". This may result in oscillations. Please ensure that your variances for each "
01424 "measured variable are set appropriately.";
01425
01426 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
01427 stateVariableNames_[stateVar] + "_configuration",
01428 stream.str(),
01429 true);
01430 }
01431 else if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] == 0)
01432 {
01433 if ((static_cast<StateMembers>(stateVar) == StateMemberX &&
01434 twistVarCounts[static_cast<StateMembers>(StateMemberVx)] == 0) ||
01435 (static_cast<StateMembers>(stateVar) == StateMemberY &&
01436 twistVarCounts[static_cast<StateMembers>(StateMemberVy)] == 0) ||
01437 (static_cast<StateMembers>(stateVar) == StateMemberZ &&
01438 twistVarCounts[static_cast<StateMembers>(StateMemberVz)] == 0 &&
01439 twoDMode_ == false) ||
01440 (static_cast<StateMembers>(stateVar) == StateMemberRoll &&
01441 twistVarCounts[static_cast<StateMembers>(StateMemberVroll)] == 0 &&
01442 twoDMode_ == false) ||
01443 (static_cast<StateMembers>(stateVar) == StateMemberPitch &&
01444 twistVarCounts[static_cast<StateMembers>(StateMemberVpitch)] == 0 &&
01445 twoDMode_ == false) ||
01446 (static_cast<StateMembers>(stateVar) == StateMemberYaw &&
01447 twistVarCounts[static_cast<StateMembers>(StateMemberVyaw)] == 0))
01448 {
01449 std::stringstream stream;
01450 stream << "Neither " << stateVariableNames_[stateVar] << " nor its "
01451 "velocity is being measured. This will result in unbounded "
01452 "error growth and erratic filter behavior.";
01453
01454 addDiagnostic(diagnostic_msgs::DiagnosticStatus::ERROR,
01455 stateVariableNames_[stateVar] + "_configuration",
01456 stream.str(),
01457 true);
01458 }
01459 }
01460 }
01461 }
01462
01463
01464 Eigen::MatrixXd processNoiseCovariance(STATE_SIZE, STATE_SIZE);
01465 processNoiseCovariance.setZero();
01466 XmlRpc::XmlRpcValue processNoiseCovarConfig;
01467
01468 if (nhLocal_.hasParam("process_noise_covariance"))
01469 {
01470 try
01471 {
01472 nhLocal_.getParam("process_noise_covariance", processNoiseCovarConfig);
01473
01474 ROS_ASSERT(processNoiseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
01475
01476 int matSize = processNoiseCovariance.rows();
01477
01478 for (int i = 0; i < matSize; i++)
01479 {
01480 for (int j = 0; j < matSize; j++)
01481 {
01482 try
01483 {
01484
01485
01486
01487 std::ostringstream ostr;
01488 ostr << processNoiseCovarConfig[matSize * i + j];
01489 std::istringstream istr(ostr.str());
01490 istr >> processNoiseCovariance(i, j);
01491 }
01492 catch(XmlRpc::XmlRpcException &e)
01493 {
01494 throw e;
01495 }
01496 catch(...)
01497 {
01498 throw;
01499 }
01500 }
01501 }
01502
01503 RF_DEBUG("Process noise covariance is:\n" << processNoiseCovariance << "\n");
01504 }
01505 catch (XmlRpc::XmlRpcException &e)
01506 {
01507 ROS_ERROR_STREAM("ERROR reading sensor config: " <<
01508 e.getMessage() <<
01509 " for process_noise_covariance (type: " <<
01510 processNoiseCovarConfig.getType() << ")");
01511 }
01512
01513 filter_.setProcessNoiseCovariance(processNoiseCovariance);
01514 }
01515
01516
01517 Eigen::MatrixXd initialEstimateErrorCovariance(STATE_SIZE, STATE_SIZE);
01518 initialEstimateErrorCovariance.setZero();
01519 XmlRpc::XmlRpcValue estimateErrorCovarConfig;
01520
01521 if (nhLocal_.hasParam("initial_estimate_covariance"))
01522 {
01523 try
01524 {
01525 nhLocal_.getParam("initial_estimate_covariance", estimateErrorCovarConfig);
01526
01527 ROS_ASSERT(estimateErrorCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
01528
01529 int matSize = initialEstimateErrorCovariance.rows();
01530
01531 for (int i = 0; i < matSize; i++)
01532 {
01533 for (int j = 0; j < matSize; j++)
01534 {
01535 try
01536 {
01537
01538
01539
01540 std::ostringstream ostr;
01541 ostr << estimateErrorCovarConfig[matSize * i + j];
01542 std::istringstream istr(ostr.str());
01543 istr >> initialEstimateErrorCovariance(i, j);
01544 }
01545 catch(XmlRpc::XmlRpcException &e)
01546 {
01547 throw e;
01548 }
01549 catch(...)
01550 {
01551 throw;
01552 }
01553 }
01554 }
01555
01556 RF_DEBUG("Initial estimate error covariance is:\n" << initialEstimateErrorCovariance << "\n");
01557 }
01558 catch (XmlRpc::XmlRpcException &e)
01559 {
01560 ROS_ERROR_STREAM("ERROR reading initial_estimate_covariance (type: " <<
01561 estimateErrorCovarConfig.getType() <<
01562 "): " <<
01563 e.getMessage());
01564 }
01565 catch(...)
01566 {
01567 ROS_ERROR_STREAM(
01568 "ERROR reading initial_estimate_covariance (type: " << estimateErrorCovarConfig.getType() << ")");
01569 }
01570
01571 filter_.setEstimateErrorCovariance(initialEstimateErrorCovariance);
01572 }
01573 }
01574
01575 template<typename T>
01576 void RosFilter<T>::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,
01577 const CallbackData &poseCallbackData, const CallbackData &twistCallbackData)
01578 {
01579
01580
01581 if (msg->header.stamp <= lastSetPoseTime_)
01582 {
01583 std::stringstream stream;
01584 stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " <<
01585 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
01586 msg->header.stamp.toSec() << ")";
01587 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
01588 topicName + "_timestamp",
01589 stream.str(),
01590 false);
01591 RF_DEBUG("Received message that preceded the most recent pose reset. Ignoring...");
01592
01593 return;
01594 }
01595
01596 RF_DEBUG("------ RosFilter::odometryCallback (" << topicName << ") ------\n" << "Odometry message:\n" << *msg);
01597
01598 if (poseCallbackData.updateSum_ > 0)
01599 {
01600
01601 geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
01602 posPtr->header = msg->header;
01603 posPtr->pose = msg->pose;
01604
01605 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
01606 poseCallback(pptr, poseCallbackData, worldFrameId_, false);
01607 }
01608
01609 if (twistCallbackData.updateSum_ > 0)
01610 {
01611
01612 geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
01613 twistPtr->header = msg->header;
01614 twistPtr->header.frame_id = msg->child_frame_id;
01615 twistPtr->twist = msg->twist;
01616
01617 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
01618 twistCallback(tptr, twistCallbackData, baseLinkFrameId_);
01619 }
01620
01621 RF_DEBUG("\n----- /RosFilter::odometryCallback (" << topicName << ") ------\n");
01622 }
01623
01624 template<typename T>
01625 void RosFilter<T>::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
01626 const CallbackData &callbackData,
01627 const std::string &targetFrame,
01628 const bool imuData)
01629 {
01630 const std::string &topicName = callbackData.topicName_;
01631
01632
01633
01634 if (msg->header.stamp <= lastSetPoseTime_)
01635 {
01636 std::stringstream stream;
01637 stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " <<
01638 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
01639 msg->header.stamp.toSec() << ")";
01640 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
01641 topicName + "_timestamp",
01642 stream.str(),
01643 false);
01644 return;
01645 }
01646
01647 RF_DEBUG("------ RosFilter::poseCallback (" << topicName << ") ------\n" <<
01648 "Pose message:\n" << *msg);
01649
01650
01651 if (lastMessageTimes_.count(topicName) == 0)
01652 {
01653 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
01654 }
01655
01656
01657 if (msg->header.stamp >= lastMessageTimes_[topicName])
01658 {
01659 RF_DEBUG("Update vector for " << topicName << " is:\n" << callbackData.updateVector_);
01660
01661 Eigen::VectorXd measurement(STATE_SIZE);
01662 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
01663
01664 measurement.setZero();
01665 measurementCovariance.setZero();
01666
01667
01668 std::vector<int> updateVectorCorrected = callbackData.updateVector_;
01669
01670
01671 if (preparePose(msg,
01672 topicName,
01673 targetFrame,
01674 callbackData.differential_,
01675 callbackData.relative_,
01676 imuData,
01677 updateVectorCorrected,
01678 measurement,
01679 measurementCovariance))
01680 {
01681
01682
01683 enqueueMeasurement(topicName,
01684 measurement,
01685 measurementCovariance,
01686 updateVectorCorrected,
01687 callbackData.rejectionThreshold_,
01688 msg->header.stamp);
01689
01690 RF_DEBUG("Enqueued new measurement for " << topicName << "\n");
01691 }
01692 else
01693 {
01694 RF_DEBUG("Did *not* enqueue measurement for " << topicName << "\n");
01695 }
01696
01697 lastMessageTimes_[topicName] = msg->header.stamp;
01698
01699 RF_DEBUG("Last message time for " << topicName << " is now " <<
01700 lastMessageTimes_[topicName] << "\n");
01701 }
01702 else if (resetOnTimeJump_ && ros::Time::isSimTime())
01703 {
01704 reset();
01705 }
01706 else
01707 {
01708 std::stringstream stream;
01709 stream << "The " << topicName << " message has a timestamp before that of the previous message received," <<
01710 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
01711 msg->header.stamp.toSec() << ")";
01712 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
01713 topicName + "_timestamp",
01714 stream.str(),
01715 false);
01716
01717 RF_DEBUG("Message is too old. Last message time for " << topicName << " is "
01718 << lastMessageTimes_[topicName] << ", current message time is "
01719 << msg->header.stamp << ".\n");
01720 }
01721
01722 RF_DEBUG("\n----- /RosFilter::poseCallback (" << topicName << ") ------\n");
01723 }
01724
01725 template<typename T>
01726 void RosFilter<T>::run()
01727 {
01728 ROS_INFO("Waiting for valid clock time...");
01729 ros::Time::waitForValid();
01730 ROS_INFO("Valid clock time received. Starting node.");
01731
01732 loadParams();
01733
01734 if (printDiagnostics_)
01735 {
01736 diagnosticUpdater_.add("Filter diagnostic updater", this, &RosFilter<T>::aggregateDiagnostics);
01737 }
01738
01739
01740 double minFrequency = frequency_ - 2;
01741 double maxFrequency = frequency_ + 2;
01742 diagnostic_updater::HeaderlessTopicDiagnostic freqDiag("odometry/filtered",
01743 diagnosticUpdater_,
01744 diagnostic_updater::FrequencyStatusParam(&minFrequency,
01745 &maxFrequency,
01746 0.1, 10));
01747
01748
01749
01750 tf2::Transform mapOdomTrans;
01751 tf2::Transform odomBaseLinkTrans;
01752 geometry_msgs::TransformStamped mapOdomTransMsg;
01753 ros::Time curTime;
01754 ros::Time lastDiagTime = ros::Time::now();
01755
01756
01757 worldBaseLinkTransMsg_.transform = tf2::toMsg(tf2::Transform::getIdentity());
01758 mapOdomTransMsg.transform = tf2::toMsg(tf2::Transform::getIdentity());
01759
01760
01761 ros::Publisher positionPub = nh_.advertise<nav_msgs::Odometry>("odometry/filtered", 20);
01762 tf2_ros::TransformBroadcaster worldTransformBroadcaster;
01763
01764
01765 ros::Publisher accelPub;
01766 if (publishAcceleration_)
01767 {
01768 accelPub = nh_.advertise<geometry_msgs::AccelWithCovarianceStamped>("accel/filtered", 20);
01769 }
01770
01771 ros::Rate loop_rate(frequency_);
01772
01773 while (ros::ok())
01774 {
01775
01776
01777 ros::spinOnce();
01778 curTime = ros::Time::now();
01779
01780
01781 integrateMeasurements(curTime);
01782
01783
01784 nav_msgs::Odometry filteredPosition;
01785
01786 if (getFilteredOdometryMessage(filteredPosition))
01787 {
01788 worldBaseLinkTransMsg_.header.stamp = filteredPosition.header.stamp + tfTimeOffset_;
01789 worldBaseLinkTransMsg_.header.frame_id = filteredPosition.header.frame_id;
01790 worldBaseLinkTransMsg_.child_frame_id = filteredPosition.child_frame_id;
01791
01792 worldBaseLinkTransMsg_.transform.translation.x = filteredPosition.pose.pose.position.x;
01793 worldBaseLinkTransMsg_.transform.translation.y = filteredPosition.pose.pose.position.y;
01794 worldBaseLinkTransMsg_.transform.translation.z = filteredPosition.pose.pose.position.z;
01795 worldBaseLinkTransMsg_.transform.rotation = filteredPosition.pose.pose.orientation;
01796
01797
01798
01799 if (publishTransform_)
01800 {
01801 if (filteredPosition.header.frame_id == odomFrameId_)
01802 {
01803 worldTransformBroadcaster.sendTransform(worldBaseLinkTransMsg_);
01804 }
01805 else if (filteredPosition.header.frame_id == mapFrameId_)
01806 {
01807 try
01808 {
01809 tf2::Transform worldBaseLinkTrans;
01810 tf2::fromMsg(worldBaseLinkTransMsg_.transform, worldBaseLinkTrans);
01811
01812 tf2::fromMsg(tfBuffer_.lookupTransform(baseLinkFrameId_, odomFrameId_, ros::Time(0)).transform,
01813 odomBaseLinkTrans);
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833 mapOdomTrans.mult(worldBaseLinkTrans, odomBaseLinkTrans);
01834
01835 mapOdomTransMsg.transform = tf2::toMsg(mapOdomTrans);
01836 mapOdomTransMsg.header.stamp = filteredPosition.header.stamp + tfTimeOffset_;
01837 mapOdomTransMsg.header.frame_id = mapFrameId_;
01838 mapOdomTransMsg.child_frame_id = odomFrameId_;
01839
01840 worldTransformBroadcaster.sendTransform(mapOdomTransMsg);
01841 }
01842 catch(...)
01843 {
01844 ROS_ERROR_STREAM_DELAYED_THROTTLE(5.0, "Could not obtain transform from "
01845 << odomFrameId_ << "->" << baseLinkFrameId_);
01846 }
01847 }
01848 else
01849 {
01850 ROS_ERROR_STREAM("Odometry message frame_id was " << filteredPosition.header.frame_id <<
01851 ", expected " << mapFrameId_ << " or " << odomFrameId_);
01852 }
01853 }
01854
01855
01856 positionPub.publish(filteredPosition);
01857
01858 if (printDiagnostics_)
01859 {
01860 freqDiag.tick();
01861 }
01862 }
01863
01864
01865 geometry_msgs::AccelWithCovarianceStamped filteredAcceleration;
01866 if (publishAcceleration_ && getFilteredAccelMessage(filteredAcceleration))
01867 {
01868 accelPub.publish(filteredAcceleration);
01869 }
01870
01871
01872
01873
01874
01875 double diagDuration = (curTime - lastDiagTime).toSec();
01876 if (printDiagnostics_ && (diagDuration >= diagnosticUpdater_.getPeriod() || diagDuration < 0.0))
01877 {
01878 diagnosticUpdater_.force_update();
01879 lastDiagTime = curTime;
01880 }
01881
01882
01883 if (smoothLaggedData_)
01884 {
01885 clearExpiredHistory(filter_.getLastMeasurementTime() - historyLength_);
01886 }
01887
01888 if (!loop_rate.sleep())
01889 {
01890 ROS_WARN_STREAM("Failed to meet update rate! Try decreasing the rate, limiting "
01891 "sensor output frequency, or limiting the number of sensors.");
01892 }
01893 }
01894 }
01895
01896 template<typename T>
01897 void RosFilter<T>::setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
01898 {
01899 RF_DEBUG("------ RosFilter::setPoseCallback ------\nPose message:\n" << *msg);
01900
01901 std::string topicName("setPose");
01902
01903
01904 initialMeasurements_.clear();
01905 previousMeasurements_.clear();
01906 previousMeasurementCovariances_.clear();
01907
01908
01909
01910 while (!measurementQueue_.empty() && ros::ok())
01911 {
01912 measurementQueue_.pop();
01913 }
01914
01915 filterStateHistory_.clear();
01916 measurementHistory_.clear();
01917
01918
01919
01920 lastSetPoseTime_ = msg->header.stamp;
01921
01922
01923 Eigen::VectorXd measurement(STATE_SIZE);
01924 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
01925 std::vector<int> updateVector(STATE_SIZE, true);
01926
01927
01928 measurement.setZero();
01929
01930
01931 measurementCovariance.setIdentity();
01932 measurementCovariance *= 1e-6;
01933
01934
01935
01936 preparePose(msg, topicName, worldFrameId_, false, false, false, updateVector, measurement, measurementCovariance);
01937
01938
01939 filter_.setState(measurement);
01940 filter_.setEstimateErrorCovariance(measurementCovariance);
01941
01942 filter_.setLastMeasurementTime(ros::Time::now().toSec());
01943 filter_.setLastUpdateTime(ros::Time::now().toSec());
01944
01945
01946
01947 ros::getGlobalCallbackQueue()->clear();
01948
01949 RF_DEBUG("\n------ /RosFilter::setPoseCallback ------\n");
01950 }
01951
01952 template<typename T>
01953 bool RosFilter<T>::setPoseSrvCallback(robot_localization::SetPose::Request& request,
01954 robot_localization::SetPose::Response&)
01955 {
01956 geometry_msgs::PoseWithCovarianceStamped::Ptr msg;
01957 msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>(request.pose);
01958 setPoseCallback(msg);
01959
01960 return true;
01961 }
01962
01963 template<typename T>
01964 void RosFilter<T>::twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
01965 const CallbackData &callbackData,
01966 const std::string &targetFrame)
01967 {
01968 const std::string &topicName = callbackData.topicName_;
01969
01970
01971
01972 if (msg->header.stamp <= lastSetPoseTime_)
01973 {
01974 std::stringstream stream;
01975 stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " <<
01976 "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " <<
01977 msg->header.stamp.toSec() << ")";
01978 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
01979 topicName + "_timestamp",
01980 stream.str(),
01981 false);
01982 return;
01983 }
01984
01985 RF_DEBUG("------ RosFilter::twistCallback (" << topicName << ") ------\n"
01986 "Twist message:\n" << *msg);
01987
01988 if (lastMessageTimes_.count(topicName) == 0)
01989 {
01990 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
01991 }
01992
01993
01994 if (msg->header.stamp >= lastMessageTimes_[topicName])
01995 {
01996 RF_DEBUG("Update vector for " << topicName << " is:\n" << callbackData.updateVector_);
01997
01998 Eigen::VectorXd measurement(STATE_SIZE);
01999 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
02000
02001 measurement.setZero();
02002 measurementCovariance.setZero();
02003
02004
02005 std::vector<int> updateVectorCorrected = callbackData.updateVector_;
02006
02007
02008 if (prepareTwist(msg, topicName, targetFrame, updateVectorCorrected, measurement, measurementCovariance))
02009 {
02010
02011
02012 enqueueMeasurement(topicName,
02013 measurement,
02014 measurementCovariance,
02015 updateVectorCorrected,
02016 callbackData.rejectionThreshold_,
02017 msg->header.stamp);
02018
02019 RF_DEBUG("Enqueued new measurement for " << topicName << "_twist\n");
02020 }
02021 else
02022 {
02023 RF_DEBUG("Did *not* enqueue measurement for " << topicName << "_twist\n");
02024 }
02025
02026 lastMessageTimes_[topicName] = msg->header.stamp;
02027
02028 RF_DEBUG("Last message time for " << topicName << " is now " <<
02029 lastMessageTimes_[topicName] << "\n");
02030 }
02031 else if (resetOnTimeJump_ && ros::Time::isSimTime())
02032 {
02033 reset();
02034 }
02035 else
02036 {
02037 std::stringstream stream;
02038 stream << "The " << topicName << " message has a timestamp before that of the previous message received," <<
02039 " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
02040 msg->header.stamp.toSec() << ")";
02041 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
02042 topicName + "_timestamp",
02043 stream.str(),
02044 false);
02045
02046 RF_DEBUG("Message is too old. Last message time for " << topicName << " is " << lastMessageTimes_[topicName] <<
02047 ", current message time is " << msg->header.stamp << ".\n");
02048 }
02049
02050 RF_DEBUG("\n----- /RosFilter::twistCallback (" << topicName << ") ------\n");
02051 }
02052
02053 template<typename T>
02054 void RosFilter<T>::addDiagnostic(const int errLevel,
02055 const std::string &topicAndClass,
02056 const std::string &message,
02057 const bool staticDiag)
02058 {
02059 if (staticDiag)
02060 {
02061 staticDiagnostics_[topicAndClass] = message;
02062 staticDiagErrorLevel_ = std::max(staticDiagErrorLevel_, errLevel);
02063 }
02064 else
02065 {
02066 dynamicDiagnostics_[topicAndClass] = message;
02067 dynamicDiagErrorLevel_ = std::max(dynamicDiagErrorLevel_, errLevel);
02068 }
02069 }
02070
02071 template<typename T>
02072 void RosFilter<T>::aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper)
02073 {
02074 wrapper.clear();
02075 wrapper.clearSummary();
02076
02077 int maxErrLevel = std::max(staticDiagErrorLevel_, dynamicDiagErrorLevel_);
02078
02079
02080 switch (maxErrLevel)
02081 {
02082 case diagnostic_msgs::DiagnosticStatus::ERROR:
02083 wrapper.summary(maxErrLevel,
02084 "Erroneous data or settings detected for a robot_localization state estimation node.");
02085 break;
02086 case diagnostic_msgs::DiagnosticStatus::WARN:
02087 wrapper.summary(maxErrLevel,
02088 "Potentially erroneous data or settings detected for "
02089 "a robot_localization state estimation node.");
02090 break;
02091 case diagnostic_msgs::DiagnosticStatus::STALE:
02092 wrapper.summary(maxErrLevel,
02093 "The state of the robot_localization state estimation node is stale.");
02094 break;
02095 case diagnostic_msgs::DiagnosticStatus::OK:
02096 wrapper.summary(maxErrLevel,
02097 "The robot_localization state estimation node appears to be functioning properly.");
02098 break;
02099 default:
02100 break;
02101 }
02102
02103
02104 for (std::map<std::string, std::string>::iterator diagIt = staticDiagnostics_.begin();
02105 diagIt != staticDiagnostics_.end();
02106 ++diagIt)
02107 {
02108 wrapper.add(diagIt->first, diagIt->second);
02109 }
02110
02111
02112 for (std::map<std::string, std::string>::iterator diagIt = dynamicDiagnostics_.begin();
02113 diagIt != dynamicDiagnostics_.end();
02114 ++diagIt)
02115 {
02116 wrapper.add(diagIt->first, diagIt->second);
02117 }
02118 dynamicDiagnostics_.clear();
02119
02120
02121 dynamicDiagErrorLevel_ = diagnostic_msgs::DiagnosticStatus::OK;
02122 }
02123
02124 template<typename T>
02125 void RosFilter<T>::copyCovariance(const double *arr,
02126 Eigen::MatrixXd &covariance,
02127 const std::string &topicName,
02128 const std::vector<int> &updateVector,
02129 const size_t offset,
02130 const size_t dimension)
02131 {
02132 for (size_t i = 0; i < dimension; i++)
02133 {
02134 for (size_t j = 0; j < dimension; j++)
02135 {
02136 covariance(i, j) = arr[dimension * i + j];
02137
02138 if (printDiagnostics_)
02139 {
02140 std::string iVar = stateVariableNames_[offset + i];
02141
02142 if (covariance(i, j) > 1e3 && (updateVector[offset + i] || updateVector[offset + j]))
02143 {
02144 std::string jVar = stateVariableNames_[offset + j];
02145
02146 std::stringstream stream;
02147 stream << "The covariance at position (" << dimension * i + j << "), which corresponds to " <<
02148 (i == j ? iVar + " variance" : iVar + " and " + jVar + " covariance") <<
02149 ", the value is extremely large (" << covariance(i, j) << "), but the update vector for " <<
02150 (i == j ? iVar : iVar + " and/or " + jVar) << " is set to true. This may produce undesirable results.";
02151
02152 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
02153 topicName + "_covariance",
02154 stream.str(),
02155 false);
02156 }
02157 else if (updateVector[i] && i == j && covariance(i, j) == 0)
02158 {
02159 std::stringstream stream;
02160 stream << "The covariance at position (" << dimension * i + j << "), which corresponds to " <<
02161 iVar << " variance, was zero. This will be replaced with a small value to maintain filter "
02162 "stability, but should be corrected at the message origin node.";
02163
02164 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
02165 topicName + "_covariance",
02166 stream.str(),
02167 false);
02168 }
02169 else if (updateVector[i] && i == j && covariance(i, j) < 0)
02170 {
02171 std::stringstream stream;
02172 stream << "The covariance at position (" << dimension * i + j <<
02173 "), which corresponds to " << iVar << " variance, was negative. This will be replaced with a "
02174 "small positive value to maintain filter stability, but should be corrected at the message "
02175 "origin node.";
02176
02177 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
02178 topicName + "_covariance",
02179 stream.str(),
02180 false);
02181 }
02182 }
02183 }
02184 }
02185 }
02186
02187 template<typename T>
02188 void RosFilter<T>::copyCovariance(const Eigen::MatrixXd &covariance,
02189 double *arr,
02190 const size_t dimension)
02191 {
02192 for (size_t i = 0; i < dimension; i++)
02193 {
02194 for (size_t j = 0; j < dimension; j++)
02195 {
02196 arr[dimension * i + j] = covariance(i, j);
02197 }
02198 }
02199 }
02200
02201 template<typename T>
02202 std::vector<int> RosFilter<T>::loadUpdateConfig(const std::string &topicName)
02203 {
02204 XmlRpc::XmlRpcValue topicConfig;
02205 std::vector<int> updateVector(STATE_SIZE, 0);
02206 std::string topicConfigName = topicName + "_config";
02207
02208 try
02209 {
02210 nhLocal_.getParam(topicConfigName, topicConfig);
02211
02212 ROS_ASSERT(topicConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
02213
02214 if (topicConfig.size() != STATE_SIZE)
02215 {
02216 ROS_WARN_STREAM("Configuration vector for " << topicConfigName << " should have 15 entries.");
02217 }
02218
02219 for (int i = 0; i < topicConfig.size(); i++)
02220 {
02221
02222
02223
02224 updateVector[i] = static_cast<int>(static_cast<bool>(topicConfig[i]));
02225 }
02226 }
02227 catch (XmlRpc::XmlRpcException &e)
02228 {
02229 ROS_FATAL_STREAM("Could not read sensor update configuration for topic " << topicName <<
02230 " (type: " << topicConfig.getType() << ", expected: " << XmlRpc::XmlRpcValue::TypeArray
02231 << "). Error was " << e.getMessage() << "\n");
02232 }
02233
02234 return updateVector;
02235 }
02236
02237 template<typename T>
02238 bool RosFilter<T>::prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg,
02239 const std::string &topicName,
02240 const std::string &targetFrame,
02241 std::vector<int> &updateVector,
02242 Eigen::VectorXd &measurement,
02243 Eigen::MatrixXd &measurementCovariance)
02244 {
02245 RF_DEBUG("------ RosFilter::prepareAcceleration (" << topicName << ") ------\n");
02246
02247
02248 tf2::Vector3 accTmp(msg->linear_acceleration.x,
02249 msg->linear_acceleration.y,
02250 msg->linear_acceleration.z);
02251
02252
02253 std::string msgFrame = (msg->header.frame_id == "" ? baseLinkFrameId_ : msg->header.frame_id);
02254
02255
02256
02257
02258
02259
02260
02261
02262
02263 tf2::Matrix3x3 maskAcc(updateVector[StateMemberAx], 0, 0,
02264 0, updateVector[StateMemberAy], 0,
02265 0, 0, updateVector[StateMemberAz]);
02266
02267
02268 Eigen::MatrixXd covarianceRotated(ACCELERATION_SIZE, ACCELERATION_SIZE);
02269 covarianceRotated.setZero();
02270
02271 this->copyCovariance(&(msg->linear_acceleration_covariance[0]),
02272 covarianceRotated,
02273 topicName,
02274 updateVector,
02275 POSITION_A_OFFSET,
02276 ACCELERATION_SIZE);
02277
02278 RF_DEBUG("Original measurement as tf object: " << accTmp <<
02279 "\nOriginal update vector:\n" << updateVector <<
02280 "\nOriginal covariance matrix:\n" << covarianceRotated << "\n");
02281
02282
02283
02284
02285 tf2::Transform targetFrameTrans;
02286 bool canTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_,
02287 targetFrame,
02288 msgFrame,
02289 msg->header.stamp,
02290 tfTimeout_,
02291 targetFrameTrans);
02292
02293 if (canTransform)
02294 {
02295
02296
02297 if (removeGravitationalAcc_[topicName])
02298 {
02299 tf2::Vector3 normAcc(0, 0, gravitationalAcc_);
02300 tf2::Quaternion curAttitude;
02301 tf2::Transform trans;
02302
02303 if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9)
02304 {
02305
02306
02307 const Eigen::VectorXd &state = filter_.getState();
02308 tf2::Vector3 stateTmp(state(StateMemberRoll),
02309 state(StateMemberPitch),
02310 state(StateMemberYaw));
02311
02312 tf2::Transform imuFrameTrans;
02313 RosFilterUtilities::lookupTransformSafe(tfBuffer_,
02314 msgFrame,
02315 targetFrame,
02316 msg->header.stamp,
02317 tfTimeout_,
02318 imuFrameTrans);
02319 stateTmp = imuFrameTrans.getBasis() * stateTmp;
02320 curAttitude.setRPY(stateTmp.getX(), stateTmp.getY(), stateTmp.getZ());
02321 }
02322 else
02323 {
02324 tf2::fromMsg(msg->orientation, curAttitude);
02325 }
02326 trans.setRotation(curAttitude);
02327 tf2::Vector3 rotNorm = trans.getBasis().inverse() * normAcc;
02328 accTmp.setX(accTmp.getX() - rotNorm.getX());
02329 accTmp.setY(accTmp.getY() - rotNorm.getY());
02330 accTmp.setZ(accTmp.getZ() - rotNorm.getZ());
02331
02332 RF_DEBUG("Orientation is " << curAttitude <<
02333 "Acceleration due to gravity is " << rotNorm <<
02334 "After removing acceleration due to gravity, acceleration is " << accTmp << "\n");
02335 }
02336
02337
02338
02339
02340
02341
02342
02343
02344
02345 accTmp = targetFrameTrans.getBasis() * accTmp;
02346 maskAcc = targetFrameTrans.getBasis() * maskAcc;
02347
02348
02349 updateVector[StateMemberAx] = static_cast<int>(
02350 maskAcc.getRow(StateMemberAx - POSITION_A_OFFSET).length() >= 1e-6);
02351 updateVector[StateMemberAy] = static_cast<int>(
02352 maskAcc.getRow(StateMemberAy - POSITION_A_OFFSET).length() >= 1e-6);
02353 updateVector[StateMemberAz] = static_cast<int>(
02354 maskAcc.getRow(StateMemberAz - POSITION_A_OFFSET).length() >= 1e-6);
02355
02356 RF_DEBUG(msg->header.frame_id << "->" << targetFrame << " transform:\n" << targetFrameTrans <<
02357 "\nAfter applying transform to " << targetFrame << ", update vector is:\n" << updateVector <<
02358 "\nAfter applying transform to " << targetFrame << ", measurement is:\n" << accTmp << "\n");
02359
02360
02361
02362
02363
02364 tf2::Matrix3x3 rot(targetFrameTrans.getRotation());
02365 Eigen::MatrixXd rot3d(ACCELERATION_SIZE, ACCELERATION_SIZE);
02366 rot3d.setIdentity();
02367
02368 for (size_t rInd = 0; rInd < ACCELERATION_SIZE; ++rInd)
02369 {
02370 rot3d(rInd, 0) = rot.getRow(rInd).getX();
02371 rot3d(rInd, 1) = rot.getRow(rInd).getY();
02372 rot3d(rInd, 2) = rot.getRow(rInd).getZ();
02373 }
02374
02375
02376 covarianceRotated = rot3d * covarianceRotated.eval() * rot3d.transpose();
02377
02378 RF_DEBUG("Transformed covariance is \n" << covarianceRotated << "\n");
02379
02380
02381 measurement(StateMemberAx) = accTmp.getX();
02382 measurement(StateMemberAy) = accTmp.getY();
02383 measurement(StateMemberAz) = accTmp.getZ();
02384
02385
02386 measurementCovariance.block(POSITION_A_OFFSET, POSITION_A_OFFSET, ACCELERATION_SIZE, ACCELERATION_SIZE) =
02387 covarianceRotated.block(0, 0, ACCELERATION_SIZE, ACCELERATION_SIZE);
02388
02389
02390 if (twoDMode_)
02391 {
02392 forceTwoD(measurement, measurementCovariance, updateVector);
02393 }
02394 }
02395 else
02396 {
02397 RF_DEBUG("Could not transform measurement into " << targetFrame << ". Ignoring...\n");
02398 }
02399
02400 RF_DEBUG("\n----- /RosFilter::prepareAcceleration(" << topicName << ") ------\n");
02401
02402 return canTransform;
02403 }
02404
02405 template<typename T>
02406 bool RosFilter<T>::preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
02407 const std::string &topicName,
02408 const std::string &targetFrame,
02409 const bool differential,
02410 const bool relative,
02411 const bool imuData,
02412 std::vector<int> &updateVector,
02413 Eigen::VectorXd &measurement,
02414 Eigen::MatrixXd &measurementCovariance)
02415 {
02416 bool retVal = false;
02417
02418 RF_DEBUG("------ RosFilter::preparePose (" << topicName << ") ------\n");
02419
02420
02421 tf2::Stamped<tf2::Transform> poseTmp;
02422
02423
02424 tf2::Transform curMeasurement;
02425
02426
02427
02428
02429 std::string finalTargetFrame;
02430 if (targetFrame == "" && msg->header.frame_id == "")
02431 {
02432
02433
02434 finalTargetFrame = worldFrameId_;
02435 poseTmp.frame_id_ = finalTargetFrame;
02436 }
02437 else if (targetFrame == "")
02438 {
02439
02440
02441 finalTargetFrame = msg->header.frame_id;
02442 poseTmp.frame_id_ = finalTargetFrame;
02443 }
02444 else
02445 {
02446
02447 finalTargetFrame = targetFrame;
02448 poseTmp.frame_id_ = (differential ? finalTargetFrame : msg->header.frame_id);
02449 }
02450
02451 RF_DEBUG("Final target frame for " << topicName << " is " << finalTargetFrame << "\n");
02452
02453 poseTmp.stamp_ = msg->header.stamp;
02454
02455
02456 poseTmp.setOrigin(tf2::Vector3(msg->pose.pose.position.x,
02457 msg->pose.pose.position.y,
02458 msg->pose.pose.position.z));
02459
02460 tf2::Quaternion orientation;
02461
02462
02463 if (msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 &&
02464 msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0)
02465 {
02466 orientation.setValue(0.0, 0.0, 0.0, 1.0);
02467
02468 if (updateVector[StateMemberRoll] || updateVector[StateMemberPitch] || updateVector[StateMemberYaw])
02469 {
02470 std::stringstream stream;
02471 stream << "The " << topicName << " message contains an invalid orientation quaternion, " <<
02472 "but its configuration is such that orientation data is being used. Correcting...";
02473
02474 addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
02475 topicName + "_orientation",
02476 stream.str(),
02477 false);
02478 }
02479 }
02480 else
02481 {
02482 tf2::fromMsg(msg->pose.pose.orientation, orientation);
02483 }
02484
02485
02486 poseTmp.setRotation(orientation);
02487
02488
02489 tf2::Transform targetFrameTrans;
02490 bool canTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_,
02491 finalTargetFrame,
02492 poseTmp.frame_id_,
02493 poseTmp.stamp_,
02494 tfTimeout_,
02495 targetFrameTrans);
02496
02497
02498 if (canTransform)
02499 {
02500
02501
02502
02503
02504
02505
02506
02507
02508
02509
02510
02511 tf2::Matrix3x3 maskPosition(updateVector[StateMemberX], 0, 0,
02512 0, updateVector[StateMemberY], 0,
02513 0, 0, updateVector[StateMemberZ]);
02514
02515 tf2::Matrix3x3 maskOrientation(updateVector[StateMemberRoll], 0, 0,
02516 0, updateVector[StateMemberPitch], 0,
02517 0, 0, updateVector[StateMemberYaw]);
02518
02519 if (imuData)
02520 {
02521
02522
02523
02524
02525
02526
02527
02528 double dummy, yaw;
02529 targetFrameTrans.getBasis().getRPY(dummy, dummy, yaw);
02530 tf2::Matrix3x3 transTmp;
02531 transTmp.setRPY(0.0, 0.0, yaw);
02532
02533 maskPosition = transTmp * maskPosition;
02534 maskOrientation = transTmp * maskOrientation;
02535 }
02536 else
02537 {
02538 maskPosition = targetFrameTrans.getBasis() * maskPosition;
02539 maskOrientation = targetFrameTrans.getBasis() * maskOrientation;
02540 }
02541
02542
02543
02544 updateVector[StateMemberX] = static_cast<int>(
02545 maskPosition.getRow(StateMemberX - POSITION_OFFSET).length() >= 1e-6);
02546 updateVector[StateMemberY] = static_cast<int>(
02547 maskPosition.getRow(StateMemberY - POSITION_OFFSET).length() >= 1e-6);
02548 updateVector[StateMemberZ] = static_cast<int>(
02549 maskPosition.getRow(StateMemberZ - POSITION_OFFSET).length() >= 1e-6);
02550 updateVector[StateMemberRoll] = static_cast<int>(
02551 maskOrientation.getRow(StateMemberRoll - ORIENTATION_OFFSET).length() >= 1e-6);
02552 updateVector[StateMemberPitch] = static_cast<int>(
02553 maskOrientation.getRow(StateMemberPitch - ORIENTATION_OFFSET).length() >= 1e-6);
02554 updateVector[StateMemberYaw] = static_cast<int>(
02555 maskOrientation.getRow(StateMemberYaw - ORIENTATION_OFFSET).length() >= 1e-6);
02556
02557
02558
02559 Eigen::MatrixXd covariance(POSE_SIZE, POSE_SIZE);
02560 covariance.setZero();
02561 copyCovariance(&(msg->pose.covariance[0]), covariance, topicName, updateVector, POSITION_OFFSET, POSE_SIZE);
02562
02563
02564
02565
02566 tf2::Matrix3x3 rot;
02567 Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE);
02568 rot6d.setIdentity();
02569 Eigen::MatrixXd covarianceRotated;
02570
02571 if (imuData)
02572 {
02573
02574 double dummy, yaw;
02575 targetFrameTrans.getBasis().getRPY(dummy, dummy, yaw);
02576 rot.setRPY(0.0, 0.0, yaw);
02577 }
02578 else
02579 {
02580 rot.setRotation(targetFrameTrans.getRotation());
02581 }
02582
02583 for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
02584 {
02585 rot6d(rInd, 0) = rot.getRow(rInd).getX();
02586 rot6d(rInd, 1) = rot.getRow(rInd).getY();
02587 rot6d(rInd, 2) = rot.getRow(rInd).getZ();
02588 rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
02589 rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
02590 rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
02591 }
02592
02593
02594 covarianceRotated = rot6d * covariance * rot6d.transpose();
02595
02596 RF_DEBUG("After rotating into the " << finalTargetFrame <<
02597 " frame, covariance is \n" << covarianceRotated << "\n");
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607 if (imuData)
02608 {
02609
02610
02611 double rollOffset = 0;
02612 double pitchOffset = 0;
02613 double yawOffset = 0;
02614 double roll = 0;
02615 double pitch = 0;
02616 double yaw = 0;
02617 RosFilterUtilities::quatToRPY(targetFrameTrans.getRotation(), rollOffset, pitchOffset, yawOffset);
02618 RosFilterUtilities::quatToRPY(poseTmp.getRotation(), roll, pitch, yaw);
02619
02620
02621 tf2::Vector3 rpyAngles(FilterUtilities::clampRotation(roll - rollOffset),
02622 FilterUtilities::clampRotation(pitch - pitchOffset),
02623 FilterUtilities::clampRotation(yaw - yawOffset));
02624
02625
02626
02627
02628 tf2::Matrix3x3 mat;
02629 mat.setRPY(0.0, 0.0, yawOffset);
02630 rpyAngles = mat * rpyAngles;
02631 poseTmp.getBasis().setRPY(rpyAngles.getX(), rpyAngles.getY(), rpyAngles.getZ());
02632
02633
02634
02635
02636
02637 targetFrameTrans.setIdentity();
02638 }
02639
02640
02641
02642 if (differential)
02643 {
02644 bool success = false;
02645
02646
02647
02648
02649 curMeasurement = poseTmp;
02650
02651
02652 if (previousMeasurements_.count(topicName) > 0 && previousMeasurementCovariances_.count(topicName) > 0)
02653 {
02654
02655
02656
02657
02658
02659
02660
02661 tf2::Transform prevMeasurement = previousMeasurements_[topicName];
02662 poseTmp.setData(prevMeasurement.inverseTimes(poseTmp));
02663
02664 RF_DEBUG("Previous measurement:\n" << previousMeasurements_[topicName] <<
02665 "\nAfter removing previous measurement, measurement delta is:\n" << poseTmp << "\n");
02666
02667
02668
02669
02670 targetFrameTrans.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));
02671 poseTmp.mult(targetFrameTrans, poseTmp);
02672
02673 RF_DEBUG("After rotating to the target frame, measurement delta is:\n" << poseTmp << "\n");
02674
02675
02676
02677 double dt = msg->header.stamp.toSec() - lastMessageTimes_[topicName].toSec();
02678 double xVel = poseTmp.getOrigin().getX() / dt;
02679 double yVel = poseTmp.getOrigin().getY() / dt;
02680 double zVel = poseTmp.getOrigin().getZ() / dt;
02681
02682 double rollVel = 0;
02683 double pitchVel = 0;
02684 double yawVel = 0;
02685
02686 RosFilterUtilities::quatToRPY(poseTmp.getRotation(), rollVel, pitchVel, yawVel);
02687 rollVel /= dt;
02688 pitchVel /= dt;
02689 yawVel /= dt;
02690
02691 RF_DEBUG("Previous message time was " << lastMessageTimes_[topicName].toSec() <<
02692 ", current message time is " << msg->header.stamp.toSec() << ", delta is " <<
02693 dt << ", velocity is (vX, vY, vZ): (" << xVel << ", " << yVel << ", " << zVel <<
02694 ")\n" << "(vRoll, vPitch, vYaw): (" << rollVel << ", " << pitchVel << ", " <<
02695 yawVel << ")\n");
02696
02697
02698 geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
02699 twistPtr->header = msg->header;
02700 twistPtr->header.frame_id = baseLinkFrameId_;
02701 twistPtr->twist.twist.linear.x = xVel;
02702 twistPtr->twist.twist.linear.y = yVel;
02703 twistPtr->twist.twist.linear.z = zVel;
02704 twistPtr->twist.twist.angular.x = rollVel;
02705 twistPtr->twist.twist.angular.y = pitchVel;
02706 twistPtr->twist.twist.angular.z = yawVel;
02707 std::vector<int> twistUpdateVec(STATE_SIZE, false);
02708 std::copy(updateVector.begin() + POSITION_OFFSET,
02709 updateVector.begin() + POSE_SIZE,
02710 twistUpdateVec.begin() + POSITION_V_OFFSET);
02711 std::copy(twistUpdateVec.begin(), twistUpdateVec.end(), updateVector.begin());
02712 geometry_msgs::TwistWithCovarianceStampedConstPtr ptr(twistPtr);
02713
02714
02715
02716
02717 Eigen::MatrixXd prevCovarRotated = rot6d * previousMeasurementCovariances_[topicName] * rot6d.transpose();
02718 covarianceRotated = (covarianceRotated.eval() + prevCovarRotated) * dt;
02719 copyCovariance(covarianceRotated, &(twistPtr->twist.covariance[0]), POSE_SIZE);
02720
02721 RF_DEBUG("Previous measurement covariance:\n" << previousMeasurementCovariances_[topicName] <<
02722 "\nPrevious measurement covariance rotated:\n" << prevCovarRotated <<
02723 "\nFinal twist covariance:\n" << covarianceRotated << "\n");
02724
02725
02726 success = prepareTwist(ptr,
02727 topicName + "_twist",
02728 twistPtr->header.frame_id,
02729 updateVector,
02730 measurement,
02731 measurementCovariance);
02732 }
02733
02734
02735 previousMeasurements_[topicName] = curMeasurement;
02736 previousMeasurementCovariances_[topicName] = covariance;
02737
02738 retVal = success;
02739 }
02740 else
02741 {
02742
02743 if (relative)
02744 {
02745 if (initialMeasurements_.count(topicName) == 0)
02746 {
02747 initialMeasurements_.insert(std::pair<std::string, tf2::Transform>(topicName, poseTmp));
02748 }
02749
02750 tf2::Transform initialMeasurement = initialMeasurements_[topicName];
02751 poseTmp.setData(initialMeasurement.inverseTimes(poseTmp));
02752 }
02753
02754
02755 poseTmp.mult(targetFrameTrans, poseTmp);
02756 poseTmp.frame_id_ = finalTargetFrame;
02757
02758
02759 measurement(StateMemberX) = poseTmp.getOrigin().x();
02760 measurement(StateMemberY) = poseTmp.getOrigin().y();
02761 measurement(StateMemberZ) = poseTmp.getOrigin().z();
02762
02763
02764 double roll, pitch, yaw;
02765 RosFilterUtilities::quatToRPY(poseTmp.getRotation(), roll, pitch, yaw);
02766 measurement(StateMemberRoll) = roll;
02767 measurement(StateMemberPitch) = pitch;
02768 measurement(StateMemberYaw) = yaw;
02769
02770 measurementCovariance.block(0, 0, POSE_SIZE, POSE_SIZE) = covarianceRotated.block(0, 0, POSE_SIZE, POSE_SIZE);
02771
02772
02773 if (twoDMode_)
02774 {
02775 forceTwoD(measurement, measurementCovariance, updateVector);
02776 }
02777
02778 retVal = true;
02779 }
02780 }
02781 else
02782 {
02783 retVal = false;
02784
02785 RF_DEBUG("Could not transform measurement into " << finalTargetFrame << ". Ignoring...");
02786 }
02787
02788 RF_DEBUG("\n----- /RosFilter::preparePose (" << topicName << ") ------\n");
02789
02790 return retVal;
02791 }
02792
02793 template<typename T>
02794 bool RosFilter<T>::prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
02795 const std::string &topicName,
02796 const std::string &targetFrame,
02797 std::vector<int> &updateVector,
02798 Eigen::VectorXd &measurement,
02799 Eigen::MatrixXd &measurementCovariance)
02800 {
02801 RF_DEBUG("------ RosFilter::prepareTwist (" << topicName << ") ------\n");
02802
02803
02804 tf2::Vector3 twistLin(msg->twist.twist.linear.x,
02805 msg->twist.twist.linear.y,
02806 msg->twist.twist.linear.z);
02807 tf2::Vector3 measTwistRot(msg->twist.twist.angular.x,
02808 msg->twist.twist.angular.y,
02809 msg->twist.twist.angular.z);
02810
02811
02812
02813
02814
02815 const Eigen::VectorXd &state = filter_.getState();
02816 tf2::Vector3 stateTwistRot(state(StateMemberVroll),
02817 state(StateMemberVpitch),
02818 state(StateMemberVyaw));
02819
02820
02821 std::string msgFrame = (msg->header.frame_id == "" ? targetFrame : msg->header.frame_id);
02822
02823
02824
02825
02826
02827
02828
02829
02830
02831 tf2::Matrix3x3 maskLin(updateVector[StateMemberVx], 0, 0,
02832 0, updateVector[StateMemberVy], 0,
02833 0, 0, updateVector[StateMemberVz]);
02834
02835 tf2::Matrix3x3 maskRot(updateVector[StateMemberVroll], 0, 0,
02836 0, updateVector[StateMemberVpitch], 0,
02837 0, 0, updateVector[StateMemberVyaw]);
02838
02839
02840 Eigen::MatrixXd covarianceRotated(TWIST_SIZE, TWIST_SIZE);
02841 covarianceRotated.setZero();
02842
02843 copyCovariance(&(msg->twist.covariance[0]),
02844 covarianceRotated,
02845 topicName,
02846 updateVector,
02847 POSITION_V_OFFSET,
02848 TWIST_SIZE);
02849
02850 RF_DEBUG("Original measurement as tf object:\nLinear: " << twistLin <<
02851 "Rotational: " << measTwistRot <<
02852 "\nOriginal update vector:\n" << updateVector <<
02853 "\nOriginal covariance matrix:\n" << covarianceRotated << "\n");
02854
02855
02856 tf2::Transform targetFrameTrans;
02857 bool canTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_,
02858 targetFrame,
02859 msgFrame,
02860 msg->header.stamp,
02861 tfTimeout_,
02862 targetFrameTrans);
02863
02864 if (canTransform)
02865 {
02866
02867
02868 measTwistRot = targetFrameTrans.getBasis() * measTwistRot;
02869 twistLin = targetFrameTrans.getBasis() * twistLin + targetFrameTrans.getOrigin().cross(stateTwistRot);
02870 maskLin = targetFrameTrans.getBasis() * maskLin;
02871 maskRot = targetFrameTrans.getBasis() * maskRot;
02872
02873
02874 updateVector[StateMemberVx] = static_cast<int>(
02875 maskLin.getRow(StateMemberVx - POSITION_V_OFFSET).length() >= 1e-6);
02876 updateVector[StateMemberVy] = static_cast<int>(
02877 maskLin.getRow(StateMemberVy - POSITION_V_OFFSET).length() >= 1e-6);
02878 updateVector[StateMemberVz] = static_cast<int>(
02879 maskLin.getRow(StateMemberVz - POSITION_V_OFFSET).length() >= 1e-6);
02880 updateVector[StateMemberVroll] = static_cast<int>(
02881 maskRot.getRow(StateMemberVroll - ORIENTATION_V_OFFSET).length() >= 1e-6);
02882 updateVector[StateMemberVpitch] = static_cast<int>(
02883 maskRot.getRow(StateMemberVpitch - ORIENTATION_V_OFFSET).length() >= 1e-6);
02884 updateVector[StateMemberVyaw] = static_cast<int>(
02885 maskRot.getRow(StateMemberVyaw - ORIENTATION_V_OFFSET).length() >= 1e-6);
02886
02887 RF_DEBUG(msg->header.frame_id << "->" << targetFrame << " transform:\n" << targetFrameTrans <<
02888 "\nAfter applying transform to " << targetFrame << ", update vector is:\n" << updateVector <<
02889 "\nAfter applying transform to " << targetFrame << ", measurement is:\n" <<
02890 "Linear: " << twistLin << "Rotational: " << measTwistRot << "\n");
02891
02892
02893
02894
02895
02896 tf2::Matrix3x3 rot(targetFrameTrans.getRotation());
02897 Eigen::MatrixXd rot6d(TWIST_SIZE, TWIST_SIZE);
02898 rot6d.setIdentity();
02899
02900 for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
02901 {
02902 rot6d(rInd, 0) = rot.getRow(rInd).getX();
02903 rot6d(rInd, 1) = rot.getRow(rInd).getY();
02904 rot6d(rInd, 2) = rot.getRow(rInd).getZ();
02905 rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
02906 rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
02907 rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
02908 }
02909
02910
02911 covarianceRotated = rot6d * covarianceRotated.eval() * rot6d.transpose();
02912
02913 RF_DEBUG("Transformed covariance is \n" << covarianceRotated << "\n");
02914
02915
02916 measurement(StateMemberVx) = twistLin.getX();
02917 measurement(StateMemberVy) = twistLin.getY();
02918 measurement(StateMemberVz) = twistLin.getZ();
02919 measurement(StateMemberVroll) = measTwistRot.getX();
02920 measurement(StateMemberVpitch) = measTwistRot.getY();
02921 measurement(StateMemberVyaw) = measTwistRot.getZ();
02922
02923
02924 measurementCovariance.block(POSITION_V_OFFSET, POSITION_V_OFFSET, TWIST_SIZE, TWIST_SIZE) =
02925 covarianceRotated.block(0, 0, TWIST_SIZE, TWIST_SIZE);
02926
02927
02928 if (twoDMode_)
02929 {
02930 forceTwoD(measurement, measurementCovariance, updateVector);
02931 }
02932 }
02933 else
02934 {
02935 RF_DEBUG("Could not transform measurement into " << targetFrame << ". Ignoring...");
02936 }
02937
02938 RF_DEBUG("\n----- /RosFilter::prepareTwist (" << topicName << ") ------\n");
02939
02940 return canTransform;
02941 }
02942
02943 template<typename T>
02944 void RosFilter<T>::saveFilterState(FilterBase& filter)
02945 {
02946 FilterStatePtr state = FilterStatePtr(new FilterState());
02947 state->state_ = Eigen::VectorXd(filter.getState());
02948 state->estimateErrorCovariance_ = Eigen::MatrixXd(filter.getEstimateErrorCovariance());
02949 state->lastMeasurementTime_ = filter.getLastMeasurementTime();
02950 state->latestControl_ = Eigen::VectorXd(filter.getControl());
02951 state->latestControlTime_ = filter.getControlTime();
02952 filterStateHistory_.push_back(state);
02953 RF_DEBUG("Saved state with timestamp " << std::setprecision(20) << state->lastMeasurementTime_ <<
02954 " to history. " << filterStateHistory_.size() << " measurements are in the queue.\n");
02955 }
02956
02957 template<typename T>
02958 bool RosFilter<T>::revertTo(const double time)
02959 {
02960 RF_DEBUG("\n----- RosFilter::revertTo -----\n");
02961 RF_DEBUG("\nRequested time was " << std::setprecision(20) << time << "\n")
02962
02963
02964
02965
02966 FilterStatePtr lastHistoryState;
02967 while (!filterStateHistory_.empty() && filterStateHistory_.back()->lastMeasurementTime_ > time)
02968 {
02969 lastHistoryState = filterStateHistory_.back();
02970 filterStateHistory_.pop_back();
02971 }
02972
02973
02974
02975 bool retVal = false;
02976 if (!filterStateHistory_.empty())
02977 {
02978 retVal = true;
02979 lastHistoryState = filterStateHistory_.back();
02980 }
02981 else
02982 {
02983 RF_DEBUG("Insufficient history to revert to time " << time << "\n");
02984
02985 if (lastHistoryState.get() != NULL)
02986 {
02987 RF_DEBUG("Will revert to oldest state at " << lastHistoryState->latestControlTime_ << ".\n");
02988 }
02989 }
02990
02991
02992 if (lastHistoryState.get() != NULL)
02993 {
02994
02995 const FilterStatePtr &state = lastHistoryState;
02996 filter_.setState(state->state_);
02997 filter_.setEstimateErrorCovariance(state->estimateErrorCovariance_);
02998 filter_.setLastMeasurementTime(state->lastMeasurementTime_);
02999
03000 RF_DEBUG("Reverted to state with time " << std::setprecision(20) << state->lastMeasurementTime_ << "\n");
03001 }
03002
03003
03004 int restored_measurements = 0;
03005 while (!measurementHistory_.empty() && measurementHistory_.back()->time_ > time)
03006 {
03007 measurementQueue_.push(measurementHistory_.back());
03008 measurementHistory_.pop_back();
03009 restored_measurements++;
03010 }
03011
03012 RF_DEBUG("Restored " << restored_measurements << " to measurement queue.\n");
03013
03014 RF_DEBUG("\n----- /RosFilter::revertTo\n");
03015
03016 return retVal;
03017 }
03018
03019 template<typename T>
03020 void RosFilter<T>::clearExpiredHistory(const double cutOffTime)
03021 {
03022 RF_DEBUG("\n----- RosFilter::clearExpiredHistory -----" <<
03023 "\nCutoff time is " << cutOffTime << "\n");
03024
03025 int poppedMeasurements = 0;
03026 int poppedStates = 0;
03027
03028 while (!measurementHistory_.empty() && measurementHistory_.front()->time_ < cutOffTime)
03029 {
03030 measurementHistory_.pop_front();
03031 poppedMeasurements++;
03032 }
03033
03034 while (!filterStateHistory_.empty() && filterStateHistory_.front()->lastMeasurementTime_ < cutOffTime)
03035 {
03036 filterStateHistory_.pop_front();
03037 poppedStates++;
03038 }
03039
03040 RF_DEBUG("\nPopped " << poppedMeasurements << " measurements and " <<
03041 poppedStates << " states from their respective queues." <<
03042 "\n---- /RosFilter::clearExpiredHistory ----\n");
03043 }
03044 }
03045
03046
03047
03048 template class RobotLocalization::RosFilter<RobotLocalization::Ekf>;
03049 template class RobotLocalization::RosFilter<RobotLocalization::Ukf>;