Ejemplo n.º 1
0
  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_));
  }
Ejemplo n.º 2
0
Pose2d add_noise(Pose2d in, double sigma) {
  Pose2d out (in.x()+sigma*snrv(),
              in.y()+sigma*snrv(),
              in.t()+sigma*snrv());
  return out;
}