Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix odometry msgs with child frame other than baseLink #728

Merged
merged 5 commits into from
Apr 25, 2022
Merged

Fix odometry msgs with child frame other than baseLink #728

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
23c742f
Fix odometry msgs with child frame other than baseLink
Feb 15, 2022
74dd50c
Fix format
Feb 18, 2022
8fc1c87
Gate pose:child_frame->header.frame_id behavior through parameter
Mar 9, 2022
e181f78
Change parameter names, e.g., diff_rel_use_child_frame -> pose_use_ch…
Apr 11, 2022
b447a50
Fix style according to linter
Apr 12, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions include/robot_localization/ros_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,12 +82,14 @@ struct CallbackData
const int updateSum,
const bool differential,
const bool relative,
const bool pose_use_child_frame,
const double rejectionThreshold) :
topicName_(topicName),
updateVector_(updateVector),
updateSum_(updateSum),
differential_(differential),
relative_(relative),
pose_use_child_frame_(pose_use_child_frame),
rejectionThreshold_(rejectionThreshold)
{
}
Expand All @@ -97,6 +99,7 @@ struct CallbackData
int updateSum_;
bool differential_;
bool relative_;
bool pose_use_child_frame_;
double rejectionThreshold_;
};

Expand Down Expand Up @@ -245,11 +248,13 @@ template<class T> class RosFilter
//! @param[in] msg - The ROS stamped pose with covariance message to take in
//! @param[in] callbackData - Relevant static callback data
//! @param[in] targetFrame - The target frame_id into which to transform the data
//! @param[in] poseSourceFrame - The source frame_id from which to transform the data
//! @param[in] imuData - Whether this data comes from an IMU
//!
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
const CallbackData &callbackData,
const std::string &targetFrame,
const std::string &poseSourceFrame,
const bool imuData);

//! @brief Callback method for manually setting/resetting the internal pose estimate
Expand Down Expand Up @@ -384,6 +389,7 @@ template<class T> class RosFilter
//! @param[in] msg - The pose message to prepare
//! @param[in] topicName - The name of the topic over which this message was received
//! @param[in] targetFrame - The target tf frame
//! @param[in] sourceFrame - The source tf frame
//! @param[in] differential - Whether we're carrying out differential integration
//! @param[in] relative - Whether this measurement is processed relative to the first
//! @param[in] imuData - Whether this measurement is from an IMU
Expand All @@ -395,6 +401,7 @@ template<class T> class RosFilter
bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
const std::string &topicName,
const std::string &targetFrame,
const std::string &sourceFrame,
const bool differential,
const bool relative,
const bool imuData,
Expand Down
67 changes: 51 additions & 16 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -525,10 +525,11 @@ namespace RobotLocalization

// IMU data gets handled a bit differently, since the message is ambiguous and has only a single frame_id,
// even though the data in it is reported in two different frames. As we assume users will specify a base_link
// to imu transform, we make the target frame baseLinkFrameId_ and tell the poseCallback that it is working
// with IMU data. This will cause it to apply different logic to the data.
// to imu transform, we make the target and child frame baseLinkFrameId_ and tell the poseCallback that it is
// working with IMU data. This will cause it to apply different logic to the data.
geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
poseCallback(pptr, poseCallbackData, baseLinkFrameId_, true);
poseCallback(pptr, poseCallbackData, baseLinkFrameId_,
baseLinkFrameId_, true);
}
}

Expand Down Expand Up @@ -1065,7 +1066,11 @@ namespace RobotLocalization
relative = false;
}

std::string odomTopic;
// Consider odometry transformation from the child_frame_id instead of the base_link_frame_id
frygge marked this conversation as resolved.
Show resolved Hide resolved
bool pose_use_child_frame;
nhLocal_.param(odomTopicName + std::string("_pose_use_child_frame"), pose_use_child_frame, false);

std::string odomTopic;
nhLocal_.getParam(odomTopicName, odomTopic);

// Check for pose rejection threshold
Expand Down Expand Up @@ -1095,9 +1100,9 @@ namespace RobotLocalization
nhLocal_.param(odomTopicName + "_queue_size", odomQueueSize, 1);

const CallbackData poseCallbackData(odomTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential,
relative, poseMahalanobisThresh);
relative, pose_use_child_frame, poseMahalanobisThresh);
const CallbackData twistCallbackData(odomTopicName + "_twist", twistUpdateVec, twistUpdateSum, false, false,
twistMahalanobisThresh);
false, twistMahalanobisThresh);

bool nodelayOdom = false;
nhLocal_.param(odomTopicName + "_nodelay", nodelayOdom, false);
Expand Down Expand Up @@ -1220,11 +1225,11 @@ namespace RobotLocalization
if (poseUpdateSum > 0)
{
const CallbackData callbackData(poseTopicName, poseUpdateVec, poseUpdateSum, differential, relative,
poseMahalanobisThresh);
false, poseMahalanobisThresh);

