Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [776] pkg/trunk
|
From: <ge...@us...> - 2008-06-12 19:51:36
|
Revision: 776
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=776&view=rev
Author: gerkey
Date: 2008-06-12 12:51:44 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
updated to new message types and Time usage; moved to map_server for occ grid retrieval
Modified Paths:
--------------
pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
pkg/trunk/nav/amcl_player/Makefile
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/nav_view/Makefile
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/Makefile
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
===================================================================
--- pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,10 +1,11 @@
<launch>
<ns name="wg">
- <node pkg="rosstage" type="rosstage" args="willow-erratic.world" respawn="true" />
- <node pkg="amcl_player" type="amcl_player" args="willow-full.png 0.1" respawn="true" />
- <node pkg="wavefront_player" type="wavefront_player" args="willow-full.png 0.1" respawn="true" />
- <node pkg="nav_view" type="nav_view" args="willow-full.png 0.1" respawn="true" />
+ <node pkg="rosstage" type="rosstage" args="willow-erratic.world" respawn="false" />
+ <node pkg="map_server" type="map_server" args="willow-full.png 0.1" respawn="false" />
+ <node pkg="amcl_player" type="amcl_player" respawn="false" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
</ns>
</launch>
Modified: pkg/trunk/nav/amcl_player/Makefile
===================================================================
--- pkg/trunk/nav/amcl_player/Makefile 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/amcl_player/Makefile 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,6 +1,5 @@
SRC = amcl_player.cc
OUT = amcl_player
PKG = amcl_player
-CFLAGS = -Wall -Werror `pkg-config --cflags gdk-pixbuf-2.0`
-LFLAGS = `pkg-config --libs gdk-pixbuf-2.0` -lplayerdrivers
+LFLAGS = -lplayerdrivers
include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-06-12 19:51:44 UTC (rev 776)
@@ -46,21 +46,9 @@
@section usage Usage
@verbatim
-$ amcl_player <map> <res> [standard ROS args]
+$ amcl_player
@endverbatim
-@param map An image file to load as an occupancy grid map. The robot will be localized against this map using laser scans. The lower-left pixel of the map is assigned the pose (0,0,0).
-@param res The resolution of the map, in meters, pixel.
-
-@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
-@todo Remove the x,y,th arguments and expose the particle filter initialization via a ROS topic.
-
-@par Example
-
-@verbatim
-$ amcl_player mymap.png 0.1 10.4 31.2 90.0
-@endverbatim
-
<hr>
@section topic ROS topics
@@ -99,22 +87,19 @@
#include <ros/node.h>
// Messages that I need
-#include <std_msgs/MsgLaserScan.h>
-#include <std_msgs/MsgRobotBase2DOdom.h>
-#include <std_msgs/MsgParticleCloud2D.h>
-#include <std_msgs/MsgPose2DFloat32.h>
+#include <std_msgs/LaserScan.h>
+#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/ParticleCloud2D.h>
+#include <std_msgs/Pose2DFloat32.h>
+#include <std_srvs/StaticMap.h>
// For transform support
#include <rosTF/rosTF.h>
-#include <gdk-pixbuf/gdk-pixbuf.h>
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
// check that given coords are valid (i.e., on the map)
#define MAP_VALID(mf, i, j) ((i >= 0) && (i < mf->sx) && (j >= 0) && (j < mf->sy))
-double get_time(void);
-int read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate);
#define PLAYER_QUEUE_LEN 32
@@ -125,7 +110,7 @@
class AmclNode: public ros::node, public Driver
{
public:
- AmclNode(char* fname, double res);
+ AmclNode();
~AmclNode();
int Setup() {return(0);}
@@ -146,11 +131,11 @@
ConfigFile* cf;
// incoming messages
- MsgRobotBase2DOdom localizedOdomMsg;
- MsgParticleCloud2D particleCloudMsg;
- MsgRobotBase2DOdom odomMsg;
- MsgLaserScan laserMsg;
- MsgPose2DFloat32 initialPoseMsg;
+ std_msgs::RobotBase2DOdom localizedOdomMsg;
+ std_msgs::ParticleCloud2D particleCloudMsg;
+ std_msgs::RobotBase2DOdom odomMsg;
+ std_msgs::LaserScan laserMsg;
+ std_msgs::Pose2DFloat32 initialPoseMsg;
// Message callbacks
void odomReceived();
@@ -178,20 +163,14 @@
double resolution;
};
-#define USAGE "USAGE: amcl_player <map.png> <res>"
+#define USAGE "USAGE: amcl_player"
int
main(int argc, char** argv)
{
- if(argc < 3)
- {
- puts(USAGE);
- exit(-1);
- }
-
ros::init(argc, argv);
- AmclNode an(argv[1],atof(argv[2]));
+ AmclNode an;
// Start up the robot
if(an.start() != 0)
@@ -212,7 +191,7 @@
return(0);
}
-AmclNode::AmclNode(char* fname, double res) :
+AmclNode::AmclNode() :
ros::node("amcl_player"),
Driver(NULL,-1,false,PLAYER_QUEUE_LEN)
{
@@ -224,19 +203,42 @@
playerxdr_ftable_init();
puts("advertising");
- advertise<MsgRobotBase2DOdom>("localizedpose");
- advertise<MsgParticleCloud2D>("particlecloud");
+ advertise<std_msgs::RobotBase2DOdom>("localizedpose");
+ advertise<std_msgs::ParticleCloud2D>("particlecloud");
puts("subscribing");
subscribe("odom", odomMsg, &AmclNode::odomReceived);
subscribe("scan", laserMsg, &AmclNode::laserReceived);
subscribe("initialpose", initialPoseMsg, &AmclNode::initialPoseReceived);
puts("done");
- // TODO: get map via RPC
- assert(read_map_from_image(&this->sx, &this->sy, &this->mapdata, fname, 0)
- == 0);
- this->resolution = res;
+ // get map via RPC
+ std_srvs::StaticMap::request req;
+ std_srvs::StaticMap::response resp;
+ puts("Requesting the map...");
+ assert(ros::service::call("static_map", req, resp));
+ printf("Received a %d X %d map @ %.3f m/pix\n",
+ resp.map.width,
+ resp.map.height,
+ resp.map.resolution);
+ this->sx = resp.map.width;
+ this->sy = resp.map.height;
+ this->resolution = resp.map.resolution;
+ // Convert to player format
+ this->mapdata = new char[this->sx*this->sy];
+ for(int i=0;i<this->sx*this->sy;i++)
+ {
+ if(resp.map.data[i] == 0)
+ this->mapdata[i] = -1;
+ else if(resp.map.data[i] == 100)
+ this->mapdata[i] = +1;
+ else
+ this->mapdata[i] = 0;
+ }
+
+ //assert(read_map_from_image(&this->sx, &this->sy, &this->mapdata, fname, 0)
+ //== 0);
+
// TODO: automatically convert between string and player_devaddr_t
// representations
@@ -389,11 +391,12 @@
localizedOdomMsg.pos.x = pdata->pos.px;
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
- localizedOdomMsg.header.stamp.sec = (unsigned long)floor(hdr->timestamp);
- localizedOdomMsg.header.stamp.nsec =
- (unsigned long)rint(1e9 * (hdr->timestamp -
- localizedOdomMsg.header.stamp.sec));
- localizedOdomMsg.__timestamp_override = true;
+ localizedOdomMsg.header.stamp.from_double(hdr->timestamp);
+ //localizedOdomMsg.header.stamp.sec = (unsigned long)floor(hdr->timestamp);
+ //localizedOdomMsg.header.stamp.nsec =
+ //(unsigned long)rint(1e9 * (hdr->timestamp -
+ //localizedOdomMsg.header.stamp.sec));
+ //localizedOdomMsg.__timestamp_override = true;
publish("localizedpose", localizedOdomMsg);
// Also request and publish the particle cloud
@@ -619,8 +622,7 @@
}
pdata.id = this->laserMsg.header.seq;
- double timestamp = this->laserMsg.header.stamp.sec +
- this->laserMsg.header.stamp.nsec / 1e9;
+ double timestamp = this->laserMsg.header.stamp.to_double();
this->Driver::Publish(this->laser_addr,
PLAYER_MSGTYPE_DATA,
@@ -653,8 +655,7 @@
pdata.vel.pa = this->odomMsg.vel.th;
pdata.stall = this->odomMsg.stall;
- double timestamp = this->odomMsg.header.stamp.sec +
- this->odomMsg.header.stamp.nsec / 1e9;
+ double timestamp = this->odomMsg.header.stamp.to_double();
this->Driver::Publish(this->position2d_addr,
PLAYER_MSGTYPE_DATA,
@@ -663,72 +664,3 @@
×tamp);
}
-int
-read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate)
-{
- GdkPixbuf* pixbuf;
- guchar* pixels;
- guchar* p;
- int rowstride, n_channels, bps;
- GError* error = NULL;
- int i,j,k;
- double occ;
- int color_sum;
- double color_avg;
-
- // Initialize glib
- g_type_init();
-
- printf("MapFile loading image file: %s...", fname);
- fflush(stdout);
-
- // Read the image
- if(!(pixbuf = gdk_pixbuf_new_from_file(fname, &error)))
- {
- printf("failed to open image file %s", fname);
- return(-1);
- }
-
- *size_x = gdk_pixbuf_get_width(pixbuf);
- *size_y = gdk_pixbuf_get_height(pixbuf);
-
- assert(*mapdata = (char*)malloc(sizeof(char) * (*size_x) * (*size_y)));
-
- rowstride = gdk_pixbuf_get_rowstride(pixbuf);
- bps = gdk_pixbuf_get_bits_per_sample(pixbuf)/8;
- n_channels = gdk_pixbuf_get_n_channels(pixbuf);
- //if(gdk_pixbuf_get_has_alpha(pixbuf))
- //n_channels++;
-
- // Read data
- pixels = gdk_pixbuf_get_pixels(pixbuf);
- for(j = 0; j < *size_y; j++)
- {
- for (i = 0; i < *size_x; i++)
- {
- p = pixels + j*rowstride + i*n_channels*bps;
- color_sum = 0;
- for(k=0;k<n_channels;k++)
- color_sum += *(p + (k * bps));
- color_avg = color_sum / (double)n_channels;
-
- if(negate)
- occ = color_avg / 255.0;
- else
- occ = (255 - color_avg) / 255.0;
- if(occ > 0.95)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = +1;
- else if(occ < 0.1)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = -1;
- else
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = 0;
- }
- }
-
- gdk_pixbuf_unref(pixbuf);
-
- puts("Done.");
- printf("MapFile read a %d X %d map\n", *size_x, *size_y);
- return(0);
-}
Modified: pkg/trunk/nav/amcl_player/manifest.xml
===================================================================
--- pkg/trunk/nav/amcl_player/manifest.xml 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/amcl_player/manifest.xml 2008-06-12 19:51:44 UTC (rev 776)
@@ -5,5 +5,5 @@
<depend package="roscpp" />
<depend package="player" />
<depend package="rosTF" />
- <depend package="std_msgs" />
+ <depend package="std_srvs" />
</package>
Modified: pkg/trunk/nav/nav_view/Makefile
===================================================================
--- pkg/trunk/nav/nav_view/Makefile 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/nav_view/Makefile 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,4 +1,5 @@
SRC = nav_view.cpp
OUT = nav_view
PKG = nav_view
+CFLAGS = -g
include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-06-12 19:51:44 UTC (rev 776)
@@ -125,8 +125,6 @@
bool setting_theta;
double gx,gy;
- std_srvs::StaticMap::response map_resp;
-
rosTFClient tf;
ros::time::clock myClock;
@@ -209,8 +207,8 @@
ros::init(argc, argv);
NavView view;
- //if(!view.load_map())
- if(!view.load_map_from_image(argv[1],atof(argv[2])))
+ if(!view.load_map())
+ //if(!view.load_map_from_image(argv[1],atof(argv[2])))
puts("WARNING: failed to load map");
view.main_loop();
ros::fini();
@@ -380,62 +378,66 @@
NavView::load_map()
{
std_srvs::StaticMap::request req;
- std_srvs::StaticMap::response res;
+ std_srvs::StaticMap::response resp;
puts("Requesting the map...");
- if(ros::service::call("static_map", req, res))
+ if(ros::service::call("static_map", req, resp))
{
puts("success");
- map_resp = res;
- map_res = map_resp.map.resolution;
- map_width = map_resp.map.width;
- map_height = map_resp.map.height;
-
printf("Received a %d X %d map @ %.3f m/pix\n",
- map_resp.map.width,
- map_resp.map.height,
- map_resp.map.resolution);
+ resp.map.width,
+ resp.map.height,
+ resp.map.resolution);
+ map_res = resp.map.resolution;
+
+ // Pad dimensions to power of 2
+ map_width = (int)pow(2,ceil(log2(resp.map.width)));
+ map_height = (int)pow(2,ceil(log2(resp.map.height)));
+
+ printf("Padded dimensions to %d X %d\n", map_width, map_height);
+
// Expand it to be RGB data
- unsigned char* pixels =
- new unsigned char[map_resp.map.width * map_resp.map.height * 4];
- for(unsigned int i=0;i<map_resp.map.width*map_resp.map.height;i++)
+ unsigned char* pixels = new unsigned char[map_width * map_height * 3];
+ assert(pixels);
+ memset(pixels,255,map_width*map_height*3);
+ for(unsigned int j=0;j<resp.map.height;j++)
{
- unsigned char val;
- if(map_resp.map.data[i] == 100)
- val = 0;
- else if(map_resp.map.data[i] == 0)
- val = 255;
- else
- val = 127;
+ for(unsigned int i=0;i<resp.map.width;i++)
+ {
+ unsigned char val;
+ if(resp.map.data[j*resp.map.width+i] == 100)
+ val = 0;
+ else if(resp.map.data[j*resp.map.width+i] == 0)
+ val = 255;
+ else
+ val = 127;
- pixels[4*i] = val;
- pixels[4*i+1] = val;
- pixels[4*i+2] = val;
- pixels[4*i+3] = 255;
+ int pidx = 3*(j*map_width + i);
+ pixels[pidx+0] = val;
+ pixels[pidx+1] = val;
+ pixels[pidx+2] = val;
+ }
}
- GLint nOfColors = 4;
- GLenum texture_format = GL_RGBA;
+ glEnable( GL_TEXTURE_2D );
// Have OpenGL generate a texture object handle for us
- glGenTextures( 1, &map_texture );
+ glGenTextures(1, &map_texture);
// Bind the texture object
- glBindTexture( GL_TEXTURE_2D, map_texture );
+ glBindTexture(GL_TEXTURE_2D, map_texture);
+ glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+
// Set the texture's stretching properties
- glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
- GL_LINEAR );
- glTexParameteri( GL_TEXTURE_2D,
- GL_TEXTURE_MAG_FILTER, GL_LINEAR );
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
- // Edit the texture object's image data using
- //the information SDL_Surface gives us
- glTexImage2D( GL_TEXTURE_2D, 0, nOfColors,
- map_width, map_height, 0,
- texture_format,
- GL_UNSIGNED_BYTE,
- pixels );
+ // Edit the texture object's image data
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB,
+ map_width, map_height, 0,
+ GL_RGB, GL_UNSIGNED_BYTE,
+ pixels);
return(true);
}
else
@@ -464,14 +466,13 @@
// Check that the image's width is a power of 2
if((temp->w & (temp->w - 1)) != 0) {
- printf("warning: image.bmp's width is not a power of 2\n");
+ printf("warning: image width is not a power of 2\n");
}
// Also check if the height is a power of 2
if((temp->h & (temp->h - 1)) != 0) {
- printf("warning: image.bmp's height is not a power of 2\n");
+ printf("warning: image height is not a power of 2\n");
}
-
// Flip the image vertically
map_surface = flip_vert(temp);
SDL_FreeSurface(temp);
Modified: pkg/trunk/nav/wavefront_player/Makefile
===================================================================
--- pkg/trunk/nav/wavefront_player/Makefile 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/wavefront_player/Makefile 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,6 +1,5 @@
SRC = wavefront_player.cc
OUT = wavefront_player
PKG = wavefront_player
-CFLAGS = -Wall -Werror `pkg-config --cflags gdk-pixbuf-2.0`
-LFLAGS = `pkg-config --libs gdk-pixbuf-2.0` -lwavefront_standalone
+LFLAGS = -lwavefront_standalone
include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2008-06-12 19:51:44 UTC (rev 776)
@@ -4,6 +4,6 @@
<license>BSD</license>
<depend package="roscpp" />
<depend package="player" />
- <depend package="std_msgs" />
+ <depend package="std_srvs" />
<depend package="rosTF" />
</package>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 19:51:44 UTC (rev 776)
@@ -64,20 +64,9 @@
@section usage Usage
@verbatim
-$ wavefront_player <map> <res> [standard ROS args]
+$ wavefront_player
@endverbatim
-@param map An image file to load as an occupancy grid map. The robot will be localized against this map using laser scans. The lower-left pixel of the map is assigned the pose (0,0,0).
-@param res The resolution of the map, in meters, pixel.
-
-@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
-
-@par Example
-
-@verbatim
-$ wavefront_player mymap.png 0.1
-@endverbatim
-
<hr>
@section topic ROS topics
@@ -124,15 +113,16 @@
#include <ros/node.h>
// The messages that we'll use
-#include <std_msgs/MsgPlanner2DState.h>
-#include <std_msgs/MsgPlanner2DGoal.h>
-#include <std_msgs/MsgBaseVel.h>
+#include <std_msgs/Planner2DState.h>
+#include <std_msgs/Planner2DGoal.h>
+#include <std_msgs/BaseVel.h>
//#include <std_msgs/MsgRobotBase2DOdom.h>
-#include <std_msgs/MsgLaserScan.h>
-#include <std_msgs/MsgPose2DFloat32.h>
+#include <std_msgs/LaserScan.h>
+#include <std_msgs/Pose2DFloat32.h>
+#include <std_srvs/StaticMap.h>
// For GUI debug
-#include <std_msgs/MsgPolyline2D.h>
+#include <std_msgs/Polyline2D.h>
// For transform support
#include <rosTF/rosTF.h>
@@ -145,15 +135,6 @@
#define RTOD(a) ((a)*180.0/M_PI)
#define SIGN(x) (((x) < 0.0) ? -1 : 1)
-//void draw_cspace(plan_t* plan, const char* fname);
-//void draw_path(plan_t* plan, double lx, double ly, const char* fname);
-#include <gdk-pixbuf/gdk-pixbuf.h>
-// compute linear index for given map coords
-#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
-// computes the signed minimum difference between the two angles.
-int read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate);
-
// A bunch of x,y points, with a timestamp
typedef struct
{
@@ -201,7 +182,7 @@
// Map update paramters (for adding obstacles)
double laser_maxrange;
ros::Duration laser_buffer_time;
- std::list<MsgLaserScan> buffered_laser_scans;
+ std::list<std_msgs::LaserScan> buffered_laser_scans;
std::list<laser_pts_t> laser_scans;
double* laser_hitpts;
size_t laser_hitpts_len, laser_hitpts_size;
@@ -212,13 +193,13 @@
double tvmin, tvmax, avmin, avmax, amin, amax;
// incoming/outgoing messages
- MsgPlanner2DGoal goalMsg;
+ std_msgs::Planner2DGoal goalMsg;
//MsgRobotBase2DOdom odomMsg;
- MsgLaserScan laserMsg;
- MsgPolyline2D polylineMsg;
- MsgPolyline2D pointcloudMsg;
- MsgPlanner2DState pstate;
- std::vector<MsgLaserScan> laserScans;
+ std_msgs::LaserScan laserMsg;
+ std_msgs::Polyline2D polylineMsg;
+ std_msgs::Polyline2D pointcloudMsg;
+ std_msgs::Planner2DState pstate;
+ std::vector<std_msgs::LaserScan> laserScans;
//MsgRobotBase2DOdom prevOdom;
bool firstodom;
@@ -236,7 +217,7 @@
// Transform client
rosTFClient tf;
- WavefrontNode(char* fname, double res);
+ WavefrontNode();
~WavefrontNode();
// Stop the robot
@@ -247,20 +228,14 @@
void sleep(double loopstart);
};
-#define USAGE "USAGE: wavefront_player <map.png> <res>"
+#define USAGE "USAGE: wavefront_player"
int
main(int argc, char** argv)
{
- if(argc < 3)
- {
- puts(USAGE);
- exit(-1);
- }
-
ros::init(argc,argv);
- WavefrontNode wn(argv[1],atof(argv[2]));
+ WavefrontNode wn;
struct timeval curr;
while(wn.ok())
@@ -273,7 +248,7 @@
return(0);
}
-WavefrontNode::WavefrontNode(char* fname, double res) :
+WavefrontNode::WavefrontNode() :
ros::node("wavfront_player"),
planner_state(NO_GOAL),
enable(true),
@@ -300,10 +275,30 @@
amax(DTOR(40.0)),
tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
- // TODO: get map via RPC
+ // get map via RPC
+ std_srvs::StaticMap::request req;
+ std_srvs::StaticMap::response resp;
+ puts("Requesting the map...");
+ assert(ros::service::call("static_map", req, resp));
+ printf("Received a %d X %d map @ %.3f m/pix\n",
+ resp.map.width,
+ resp.map.height,
+ resp.map.resolution);
char* mapdata;
int sx, sy;
- assert(read_map_from_image(&sx, &sy, &mapdata, fname, 0) == 0);
+ sx = resp.map.width;
+ sy = resp.map.height;
+ // Convert to player format
+ mapdata = new char[sx*sy];
+ for(int i=0;i<sx*sy;i++)
+ {
+ if(resp.map.data[i] == 0)
+ mapdata[i] = -1;
+ else if(resp.map.data[i] == 100)
+ mapdata[i] = +1;
+ else
+ mapdata[i] = 0;
+ }
assert((this->plan = plan_alloc(this->robot_radius+this->safety_dist,
this->robot_radius+this->safety_dist,
@@ -322,9 +317,9 @@
for(int i=0;i<sx;i++)
this->plan->cells[i+j*sx].occ_state = mapdata[i+j*sx];
}
- free(mapdata);
+ delete[] mapdata;
- this->plan->scale = res;
+ this->plan->scale = resp.map.resolution;
this->plan->size_x = sx;
this->plan->size_y = sy;
this->plan->origin_x = 0.0;
@@ -347,10 +342,10 @@
FRAMEID_ROBOT,
0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
- advertise<MsgPlanner2DState>("state");
- advertise<MsgPolyline2D>("gui_path");
- advertise<MsgPolyline2D>("gui_laser");
- advertise<MsgBaseVel>("cmd_vel");
+ advertise<std_msgs::Planner2DState>("state");
+ advertise<std_msgs::Polyline2D>("gui_path");
+ advertise<std_msgs::Polyline2D>("gui_laser");
+ advertise<std_msgs::BaseVel>("cmd_vel");
subscribe("goal", goalMsg, &WavefrontNode::goalReceived);
subscribe("scan", laserMsg, &WavefrontNode::laserReceived);
}
@@ -385,8 +380,9 @@
WavefrontNode::laserReceived()
{
// Copy and push this scan into the list of buffered scans
- MsgLaserScan newscan;
+ std_msgs::LaserScan newscan(laserMsg);
// Do a deep copy
+ /*
newscan.header.stamp.sec = laserMsg.header.stamp.sec;
newscan.header.stamp.nsec = laserMsg.header.stamp.nsec;
newscan.header.frame_id = laserMsg.header.frame_id;
@@ -396,10 +392,11 @@
newscan.angle_increment = laserMsg.angle_increment;
newscan.set_ranges_size(laserMsg.get_ranges_size());
memcpy(newscan.ranges,laserMsg.ranges,laserMsg.get_ranges_size()*sizeof(float));
+ */
this->buffered_laser_scans.push_back(newscan);
// Iterate through the buffered scans, trying to interpolate each one
- for(std::list<MsgLaserScan>::iterator it = this->buffered_laser_scans.begin();
+ for(std::list<std_msgs::LaserScan>::iterator it = this->buffered_laser_scans.begin();
it != this->buffered_laser_scans.end();
it++)
{
@@ -414,8 +411,9 @@
libTF::TFPose2D local,global;
local.frame = it->header.frame_id;
- local.time = it->header.stamp.sec * 1000000000ULL +
- it->header.stamp.nsec;
+ local.time = it->header.stamp.to_ull();
+ //local.time = it->header.stamp.sec * 1000000000ULL +
+ //it->header.stamp.nsec;
float b=it->angle_min;
float* r=it->ranges;
unsigned int i;
@@ -462,7 +460,10 @@
{
pts.pts_num = cnt;
this->laser_scans.push_back(pts);
- delete[] it->ranges;
+ // Don't delete ranges here, because the LaserScan destructor does it
+ // when the object falls out of scope after being erased from the
+ // list.
+ //delete[] it->ranges;
it = this->buffered_laser_scans.erase(it);
it--;
}
@@ -540,13 +541,13 @@
// Declare this globally, so that it never gets desctructed (message
// desctruction causes master disconnect)
-MsgBaseVel* cmdvel;
+std_msgs::BaseVel* cmdvel;
void
WavefrontNode::sendVelCmd(double vx, double vy, double vth)
{
if(!cmdvel)
- cmdvel = new MsgBaseVel();
+ cmdvel = new std_msgs::BaseVel();
cmdvel->vx = vx;
cmdvel->vw = vth;
this->ros::node::publish("cmd_vel", *cmdvel);
@@ -739,73 +740,3 @@
usleep((unsigned int)rint(tdiff*1e6));
}
-int
-read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate)
-{
- GdkPixbuf* pixbuf;
- guchar* pixels;
- guchar* p;
- int rowstride, n_channels, bps;
- GError* error = NULL;
- int i,j,k;
- double occ;
- int color_sum;
- double color_avg;
-
- // Initialize glib
- g_type_init();
-
- printf("MapFile loading image file: %s...", fname);
- fflush(stdout);
-
- // Read the image
- if(!(pixbuf = gdk_pixbuf_new_from_file(fname, &error)))
- {
- printf("failed to open image file %s", fname);
- return(-1);
- }
-
- *size_x = gdk_pixbuf_get_width(pixbuf);
- *size_y = gdk_pixbuf_get_height(pixbuf);
-
- assert(*mapdata = (char*)malloc(sizeof(char) * (*size_x) * (*size_y)));
-
- rowstride = gdk_pixbuf_get_rowstride(pixbuf);
- bps = gdk_pixbuf_get_bits_per_sample(pixbuf)/8;
- n_channels = gdk_pixbuf_get_n_channels(pixbuf);
- //if(gdk_pixbuf_get_has_alpha(pixbuf))
- //n_channels++;
-
- // Read data
- pixels = gdk_pixbuf_get_pixels(pixbuf);
- for(j = 0; j < *size_y; j++)
- {
- for (i = 0; i < *size_x; i++)
- {
- p = pixels + j*rowstride + i*n_channels*bps;
- color_sum = 0;
- for(k=0;k<n_channels;k++)
- color_sum += *(p + (k * bps));
- color_avg = color_sum / (double)n_channels;
-
- if(negate)
- occ = color_avg / 255.0;
- else
- occ = (255 - color_avg) / 255.0;
- if(occ > 0.5)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = +1;
- else if(occ < 0.1)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = -1;
- else
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = 0;
- }
- }
-
- gdk_pixbuf_unref(pixbuf);
-
- puts("Done.");
- printf("MapFile read a %d X %d map\n", *size_x, *size_y);
- return(0);
-}
-
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-06-12 19:51:44 UTC (rev 776)
@@ -242,11 +242,12 @@
// TODO: get the frame ID from somewhere
this->laserMsg.header.frame_id = FRAMEID_LASER;
- this->laserMsg.header.stamp.sec =
- (unsigned long)floor(world->SimTimeNow() / 1e6);
- this->laserMsg.header.stamp.nsec =
- (unsigned long)rint(1e3 * (world->SimTimeNow() -
- this->laserMsg.header.stamp.sec * 1e6));
+ this->laserMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
+ //this->laserMsg.header.stamp.sec =
+ //(unsigned long)floor(world->SimTimeNow() / 1e6);
+ //this->laserMsg.header.stamp.nsec =
+ //(unsigned long)rint(1e3 * (world->SimTimeNow() -
+ //this->laserMsg.header.stamp.sec * 1e6));
this->laserMsg.__timestamp_override = true;
publish("scan",this->laserMsg);
}
@@ -264,11 +265,12 @@
// TODO: get the frame ID from somewhere
this->odomMsg.header.frame_id = FRAMEID_ODOM;
- this->odomMsg.header.stamp.sec =
- (unsigned long)floor(world->SimTimeNow() / 1e6);
- this->odomMsg.header.stamp.nsec =
- (unsigned long)rint(1e3 * (world->SimTimeNow() -
- this->odomMsg.header.stamp.sec * 1e6));
+ this->odomMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
+ //this->odomMsg.header.stamp.sec =
+ //(unsigned long)floor(world->SimTimeNow() / 1e6);
+ //this->odomMsg.header.stamp.nsec =
+ //(unsigned long)rint(1e3 * (world->SimTimeNow() -
+ //this->odomMsg.header.stamp.sec * 1e6));
this->odomMsg.__timestamp_override = true;
publish("odom",this->odomMsg);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
