Hi,<div><br></div><div>This is kind of an old issue but we had a similar problem with our custom robot in Gazebo while using a map previously built with gmapping. The amcl node hangs in the loop in amcl_node.cpp between line 457 and 466. For some reason, the randomly generated coordinates in i are out of bounds (more than map->size_x). This locks the entire node and everything that depend on the map -> odom transform. This would happen every time a global localization was triggered. I rewrote the code a little bit to clear the issue :</div>
<div><br></div><div><div><div>pf_vector_t</div><div>AmclNode::uniformPoseGenerator(void* arg)</div><div>{</div><div>  map_t* map = (map_t*)arg;</div><div>  pf_vector_t p;</div><div><br></div><div>  int i, j;</div><div>  for(;;)</div>
<div>  {</div><div>    // Check that it's a free cell</div><div>    i = lrand48() % map->size_x;</div><div>    j = lrand48() % map->size_y; </div><div><br></div><div>    if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))</div>
<div>      break;</div><div>  }</div><div><br></div><div>  p.v[0] = MAP_WXGX(map, i);</div><div>  p.v[1] = MAP_WYGY(map, j);</div><div>  p.v[2] = drand48() * 2 * M_PI - M_PI;</div><div><br></div><div>  return p;</div><div>
}</div></div><div><br></div><div>I didn't really check into what might cause this, however.</div><div><br></div><div>François Ferland</div><br><div class="gmail_quote">On Tue, Nov 2, 2010 at 10:23 AM, Dominik Franěk <span dir="ltr"><<a href="mailto:dominik.franek@gmail.com">dominik.franek@gmail.com</a>></span> wrote:<br>
<blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex;">Hi,<br>
<br>
The amcl parameters are below. As in Torsten's case, it's a likelyhood<br>
field version. Beam model doesn't really work with sensor system I<br>
use, so I haven't tryed that one. It fails even when I tried using the<br>
default param values, and it fails with values I believe to have used<br>
before successfully. As I'm now unable to create a stable settings I<br>
can't say which parameters cause the failure.<br>
There are no messages published on the topics after the crash. There<br>
is no output to the console, below is the end of the logfile.<br>
I have never used stage and unfortunatelly can't try it now.<br>
In few days I'm leaving the country for some time, so I will be unable<br>
to answer more questions after that, I'm sorry. So I hope these<br>
details are of some help.<br>
<br>
Regards, Dominik<br>
<br>
<br>
---- Launch file<br>
<br>
  <param name="odom_model_type" value="diff"/><br>
  <param name="transform_tolerance" value="0.5" /><br>
  <param name="gui_publish_rate" value="10.0"/><br>
  <param name="laser_max_beams" value="55"/><br>
  <param name="min_particles" value="200"/> <!-- was 500 --><br>
  <param name="max_particles" value="500"/> <!-- was 5000 --><br>
  <param name="kld_err" value="0.05"/><br>
  <param name="kld_z" value="0.99"/><br>
  <param name="odom_alpha1" value="1.0"/> <!-- 0.2 --><br>
  <param name="odom_alpha2" value="0.5"/> <!-- 0.2 --><br>
  <param name="odom_alpha3" value="1.0"/> <!-- 0.8 --><br>
  <param name="odom_alpha4" value="0.5"/> <!-- 0.2 --><br>
  <param name="laser_z_hit" value="0.6"/> <!-- 0.5 --><br>
  <param name="laser_z_short" value="0.1"/> <!-- 0.05 --><br>
  <param name="laser_z_max" value="0.15"/> <!-- 0.05 --><br>
  <param name="laser_z_rand" value="0.15"/> <!-- 0.5 --><br>
  <param name="laser_sigma_hit" value="0.2"/><br>
  <param name="laser_lambda_short" value="0.1"/><br>
  <param name="laser_model_type" value="likelihood_field"/>   <!-- beam --><br>
  <param name="laser_likelihood_max_dist" value="2.0"/><br>
  <param name="update_min_d" value="0.1"/><br>
  <param name="update_min_a" value="0.1"/><br>
  <param name="odom_frame_id" value="/odom"/><br>
  <param name="base_frame_id" value="/base_link"/><br>
  <param name="global_frame_id" value="/map"/><br>
  <param name="resample_interval" value="2"/><br>
  <param name="save_pose_rate" value="-1"/><br>
