Skip to content

WIP: Refactor and correct prediction code and transforms. #1

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

Open
malban wants to merge 2 commits into
base: ros1
Choose a base branch
from
Open

WIP: Refactor and correct prediction code and transforms. #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3113d4a
Refactor and correct prediction code and transforms.
malban Jul 13, 2022
4ad6c92
Catch tf exceptions by reference.
malban Jul 16, 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
30 changes: 20 additions & 10 deletions laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,8 @@ class LaserScanMatcher
tf::TransformListener tf_listener_;
tf::TransformBroadcaster tf_broadcaster_;

tf::Transform base_to_laser_; // static, cached
tf::Transform laser_to_base_; // static, cached, calculated from base_to_laser_
tf::Transform base_from_laser_; // static, cached
tf::Transform laser_from_base_; // static, cached

ros::Publisher pose_publisher_;
ros::Publisher pose_stamped_publisher_;
Expand Down Expand Up @@ -120,11 +120,14 @@ class LaserScanMatcher
// **** What predictions are available to speed up the ICP?
// 1) imu - [theta] from imu yaw angle - /imu topic
// 2) odom - [x, y, theta] from wheel odometry - /odom topic
// 3) tf - [x, y, theta] from /tf topic
// 3) velocity [vx, vy, vtheta], usually from ab-filter - /vel.
// If more than one is enabled, priority is imu > odom > velocity
// If more than one is enabled, priority is tf > imu > odom > velocity

bool use_imu_;
bool use_odom_;
bool use_tf_;
double tf_timeout_;
bool use_vel_;
bool stamped_vel_;

Expand All @@ -137,15 +140,15 @@ class LaserScanMatcher
bool received_odom_;
bool received_vel_;

tf::Transform f2b_; // fixed-to-base tf (pose of base frame in fixed frame)
tf::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame
tf::Transform last_base_in_fixed_; // pose of the base of the last scan in fixed frame
tf::Transform keyframe_base_in_fixed_; // pose of the base of last keyframe scan in fixed frame

ros::Time last_icp_time_;

sensor_msgs::Imu latest_imu_msg_;
sensor_msgs::Imu last_used_imu_msg_;
tf::Quaternion last_used_imu_orientation_;
nav_msgs::Odometry latest_odom_msg_;
nav_msgs::Odometry last_used_odom_msg_;
tf::Transform last_used_odom_pose_;

geometry_msgs::Twist latest_vel_msg_;

Expand Down Expand Up @@ -175,12 +178,19 @@ class LaserScanMatcher
void velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg);

void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
bool getBaseToLaserTf (const std::string& frame_id);

/**
* Cache the static transform between the base and laser.
*
* @param[in] frame_id The laser frame.
*
* @returns True if the transform was found, false otherwise.
*/
bool getBaseLaserTransform(const std::string& frame_id);

bool newKeyframeNeeded(const tf::Transform& d);

void getPrediction(double& pr_ch_x, double& pr_ch_y,
double& pr_ch_a, double dt);
tf::Transform getPrediction(const ros::Time& stamp);

void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t);
};
Expand Down
Loading