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