<br>
----- AMCL log<br>
<br>
...<br>
<br>
[ros.amcl] [2010-11-02 15:00:31,328] [thread 0x7f216d0e2760]: [INFO]<br>
Setting pose (1288706431.328592): -0.223 -1.437 -0.212<br>
[roscpp_internal] [2010-11-02 15:03:48,715] [thread 0x7f21653f8710]:<br>
[DEBUG] Accepted connection on socket [7], new socket [17]<br>
[roscpp_internal] [2010-11-02 15:03:48,715] [thread 0x7f21653f8710]:<br>
[DEBUG] TCPROS received a connection from [<a href="http://10.42.43.1:50156" target="_blank">10.42.43.1:50156</a>]<br>
[roscpp_internal] [2010-11-02 15:03:48,715] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection: Creating TransportSubscriberLink for topic<br>
[/amcl_pose] connected to [callerid=[/rostopic_2384_1288706627062]<br>
address=[TCPROS connection to [<a href="http://10.42.43.1:50156" target="_blank">10.42.43.1:50156</a> on socket 17]]]<br>
[roscpp_internal] [2010-11-02 15:03:57,120] [thread 0x7f21653f8710]:<br>
[DEBUG] Socket [11] received 0/4 bytes, closing<br>
[roscpp_internal] [2010-11-02 15:03:57,120] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [11] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,120] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to publisher [TCPROS connection to<br>
[<a href="http://10.42.43.10:48895" target="_blank">10.42.43.10:48895</a> on socket 11]] to topic [/tf] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,186] [thread 0x7f21653f8710]:<br>
[DEBUG] Socket [13] received 0/4 bytes, closing<br>
[roscpp_internal] [2010-11-02 15:03:57,186] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [13] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,186] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to publisher [TCPROS connection to<br>
[<a href="http://10.42.43.10:56896" target="_blank">10.42.43.10:56896</a> on socket 13]] to topic [/scan] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,217] [thread 0x7f2164bf7710]:<br>
[DEBUG] Publisher update for [/tf]: <a href="http://10.42.43.10:34723/" target="_blank">http://10.42.43.10:34723/</a>,<br>
<a href="http://10.42.43.1:45920/" target="_blank">http://10.42.43.1:45920/</a>,  already have these connections:<br>
<a href="http://10.42.43.1:45920/" target="_blank">http://10.42.43.1:45920/</a>, <a href="http://10.42.43.10:53054/" target="_blank">http://10.42.43.10:53054/</a>,<br>
<a href="http://10.42.43.10:34723/" target="_blank">http://10.42.43.10:34723/</a>,<br>
[roscpp_internal] [2010-11-02 15:03:57,217] [thread 0x7f2164bf7710]:<br>
[DEBUG] Disconnecting from publisher [/sensor_link_broadcaster] of<br>
topic [/tf] at [<a href="http://10.42.43.10:53054/" target="_blank">http://10.42.43.10:53054/</a>]<br>
[roscpp_internal] [2010-11-02 15:03:57,287] [thread 0x7f2163bf5710]:<br>
[DEBUG] Retrying connection to [<a href="http://10.42.43.10:56896" target="_blank">10.42.43.10:56896</a>] for topic [/scan]<br>
[roscpp_internal] [2010-11-02 15:03:57,287] [thread 0x7f2163bf5710]:<br>
[DEBUG] Async connect() in progress to [<a href="http://10.42.43.10:56896" target="_blank">10.42.43.10:56896</a>] on socket<br>
[13]<br>
[roscpp_internal] [2010-11-02 15:03:57,302] [thread 0x7f2164bf7710]:<br>
[DEBUG] Publisher update for [/tf]: <a href="http://10.42.43.1:45920/" target="_blank">http://10.42.43.1:45920/</a>,  already<br>
have these connections: <a href="http://10.42.43.1:45920/" target="_blank">http://10.42.43.1:45920/</a>,<br>
<a href="http://10.42.43.10:34723/" target="_blank">http://10.42.43.10:34723/</a>,<br>
[roscpp_internal] [2010-11-02 15:03:57,302] [thread 0x7f2164bf7710]:<br>
[DEBUG] Disconnecting from publisher [/pepa_robot] of topic [/tf] at<br>
[<a href="http://10.42.43.10:34723/" target="_blank">http://10.42.43.10:34723/</a>]<br>
[roscpp_internal] [2010-11-02 15:03:57,302] [thread 0x7f2164bf7710]:<br>
[DEBUG] TCP socket [15] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,530] [thread 0x7f21653f8710]:<br>
[DEBUG] recv() on socket [13] failed with error [Connection reset by<br>
peer]<br>
[roscpp_internal] [2010-11-02 15:03:57,530] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [13] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,530] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to publisher [TCPROS connection to<br>
[<a href="http://10.42.43.10:56896" target="_blank">10.42.43.10:56896</a> on socket 13]] to topic [/scan] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,533] [thread 0x7f2164bf7710]:<br>
[DEBUG] Publisher update for [/scan]:  already have these connections:<br>
<a href="http://10.42.43.10:44973/" target="_blank">http://10.42.43.10:44973/</a>,<br>
[roscpp_internal] [2010-11-02 15:03:57,533] [thread 0x7f2164bf7710]:<br>
[DEBUG] Disconnecting from publisher [/disp_to_laser_transform] of<br>
topic [/scan] at [<a href="http://10.42.43.10:44973/" target="_blank">http://10.42.43.10:44973/</a>]<br>
[roscpp_internal] [2010-11-02 15:03:57,621] [thread 0x7f21653f8710]:<br>
[DEBUG] Shutting down roscpp<br>
[roscpp_internal] [2010-11-02 15:03:57,644] [thread 0x7f21653f8710]:<br>
[DEBUG] Shutting down topics...<br>
[roscpp_internal] [2010-11-02 15:03:57,644] [thread 0x7f21653f8710]:<br>
[DEBUG]   shutting down publishers<br>
[roscpp_internal] [2010-11-02 15:03:57,662] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to subscriber [callerid=[/rosout] address=[TCPROS<br>
connection to [<a href="http://10.42.43.10:47442" target="_blank">10.42.43.10:47442</a> on socket 10]]] to topic [/rosout]<br>
dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,662] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [10] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,725] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to local publisher on topic [/tf] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,725] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to local subscriber on topic [/tf] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,725] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to subscriber [callerid=[/move_base]<br>
address=[TCPROS connection to [<a href="http://10.42.43.1:49837" target="_blank">10.42.43.1:49837</a> on socket 14]]] to<br>
topic [/tf] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,725] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [14] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,725] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to subscriber [callerid=[/rviz_1288706392038486363]<br>
address=[TCPROS connection to [<a href="http://10.42.43.1:49954" target="_blank">10.42.43.1:49954</a> on socket 12]]] to<br>
topic [/tf] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,725] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [12] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,769] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to subscriber<br>
[callerid=[/rostopic_2384_1288706627062] address=[TCPROS connection to<br>
[<a href="http://10.42.43.1:50156" target="_blank">10.42.43.1:50156</a> on socket 17]]] to topic [/amcl_pose] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,770] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [17] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,848] [thread 0x7f21653f8710]:<br>
[DEBUG] Connection to subscriber [callerid=[/rviz_1288706392038486363]<br>
address=[TCPROS connection to [<a href="http://10.42.43.1:49995" target="_blank">10.42.43.1:49995</a> on socket 18]]] to<br>
topic [/particlecloud] dropped<br>
[roscpp_internal] [2010-11-02 15:03:57,848] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [18] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,848] [thread 0x7f21653f8710]:<br>
[DEBUG]   shutting down subscribers<br>
[roscpp_internal] [2010-11-02 15:03:57,975] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [16] closed<br>
[roscpp_internal] [2010-11-02 15:03:57,975] [thread 0x7f21653f8710]:<br>
[DEBUG] ServiceManager::shutdown(): unregistering our advertised<br>
services<br>
[roscpp_internal] [2010-11-02 15:03:58,066] [thread 0x7f21653f8710]:<br>
[DEBUG] UDP socket [8] closed<br>
[roscpp_internal] [2010-11-02 15:03:58,066] [thread 0x7f21653f8710]:<br>
[DEBUG] TCP socket [7] closed<br>
[roscpp_internal] [2010-11-02 15:03:58,066] [thread 0x7f21653f8710]:<br>
[DEBUG] Shutdown finished<br>
<br>
<br>
2010/11/2 Torsten Fiolka <<a href="mailto:fiolka@cs.uni-bonn.de">fiolka@cs.uni-bonn.de</a>>:<br>
<div><div></div><div class="h5">> Hi all,<br>
><br>
> same problem here, AMCL locks up very fast in Stage and sometimes on the<br>
> real robot.<br>
><br>
> My best parameters so far are:<br>
><br>
>  <node pkg="amcl" type="amcl" name="amcl"><br>
>  <remap from="scan" to="laserscan"/><br>
>  <param name="min_particles" value="100"/><br>
>  <param name="max_particles" value="500"/><br>
>  <param name="initial_pose_x" value="-4.5"/><br>
>  <param name="initial_pose_y" value="-0.5"/><br>
>  <param name="laser_max_beams" value="30"/><br>
>  <param name="laser_model_type" value="beam"/><br>
>  </node><br>
><br>
> With these parameters AMCL runs nearly normal; more than 700 particles<br>
> and the laser model "likelihood field" are causing problems.<br>
><br>
> AMCL still locks up if you move (or just touch) the robot with the mouse<br>
> in stage; with the parameters above the particlecloud disperses one or<br>
> two times while moving the robot, but then amcl is dead.<br>
> Interestingly, on the robot it is more stable, but still locks up sometimes.<br>
><br>
> It stops sending anything (transforms, topics) and CPU-load goes up to<br>
> 100%. I have seen no messages from AMCL before locking up.<br>
><br>
> Tested on: ROS boxturtle/cturtle on Ubuntu 9.10 (2.6.31) and 10.04<br>
> (2.6.32). Robot: iRobot roomba 500 with a Sick 300 laserscanner and a<br>
> notebook on top.<br>
><br>
> I hope this will help a bit to track down the problem.<br>
><br>
> Regards,<br>
> Torsten Fiolka<br>
><br>
> Am 27.10.2010 14:50, schrieb Eric Perko:<br>
>> Dominik,<br>
>><br>
>> A few questions to help debugging get started:<br>
>><br>
>>    1. Can you post your AMCL launch file with parameter values that<br>
>>       produces the problem?<br>
>>    2. If you have narrowed things down to just one or two parameters,<br>
>>       can you post both working and failing versions that differ only in<br>
>>       the parameter that leads to a problem?<br>
>>    3. Can you reproduce the crash in Stage or does it only happen with<br>
>>       the real robot?<br>
>>    4. You mention AMCL stops showing up in tf_monitor. Does it also stop<br>
>>       outputting on it's output topics such as amcl_pose or<br>
>>       particlecloud? You can check those with 'rostopic echo <topic>'.<br>
>>    5. Do you get any error output in the console where you started AMCL?<br>
>>       Anything in the log file for AMCL?<br>
>><br>
>> I'm definitely interested to see what parameters you are using - I had<br>
>> some similar sounding problems a few months ago while playing with the<br>
>> resampling parameters but didn't really spend much time looking into it.<br>
>><br>
>> - Eric<br>
>><br>
>> On Wed, Oct 27, 2010 at 4:28 AM, Dominik Franěk<br>
>> <<a href="mailto:dominik.franek@gmail.com">dominik.franek@gmail.com</a> <mailto:<a href="mailto:dominik.franek@gmail.com">dominik.franek@gmail.com</a>>> wrote:<br>
>><br>
>>     Hi guys and girls,<br>
>><br>
>>     recently I'm having difficulties running my robot as it stops working<br>
>>     after a very short time. I have narrowed the problem to AMCL, which<br>
>>     stops working either (probably) at it's first resampling, or some<br>
>>     longer time later. I have had all the navigation with amcl and static<br>
>>     map on both costmaps running all right for a long time and this error<br>
>>     started appearing during some testing of various parameters (no<br>
>>     changes in packages) and then stayed untill now, though I have tried<br>
>>     reverting or otherwise changing all I could think of. So, now some<br>
>>     details.<br>
>><br>
>>     My ROS and all substantial components are the most recent version,<br>
>>     including the yesterdays update. Costmaps are both using the same<br>
>>     static map with no marking/clearing. TF tree is default<br>
>>     (map->odom->base_link->laser_scan) and running on about 13Hz, all fine<br>
>>     on view_frames.  In rviz everything looks ok. Pose cloud appears after<br>
>>     setting initial pose.<br>
>>     When I send a goal, robot starts moving towards it, both in the real<br>
>>     and rviz. And after traversing around half a meter it freezes in rviz<br>
>>     (along with pose cloud and laser measurments), in command starts<br>
>>     printing transform timeout with pose timestamp staying on the same<br>
>>     value and amcl disappears from tf_monitor. In rxgraph and node info<br>
>>     amcl still looks all right though. In the real, robot keeps going<br>
>>     until it hits a wall... So the thing is, AMCL stops updating the pose<br>
>>     estimate.<br>
>>     What works a bit is to give the first goal very close, then the robot<br>
>>     gets to it alive, and then usually manages to get to a second goal<br>
>>     that can be far away. And then it freezes after reaching the second<br>
>>     goal.<br>
>><br>
>>     I have been trying to repair it for a long time and run out of ideas..<br>
>>      Would be gratefull for any help.<br>
>><br>
>>     Dominik<br>
>>     _______________________________________________<br>
>>     ros-users mailing list<br>
>>     <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a> <mailto:<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a>><br>
>>     <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
>><br>
>><br>
>><br>
>><br>
>> _______________________________________________<br>
>> ros-users mailing list<br>
>> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
>> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
><br>
> _______________________________________________<br>
> ros-users mailing list<br>
> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
><br>
_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
</div></div></blockquote></div><br></div>