void laserCb(const sensor_msgs::LaserScan &scan) {
// No odom estimate
bool map_change = matcher_.addScan(Pose2d(0.0, 0.0, 0.0), scan);
if (debug_) {
if (map_change) {
pmap_.publish(matcher_.map().occGrid());
}
}
odom_.header.stamp = scan.header.stamp;
tf::poseTFToMsg(matcher_.pose().tf(), odom_.pose.pose);
// Get velocity estimate
if (!have_pose_) {
last_pose_ = matcher_.pose();
have_pose_ = true;
} else {
double dt = (scan.header.stamp - last_pose_time_).toSec();
if (dt > 0.2) {
Pose2d change = matcher_.pose().ominus(last_pose_);
odom_.twist.twist.linear.x = change.x() / dt;
odom_.twist.twist.linear.y = change.y() / dt;
odom_.twist.twist.angular.z = change.t() / dt;
last_pose_time_ = scan.header.stamp;
last_pose_ = matcher_.pose();
}
}
podom_.publish(odom_);
tf_.sendTransform(
tf::StampedTransform(matcher_.pose().tf(), scan.header.stamp,
odom_frame_, base_frame_));
}
Pose2d add_noise(Pose2d in, double sigma) {
Pose2d out (in.x()+sigma*snrv(),
in.y()+sigma*snrv(),
in.t()+sigma*snrv());
return out;
}