[ros-users] AMCL, Map Server, Stage World file
Suraj Swami
suraj.g.swami at gmail.com
Tue Jun 29 01:19:29 UTC 2010
Hi Brian,
Thanks for all the help. I could get the amcl driver working.
Other than changes in the .world file and .yaml file I changed the client
code by which I would drive the robot move in the map. Here is the client
code
while (ros::ok())
{
vel_pub_.publish(cmd);
ros::spinOnce();
loop_rate.sleep();
}
Earlier I did not include spinOnce and loop_rate.sleep().
This is got it working. There are some areas in the map that the robot would
not localize itself but that could be mainly due to non uniqueness in the
map.
Thanks again.
Regards,
Suraj Swami
Date: Fri, 25 Jun 2010 17:33:21 -0400
> From: Suraj Swami <suraj.g.swami at gmail.com>
> Subject: Re: [ros-users] AMCL, Map Server, Stage World file
> To: ros-users at code.ros.org
> Message-ID:
> <AANLkTimDdqXpIR0ERAsO31in6L3m0UxqE58dUz4H8W4K at mail.gmail.com>
> Content-Type: text/plain; charset="iso-8859-1"
>
> Hi Brain,
>
> I am still not able to get AMCL working efficiently. Though I was able to
> solve some of the issues.
>
>
> - In the previous world file. The Pose was set to [ 0 0 0 ]
> floorplan
> > > (
> > > ??name "willow"
> > > ??bitmap "cave.pgm"
> > > ??size3 [54.0 58.7 0.5]
> > > ??pose [0 0 0]
> > > )
>
> Hence my map would was in 2,3,4th quadrant also. But the player map.h macro
> wants the points to be only in the 1st quadrant. And I guess hence the
> AMCL
> driver also requires the MAP to be in the first quadrant starting from [0 0
> 0]. This did help.
>
> - Next consideration was the resolution parameter in the map.yaml file. I
> set it to 22.4/ 500 rather than (22.4 * 22.4) / ( 500 * 500 ).In the second
> case the amcl would not work for me.
>
>
> I have used the AMCL driver in Player. It gives very accurate calculated
> location for the same map and smaller resolution for Laser Scanner. So I
> guess there is still something wrong that I am doing. The files are as
> follows.
>
>
> MCL.World file:
>
> define block model
> (
> size3 [0.5 0.5 0.5]
> gui_nose 0
> )
>
> define topurg laser
> (
> range_min 0.0
> range_max 15.0
> fov 240
> samples 1024
> # generic model properties
> color "black"
> size [ 0.05 0.05 0.1 ]
> )
>
> define erratic position
> (
> #size3 [0.415 0.392 0.25]
> size3 [0.35 0.35 0.25]
> origin3 [-0.05 0 0 0]
> gui_nose 1
> drive "diff"
> topurg(pose [0.050 0.000 0.000])
> )
>
> define floorplan model
> (
> # sombre, sensible, artistic
> color "gray30"
>
> # most maps will need a bounding box
> boundary 0
>
> gui_nose 0
> gui_grid 0
> gui_movemask 0
> gui_outline 0
> gripper_return 0
> fiducial_return 0
> laser_return 1
> )
>
> # set the resolution of the underlying raytrace model in meters
> resolution 0.02
>
> interval_sim 1 # simulation timestep in milliseconds
> interval_real 1 # real-time interval between simulation updates in
> milliseconds
>
> window
> (
> size [ 675.0 745.0 ]
> center [678.990 293.960]
> rotate [ 0.000 0 ]
> scale 28.806
> )
>
> # load an environment bitmap
> floorplan
> (
> name "willow"
> bitmap "cave.pgm"
> size3 [22.36067 22.36067 0.5]
> #pose [0 0 0]
> pose [11.2 11.2 0]
>
> )
>
>
>
> # throw in a robot
> erratic( pose [0 0 0.000] name "era" color "blue")
> block( pose [-13.924 25.020 0.000] color "red")
>
>
>
> #######################################################################################################
>
> map.yaml file :
>
> image: cave.pgm
> resolution: 0.0448
> origin: [0.0, 0.0, 0.0]
> occupied_thresh: 0.40
> free_thresh: 0.05
> negate: 0
>
> ###########################################################
>
>
> node file :
>
>
> #include <ros/ros.h>
> #include <std_msgs/String.h>
> #include <sstream>
> #include <opencv/cv.h>
> #include <opencv/highgui.h>
> #include "geometry_msgs/Twist.h"
> #include <sensor_msgs/LaserScan.h>
> #include "nav_msgs/GetMap.h"
> #include "geometry_msgs/PoseWithCovarianceStamped.h"
> #include "geometry_msgs/PoseArray.h"
>
> #include <map>
>
> #include <boost/bind.hpp>
> #include <boost/thread/mutex.hpp>
>
> #include "map/map.h" // To Include all Map Functions.
> #include "map/map.c"
> #include "map/map_range.c"
> //#include "map/map_store.c"
> #include "map/map_draw.c"
>
>
>
> int main(int argc, char** argv)
> {
> geometry_msgs::Twist cmd;
> geometry_msgs::Twist Pose_Array[100];
>
>
>
> char ch;
> int i;
> float rand,x;
> double range;
> ros::init(argc, argv, "talker");
> ros::NodeHandle n;
>
> map_t* map = map_alloc();
>
>
> ros::Rate loop_rate(5);
>
> cmd.linear.x = 0.5;
> cmd.linear.y = 0;
> cmd.angular.z = 0;
>
> ros::Publisher vel_pub_;
> vel_pub_ = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
>
>
> while (ros::ok())
> {
> vel_pub_.publish(cmd);
>
> }
>
>
> }
>
>
> I have attached the image file.
>
>
>
> Thank you for the help.
>
>
>
>
>
>
>
> Regards,
> Suraj Swami
>
> _______________________________________________
>
> > 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: /discuss/ros-users/attachments/20100623/ebd7971a/attachment-0001.htm
> >
> > ------------------------------
> >
> > Message: 11
> > Date: Wed, 23 Jun 2010 19:45:37 -0700
> > From: Brian Gerkey <gerkey at willowgarage.com>
> > Subject: Re: [ros-users] AMCL, Map Server, Stage World file
> > To: ros-users at code.ros.org
> > Message-ID:
> > <AANLkTikZ7w8AaZtic8Lvpga8eqHn-qmnrRVw8BdzSaPq at mail.gmail.com>
> > Content-Type: text/plain; charset="iso-8859-1"
> >
> > Send me your world and launch files and I'll try to reproduce the
> problem.
> >
> > brian.
> >
> > On Jun 21, 2010 2:45 PM, "Suraj Swami" <suraj.g.swami at gmail.com> wrote:
> >
> > Hi Brain,
> >
> > I am still not able to get amcl working. I changed the resolution in
> stage
> > world file and in the map.yamp file .
> >
> >
> > # load an environment bitmap
> > floorplan
> > (
> > name "willow"
> > bitmap "cave.pgm"
> > size3 [22.36067 22.36067 0.5] # Its at 500 * 500 pixel image.
> > pose [0 0 0]
> > )
> >
> > image: cave.pgm
> > resolution: 0.002
> > origin: [0.0, 0.0, 0.0]
> > occupied_thresh: 0.65
> > free_thresh: 0.196
> > negate: 0
> >
> > I do not get right location. Do I need to take care of anything else.
> >
> > Thanks for the help.
> >
> > -Suraj.
> >
> >
> > > hi Suraj,
> > >
> > > You probably have the resolution set incorrectly, but understandably
> > > so, in map.yaml. In the stage .world file, 'resolution' determines
> > > the resolution of the underlying grid that's used for simulation. The
> > > resolution of a bitmap that you load (for example, to be used as a
> > > background map), is determined by the combination of the bitmap's size
> > > in pixels, and the size that you specify for the bitmap.
> > >
> > > In this case, you're using cave.pgm, which I assume was created from
> > > Stage's cave.png, which is 500x500 pixels. Given the size you're
> > > setting, 54.0x58.7, the map resolution is about 0.11 m/pixel. That's
> > > the value you want to put in map.yaml.
> > >
> > > Now, why would a square bitmap be given a different size in X and Y?
> > > Depending on the version of Stage you're using, the map boundaries
> > > might be determined by the farthest-out black pixels, as opposed to
> > > the actual image boundaries. The slightly different values you're
> > > using might be an attempt to account for the non-square scaling that
> > > can result. There's a ticket for that:
> > > https://code.ros.org/trac/ros-pkg/ticket/4133 .
> > >
> > > brian.
> > >
> > > On Thu, Jun 10, 2010 at 11:42 PM, Suraj Swami <suraj.g.swami at gmail.com
> >
> > > wrote:
> > > > Hi,
> > > > I am trying to get understand how to use the AMCL package. I am using
> > > > simulator stage. And have written a ?small node to make the robot
> move
> > in
> > > > the map. But I am not getting right amcl_pose values.
> > > > I am not sure if my map_server and stage are well synchronized to
> have
> > > the
> > > > map data, resolution and origin.
> > > > FILE: map.yaml
> > > > image: cave.pgm
> > > > resolution: 0.02
> > > > origin: [0.0, 0.0, 0.0]
> > > > occupied_thresh: 0.65
> > > > free_thresh: 0.196
> > > > negate: 0
> > > >
> > > > FILE: MCL.world
> > > > define block model
> > > > (
> > > > ??size3 [0.5 0.5 0.5]
> > > > ??gui_nose 0
> > > > )
> > > > define topurg laser
> > > > (
> > > > ??range_min 0.0
> > > > ??range_max 30.0
> > > > ??fov 360
> > > > ??samples 720
> > > > ??# generic model properties
> > > > ??color "black"
> > > > ??size [ 0.05 0.05 0.1 ]
> > > > )
> > > > define erratic position
> > > > (
> > > > ??#size3 [0.415 0.392 0.25]
> > > > ??size3 [0.35 0.35 0.25]
> > > > ??origin3 [-0.05 0 0 0]
> > > > ??gui_nose 1
> > > > ??drive "diff"
> > > > ??topurg(pose [0.050 0.000 0.000])
> > > > )
> > > > define floorplan model
> > > > (
> > > > ??# sombre, sensible, artistic
> > > > ??color "gray30"
> > > > ??# most maps will need a bounding box
> > > > ??boundary 0
> > > > ??gui_nose 0
> > > > ??gui_grid 0
> > > > ??gui_movemask 0
> > > > ??gui_outline 0
> > > > ??gripper_return 0
> > > > ??fiducial_return 0
> > > > ??laser_return 1
> > > > )
> > > > # set the resolution of the underlying raytrace model in meters
> > > > resolution 0.02
> > > > interval_sim 1 ?# simulation timestep in milliseconds
> > > > interval_real 1 ?# real-time interval between simulation updates in
> > > > milliseconds
> > > > window
> > > > (
> > > > ??size [ 675.0 745.0 ]
> > > > ??center [678.990 293.960]
> > > > ??rotate [ 0.000 0 ]
> > > > ??scale 28.806
> > > > )
> > > > # load an environment bitmap
> > > > floorplan
> > > > (
> > > > ??name "willow"
> > > > ??bitmap "cave.pgm"
> > > > ??size3 [54.0 58.7 0.5]
> > > > ??pose [0 0 0]
> > > > )
> > > >
> > > >
> > > > # throw in a robot
> > > > erratic( pose [0 0 0.000] name "era" color "blue")
> > > > block( pose [-13.924 25.020 0.000] color "red")
> > > >
> > > >
> > > > Is there a problem to synchronize the two files or is there any other
> > > > parameters that I need to take care of ?
> > > > Thank you.
> > > > Regards,
> > > > Suraj Swami
> > > >
> > > >
> > > >
> > > >
> > > > _______________________________________________
> > > > 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: /discuss/ros-users/attachments/20100623/5d853db0/attachment-0001.htm
> >
> > ------------------------------
> >
> >
> -------------- next part --------------
> An HTML attachment was scrubbed...
> URL: /discuss/ros-users/attachments/20100625/3997274b/attachment-0001.htm
> -------------- next part --------------
> A non-text attachment was scrubbed...
> Name: MCL.world
> Type: application/octet-stream
> Size: 1218 bytes
> Desc: not available
> Url : /discuss/ros-users/attachments/20100625/3997274b/attachment-0002.obj
> -------------- next part --------------
> A non-text attachment was scrubbed...
> Name: map.yaml
> Type: application/octet-stream
> Size: 109 bytes
> Desc: not available
> Url : /discuss/ros-users/attachments/20100625/3997274b/attachment-0003.obj
> -------------- next part --------------
> A non-text attachment was scrubbed...
> Name: cave.pgm
> Type: image/x-portable-graymap
> Size: 250054 bytes
> Desc: not available
> Url : /discuss/ros-users/attachments/20100625/3997274b/attachment-0001.pgm
>
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20100628/633ac454/attachment-0003.html>
More information about the ros-users
mailing list