topicSubs_.push_back(
nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>(poseTopic, poseQueueSize,
boost::bind(&RosFilter::poseCallback, this, _1, callbackData, worldFrameId_, false),
boost::bind(&RosFilter::poseCallback, this, _1, callbackData, worldFrameId_, baseLinkFrameId_, false),
ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayPose)));

if (differential)
Expand Down Expand Up @@ -1297,7 +1302,7 @@ namespace RobotLocalization
if (twistUpdateSum > 0)
{
const CallbackData callbackData(twistTopicName, twistUpdateVec, twistUpdateSum, false, false,
twistMahalanobisThresh);
false, twistMahalanobisThresh);

topicSubs_.push_back(
nh_.subscribe<geometry_msgs::TwistWithCovarianceStamped>(twistTopic, twistQueueSize,
Expand Down Expand Up @@ -1463,11 +1468,11 @@ namespace RobotLocalization
if (poseUpdateSum + twistUpdateSum + accelUpdateSum > 0)
{
const CallbackData poseCallbackData(imuTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential,
relative, poseMahalanobisThresh);
relative, false, poseMahalanobisThresh);
const CallbackData twistCallbackData(imuTopicName + "_twist", twistUpdateVec, twistUpdateSum, differential,
relative, twistMahalanobisThresh);
relative, false, twistMahalanobisThresh);
const CallbackData accelCallbackData(imuTopicName + "_acceleration", accelUpdateVec, accelUpdateSum,
differential, relative, accelMahalanobisThresh);
differential, relative, false, accelMahalanobisThresh);

topicSubs_.push_back(
nh_.subscribe<sensor_msgs::Imu>(imuTopic, imuQueueSize,
Expand Down Expand Up @@ -1741,7 +1746,14 @@ namespace RobotLocalization
posPtr->pose = msg->pose; // Entire pose object, also copies covariance

geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
poseCallback(pptr, poseCallbackData, worldFrameId_, false);
if (poseCallbackData.pose_use_child_frame_)
{
poseCallback(pptr, poseCallbackData, worldFrameId_, msg->child_frame_id, false);
}
else
{
poseCallback(pptr, poseCallbackData, worldFrameId_, baseLinkFrameId_, false);
}
}

if (twistCallbackData.updateSum_ > 0)
Expand All @@ -1763,6 +1775,7 @@ namespace RobotLocalization
void RosFilter<T>::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
const CallbackData &callbackData,
const std::string &targetFrame,
const std::string &poseSourceFrame,
const bool imuData)
{
const std::string &topicName = callbackData.topicName_;
Expand Down Expand Up @@ -1809,6 +1822,7 @@ namespace RobotLocalization
if (preparePose(msg,
topicName,
targetFrame,
poseSourceFrame,
callbackData.differential_,
callbackData.relative_,
imuData,
Expand Down Expand Up @@ -2070,7 +2084,9 @@ namespace RobotLocalization

// Prepare the pose data (really just using this to transform it into the target frame).
// Twist data is going to get zeroed out.
preparePose(msg, topicName, worldFrameId_, false, false, false, updateVector, measurement, measurementCovariance);
// Since pose messages do not provide a child_frame_id, it defaults to baseLinkFrameId_
preparePose(msg, topicName, worldFrameId_, baseLinkFrameId_, false, false,
false, updateVector, measurement, measurementCovariance);

// For the state
filter_.setState(measurement);
Expand Down Expand Up @@ -2564,6 +2580,7 @@ namespace RobotLocalization
bool RosFilter<T>::preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
const std::string &topicName,
const std::string &targetFrame,
const std::string &sourceFrame,
const bool differential,
const bool relative,
const bool imuData,
Expand Down Expand Up @@ -2861,7 +2878,7 @@ namespace RobotLocalization
// 7d. Fill out the velocity data in the message
geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
twistPtr->header = msg->header;
twistPtr->header.frame_id = baseLinkFrameId_;
twistPtr->header.frame_id = sourceFrame;
twistPtr->twist.twist.linear.x = xVel;
twistPtr->twist.twist.linear.y = yVel;
twistPtr->twist.twist.linear.z = zVel;
Expand Down Expand Up @@ -2889,7 +2906,7 @@ namespace RobotLocalization
// Now pass this on to prepareTwist, which will convert it to the required frame
success = prepareTwist(ptr,
topicName + "_twist",
twistPtr->header.frame_id,
baseLinkFrameId_,
updateVector,
measurement,
measurementCovariance);
Expand All @@ -2903,6 +2920,24 @@ namespace RobotLocalization
}
else
{
// make pose refer to the baseLinkFrame as source
if ( sourceFrame != baseLinkFrameId_)
{
tf2::Transform sourceFrameTrans;
bool canSrcTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_,
sourceFrame,
baseLinkFrameId_,
poseTmp.stamp_,
tfTimeout_,
sourceFrameTrans,
tfSilentFailure_);

if (canSrcTransform)
{
poseTmp.setData(poseTmp * sourceFrameTrans);
}
}

// 7g. If we're in relative mode, remove the initial measurement
if (relative)
{
Expand Down