Amcl Improvements

Amcl update:

Main update is the calculation of the particle mean when the resampling is not done (but motion and observation is integrated) - so that amcl publishes corrections every time an observation is integrated. 

Also added drawing of the corrected laser points (for use when comparing with ground truth). 
I would say leave it for now - might be useful at somepoint.


On Thu, Aug 14, 2014 at 8:37 PM, Dejan Pangercic <notifications@github.com>
wrote:

> In amcl/src/amcl_node.cpp:
>
> > +      tf::Transform test_base_tf(tf::createQuaternionFromYaw(yaw),
> > +                                 tf::Vector3(map_pose.getOrigin().x(),
> > +                                             map_pose.getOrigin().y(),
> > +                                             0.0));
> > +
> > +      tf::Stamped<tf::Pose> test_base_to_map(test_base_tf,
> > +                                             laser_scan->header.stamp,
> > +                                             test_frame_id_);
> > +      ros::Duration test_transform_tolerance_(0.01);
> > +      ros::Time transform_expiration = (laser_scan->header.stamp +
> > +                                        test_transform_tolerance_);
> > +      tf::StampedTransform test_base_tf_stamped(test_base_to_map,
> > +                                                transform_expiration,
> > +                                                global_frame_id_, test_frame_id_);
> > +      this->tfb_->sendTransform(test_base_tf_stamped);
> > +    }
>
> How about this, is this still needed?
>
> —
> Reply to this email directly or view it on GitHub
> <https://github.com/bosch-ros-pkg/navigation/pull/12/files#r16279132>.
>