33 #ifndef ROBOT_LOCALIZATION_ROS_FILTER_H 34 #define ROBOT_LOCALIZATION_ROS_FILTER_H 40 #include <robot_localization/SetPose.h> 41 #include <robot_localization/ToggleFilterProcessing.h> 44 #include <std_msgs/String.h> 45 #include <std_srvs/Empty.h> 46 #include <nav_msgs/Odometry.h> 47 #include <sensor_msgs/Imu.h> 48 #include <geometry_msgs/Twist.h> 49 #include <geometry_msgs/TwistStamped.h> 50 #include <geometry_msgs/TwistWithCovarianceStamped.h> 51 #include <geometry_msgs/PoseWithCovarianceStamped.h> 52 #include <geometry_msgs/AccelWithCovarianceStamped.h> 60 #include <diagnostic_msgs/DiagnosticStatus.h> 64 #include <Eigen/Dense> 80 const std::vector<int> &updateVector,
82 const bool differential,
84 const double rejectionThreshold) :
114 explicit RosFilter(std::vector<double>
args = std::vector<double>());
130 bool toggleFilterProcessingCallback(robot_localization::ToggleFilterProcessing::Request&,
131 robot_localization::ToggleFilterProcessing::Response&);
138 void accelerationCallback(
const sensor_msgs::Imu::ConstPtr &
msg,
140 const std::string &targetFrame);
145 void controlCallback(
const geometry_msgs::Twist::ConstPtr &msg);
150 void controlCallback(
const geometry_msgs::TwistStamped::ConstPtr &msg);
161 void enqueueMeasurement(
const std::string &topicName,
162 const Eigen::VectorXd &measurement,
163 const Eigen::MatrixXd &measurementCovariance,
164 const std::vector<int> &updateVector,
165 const double mahalanobisThresh,
177 void forceTwoD(Eigen::VectorXd &measurement,
178 Eigen::MatrixXd &measurementCovariance,
179 std::vector<int> &updateVector);
185 bool getFilteredOdometryMessage(nav_msgs::Odometry &message);
191 bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message);
203 void imuCallback(
const sensor_msgs::Imu::ConstPtr &msg,
const std::string &topicName,
211 void integrateMeasurements(
const ros::Time ¤tTime);
226 void odometryCallback(
const nav_msgs::Odometry::ConstPtr &msg,
const std::string &topicName,
235 void poseCallback(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
237 const std::string &targetFrame,
247 void setPoseCallback(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
253 bool setPoseSrvCallback(robot_localization::SetPose::Request& request,
254 robot_localization::SetPose::Response&);
260 bool enableFilterSrvCallback(std_srvs::Empty::Request&,
261 std_srvs::Empty::Response&);
268 void twistCallback(
const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
270 const std::string &targetFrame);
276 bool validateFilterOutput(
const nav_msgs::Odometry &message);
287 bool revertTo(
const double time);
299 void clearExpiredHistory(
const double cutoffTime);
303 void clearMeasurementQueue();
311 void addDiagnostic(
const int errLevel,
312 const std::string &topicAndClass,
313 const std::string &message,
314 const bool staticDiag);
333 void copyCovariance(
const double *arr,
334 Eigen::MatrixXd &covariance,
335 const std::string &topicName,
336 const std::vector<int> &updateVector,
338 const size_t dimension);
347 void copyCovariance(
const Eigen::MatrixXd &covariance,
349 const size_t dimension);
355 std::vector<int> loadUpdateConfig(
const std::string &topicName);
365 bool prepareAcceleration(
const sensor_msgs::Imu::ConstPtr &msg,
366 const std::string &topicName,
367 const std::string &targetFrame,
368 std::vector<int> &updateVector,
369 Eigen::VectorXd &measurement,
370 Eigen::MatrixXd &measurementCovariance);
384 bool preparePose(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
385 const std::string &topicName,
386 const std::string &targetFrame,
387 const bool differential,
390 std::vector<int> &updateVector,
391 Eigen::VectorXd &measurement,
392 Eigen::MatrixXd &measurementCovariance);
403 bool prepareTwist(
const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
404 const std::string &topicName,
405 const std::string &targetFrame,
406 std::vector<int> &updateVector,
407 Eigen::VectorXd &measurement,
408 Eigen::MatrixXd &measurementCovariance);
677 #endif // ROBOT_LOCALIZATION_ROS_FILTER_H
std::priority_queue< MeasurementPtr, std::vector< MeasurementPtr >, Measurement > MeasurementQueue
CallbackData(const std::string &topicName, const std::vector< int > &updateVector, const int updateSum, const bool differential, const bool relative, const double rejectionThreshold)
bool useControl_
Whether or not we use a control term.
bool publishAcceleration_
Whether we publish the acceleration.
std::vector< int > updateVector_
ros::Subscriber controlSub_
Subscribes to the control input topic.
ros::NodeHandle nhLocal_
Local node handle (for params)
std::map< std::string, Eigen::MatrixXd > previousMeasurementCovariances_
We also need the previous covariance matrix for differential data.
bool resetOnTimeJump_
Whether to reset the filters when backwards jump in time is detected.
MeasurementHistoryDeque measurementHistory_
A deque of previous measurements which is implicitly ordered from earliest to latest measurement...
std::string baseLinkFrameId_
tf frame name for the robot's body frame
std::map< std::string, ros::Time > lastMessageTimes_
Store the last time a message from each topic was received.
ros::Subscriber setPoseSub_
Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWit...
bool enabled_
Whether the filter is enabled or not. See disabledAtStartup_.
ros::ServiceServer toggleFilterProcessingSrv_
Service that allows another node to toggle on/off filter processing while still publishing. Uses a robot_localization ToggleFilterProcessing service.
Eigen::VectorXd latestControl_
The most recent control input.
ros::Duration tfTimeout_
Parameter that specifies the how long we wait for a transform to become available.
Structure used for storing and comparing measurements (for priority queues)
ros::Time latestControlTime_
The time of the most recent control input.
std::vector< ros::Subscriber > topicSubs_
Vector to hold our subscribers until they go out of scope.
bool printDiagnostics_
Whether or not we print diagnostic messages to the /diagnostics topic.
std::string worldFrameId_
tf frame name that is the parent frame of the transform that this node will calculate and broadcast...
int staticDiagErrorLevel_
The max (worst) static diagnostic level.
ros::ServiceServer setPoseSrv_
Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service.
int dynamicDiagErrorLevel_
The max (worst) dynamic diagnostic level.
ros::Time lastSetPoseTime_
Store the last time set pose message was received.
bool publishTransform_
Whether we publish the transform from the world_frame to the base_link_frame.
std::deque< FilterStatePtr > FilterStateHistoryDeque
tf2_ros::TransformListener tfListener_
Transform listener for receiving transforms.
std::map< std::string, tf2::Transform > previousMeasurements_
Stores the last measurement from a given topic for differential integration.
std::string baseLinkOutputFrameId_
tf frame name for the robot's body frame
MeasurementQueue measurementQueue_
We process measurements by queueing them up in callbacks and processing them all at once within each ...
bool disabledAtStartup_
Start the Filter disabled at startup.
std::deque< MeasurementPtr > MeasurementHistoryDeque
ros::Duration tfTimeOffset_
For future (or past) dating the world_frame->base_link_frame transform.
ros::ServiceServer enableFilterSrv_
Service that allows another node to enable the filter. Uses a standard Empty service.
std::map< std::string, std::string > staticDiagnostics_
This object accumulates static diagnostics, e.g., diagnostics relating to the configuration parameter...
bool smoothLaggedData_
Whether or not we use smoothing.
bool toggledOn_
$brief Whether the filter should process new measurements or not.
geometry_msgs::TransformStamped worldBaseLinkTransMsg_
Message that contains our latest transform (i.e., state)
double gravitationalAcc_
What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
std::string mapFrameId_
tf frame name for the robot's map (world-fixed) frame
double rejectionThreshold_
std::map< std::string, tf2::Transform > initialMeasurements_
Stores the first measurement from each topic for relative measurements.
double historyLength_
The depth of the history we track for smoothing/delayed measurement processing.
bool predictToCurrentTime_
By default, the filter predicts and corrects up to the time of the latest measurement. If this is set to true, the filter does the same, but then also predicts up to the current time step.
std::map< std::string, std::string > dynamicDiagnostics_
This object accumulates dynamic diagnostics, e.g., diagnostics relating to sensor data...
tf2_ros::Buffer tfBuffer_
Transform buffer for managing coordinate transforms.
diagnostic_updater::Updater diagnosticUpdater_
Used for updating the diagnostics.
bool twoDMode_
Whether or not we're in 2D mode.
std::string odomFrameId_
tf frame name for the robot's odometry (world-fixed) frame
double frequency_
The frequency of the run loop.
std::ofstream debugStream_
Used for outputting debug messages.
std::vector< std::string > stateVariableNames_
Contains the state vector variable names in string format.
ros::NodeHandle nh_
Node handle.
T filter_
Our filter (EKF, UKF, etc.)
FilterStateHistoryDeque filterStateHistory_
An implicitly time ordered queue of past filter states used for smoothing.
std::map< std::string, bool > removeGravitationalAcc_
If including acceleration for each IMU input, whether or not we remove acceleration due to gravity...