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