Inounx's profile - activity

@Lofti_Z Hi, I didn't use the ICP in the end, check the https://github.com/dejanpan/snap_map_icp package, it should give

Back then I worked around this problem by manually saving a laser scan and when receiving an AMCL pose, match with the l

@adityakamath Hi, my email is in the package.xml in the archive you downloaded. @linusnie Hi, It is written in the fist

Here are the last modifications I did to improve this localization system, as I think it is not useful to let this post

Did you check that your "sizeof(msg)" returns the value you want ? it returns a size in bytes not the number of elements in array. It could lead to an out-of-bound access

Hi all,

Context

In order to assess the performance of AMCL, I am trying to synchronize two topics :

• The estimated pose from AMCL output

• The scan from the laser range

  /scan topic is published at 25Hz

  /localizer_pose topic is published at a variable frequency depending on the robot's movement (Max is about 3Hz).

The topics are not synchronized, as AMCL introduce a variable delay. As the /localizer_pose topic is published at a low frequency, I do not want to lose any pose message, or at worse very few.

I use a bagfile to replay the scenario to be able to compare different solutions. I also added a simple subscriber on the /localizer_pose topic, to get the total amount of pose received.

ROS Indigo, with Ubuntu Server 14.04.

Test1: Message filters, exact time

First test, add a TimeSynchronizer to see if an exact timestamp match could do the trick.

//Declarations
message_filters::Subscriber<sensor_msgs::LaserScan>* m_scanSubscriber;
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_localizerPoseMsgFilterSubscriber;
message_filters::TimeSynchronizer<geometry_msgs::PoseWithCovarianceStamped, sensor_msgs::LaserScan>* m_scanLocalizerPoseSynchronizer;

//Callbacks
void MyClass::localizerPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose)
{
    m_localizerPoseCount++;
}

void MyClass::localizerPoseWithScanCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const sensor_msgs::LaserScan::ConstPtr& laserScan)
{
    m_localizerPoseWithScanCount++;
}

//Initialization
m_localizerPoseMsgFilterSubscriber = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(m_nodeHandle, m_localizerPoseInputTopic, 5);
m_scanSubscriber = new message_filters::Subscriber<sensor_msgs::LaserScan>(m_nodeHandle, m_laserScanTopic, 50);

m_scanLocalizerPoseSynchronizer = new message_filters::TimeSynchronizer<geometry_msgs::PoseWithCovarianceStamped, sensor_msgs::LaserScan>(*m_localizerPoseMsgFilterSubscriber, *m_scanSubscriber, 10);
m_scanLocalizerPoseSynchronizer->registerCallback(boost::bind(&MyClass::localizerPoseWithScanCallback, this, _1, _2));

Results: I get 95 pairs for 203 poses, pretty bad, as expected.

Test2: Message filters, approximate time

That's the best candidate for now.

//Declarations
message_filters::Subscriber<sensor_msgs::LaserScan>* m_scanSubscriber;
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_localizerPoseMsgFilterSubscriber;
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PoseWithCovarianceStamped, sensor_msgs::LaserScan> SyncScanWithLocalizerPose;
message_filters::Synchronizer<SyncScanWithLocalizerPose>* m_scanLocalizerPoseSynchronizer;

//Callbacks
void MyClass::localizerPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose)
{
    m_localizerPoseCount++;
}

void MyClass::localizerPoseWithScanCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const sensor_msgs::LaserScan::ConstPtr& laserScan)
{
    m_localizerPoseWithScanCount++;
}

//Initialization
m_localizerPoseMsgFilterSubscriber = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(m_nodeHandle, m_localizerPoseInputTopic, 5);
m_scanSubscriber = new message_filters::Subscriber<sensor_msgs::LaserScan>(m_nodeHandle, m_laserScanTopic, 50);

m_scanLocalizerPoseSynchronizer = new message_filters::Synchronizer<SyncScanWithLocalizerPose>(SyncScanWithLocalizerPose(10), *m_localizerPoseMsgFilterSubscriber, *m_scanSubscriber);
m_scanLocalizerPoseSynchronizer->registerCallback(&MyClass::localizerPoseWithScanCallback, this);

Results: Unfortunately, the results are not better: 95 pairs for 204 poses. Another run gave me 84 pairs for 194 total poses.

Test2: Tuning

I tried with AMCL tf_tolerance at 0 and at 0.1 (this is the default value), no difference.

I also tried to set the inter-message lower bound (parameter of ApproximateTime policy) to 0.02 (Half a period of laser scan) and to set the maximum interval duration (parameter of ApproximateTime policy) to 0.15 (maximum allowable time difference between pose and scan ?), but no difference.

So far, the approximate time didn't gave me good results, so I am wondering what I am doing wrong ? According to the documentation I would expect to not loose any pose message, and without doing too much parameter tuning.

Any thoughts ?

Thanks

Check the warning in "Global Status/ Fixed Frame" it seems that you do not publish the tranform from rgbd_camera_optical_frame to your base frame, It could explain why you cannot see anything. Are you using a transform publisher ?

Yes exactly, what is displayed here is the output of the global ekf_localization node. I am trying to find a good way to display with rviz when amcl pose is fused, and when it's not (color change?), as the transition between the two is not always clear.

Ok, it's much clearer now. With a few tests and a process noise covariance at 0.0001 in the diagonal values, I get pretty good results. I will update my post as soon as I have some nice screenshots.

Thanks for your answer! If I understand correctly, what your are saying is that the process noise covariance Q is added during the state estimation process, so the values in this matrix must be choosen according to the frequency parameter ?

I am using robot_state_publisher with an urdf model. The default parameters are used, so use_tf_static is false. I didn't heard about tf2 before, so yes I guess, I am using regular tf. I will look into it.

I have some warnings from both robot_localization nodes : Transform from imu to base_link was unavailable for the time requested. Using latest instead. I think it comes from the non-synchronization between /odom /imu_data and /amcl_pose, but maybe I am wrong ?

Concerning Xacro the package is ros-indigo-xacro

I have the same environment than yours (Ubuntu 14.04 / ROS Indigo) and to install Gazebo, I just installed the version provided with ROS. I currently have ros-indigo-gazebo-msgs ros-indigo-gazebo-plugins ros-indigo-gazebo-ros ros-indigo-gazebo-ros-control ros-indigo-gazebo-ros-pkgs installed.

You're welcome ;). I don't understand why you ask this, the version you have is not ok ?