Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14484] pkg/trunk
|
From: <tf...@us...> - 2009-04-27 21:34:46
|
Revision: 14484
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14484&view=rev
Author: tfoote
Date: 2009-04-27 21:34:34 +0000 (Mon, 27 Apr 2009)
Log Message:
-----------
resolving ticket:937
Modified Paths:
--------------
pkg/trunk/deprecated/amcl_player/amcl_player.cc
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/map_server/manifest.xml
pkg/trunk/nav/map_server/src/image_loader.cpp
pkg/trunk/nav/map_server/src/main.cpp
pkg/trunk/nav/map_server/src/map_saver.cpp
pkg/trunk/nav/map_server/test/rtest.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv
pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_map.h
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg
Removed Paths:
-------------
pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
Modified: pkg/trunk/deprecated/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-04-27 21:34:34 UTC (rev 14484)
@@ -250,13 +250,13 @@
usleep(1000000);
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- this->sx = resp.map.width;
- this->sy = resp.map.height;
- this->resolution = resp.map.resolution;
+ this->sx = resp.map.info.width;
+ this->sy = resp.map.info.height;
+ this->resolution = resp.map.info.resolution;
// Convert to player format
this->mapdata = new char[this->sx*this->sy];
for(int i=0;i<this->sx*this->sy;i++)
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -114,12 +114,12 @@
usleep(1000000);
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
int sx, sy;
- sx = resp.map.width;
- sy = resp.map.height;
+ sx = resp.map.info.width;
+ sy = resp.map.info.height;
// Convert to player format
unsigned char* mapdata = new unsigned char[sx*sy];
for(int i=0;i<sx*sy;i++)
@@ -131,7 +131,7 @@
else
mapdata[i] = 0;
}
- costmap_.setStaticMap(sx, sy, resp.map.resolution, mapdata);
+ costmap_.setStaticMap(sx, sy, resp.map.info.resolution, mapdata);
delete[] mapdata;
tf_.setWithEulers("FRAMEID_LASER",
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -264,12 +264,12 @@
}
ROS_DEBUG("Received a %d X %d map at %f m/pix\n",
- resp.map.width, resp.map.height, resp.map.resolution);
+ resp.map.info.width, resp.map.info.height, resp.map.info.resolution);
// We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
// our planner and controller do not reason about the no obstacle case
std::vector<unsigned char> inputData;
- unsigned int numCells = resp.map.width * resp.map.height;
+ unsigned int numCells = resp.map.info.width * resp.map.info.height;
for(unsigned int i = 0; i < numCells; i++){
inputData.push_back((unsigned char) resp.map.data[i]);
}
@@ -282,21 +282,26 @@
ros::Node::instance()->param("~costmap_2d/raytrace_range", rayTraceRange, 10.0);
ros::Node::instance()->param("~costmap_2d/obstacle_range", obstacleRange, 10.0);
- costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
- inputData , resp.map.resolution,
+ costMap_ = new CostMap2D((unsigned int)resp.map.info.width, (unsigned int)resp.map.info.height,
+ inputData , resp.map.info.resolution,
lethalObstacleThreshold, maxZ_, zLB, zUB,
inflationRadius, circumscribedRadius, inscribedRadius, weight,
obstacleRange, rayTraceRange, raytraceWindow);
// set up costmap service response
- costmap_response_.map.width=resp.map.width;
- costmap_response_.map.height=resp.map.height;
- costmap_response_.map.resolution=resp.map.resolution;
- costmap_response_.map.origin.x = 0.0;
- costmap_response_.map.origin.y = 0.0;
- costmap_response_.map.origin.th = 0.0;
- costmap_response_.map.set_data_size(resp.map.width*resp.map.height);
+ costmap_response_.map.info.width=resp.map.info.width;
+ costmap_response_.map.info.height=resp.map.info.height;
+ costmap_response_.map.info.resolution=resp.map.info.resolution;
+ costmap_response_.map.info.origin.position.x = 0.0;
+ costmap_response_.map.info.origin.position.y = 0.0;
+ costmap_response_.map.info.origin.position.z = 0.0;
+ costmap_response_.map.info.origin.orientation.x = 0.0;
+ costmap_response_.map.info.origin.orientation.y = 0.0;
+ costmap_response_.map.info.origin.orientation.z = 0.0;
+ costmap_response_.map.info.origin.orientation.w = 1.0;
+ costmap_response_.map.set_data_size(resp.map.info.width*resp.map.info.height);
+
ros::Node::instance()->param("~costmap_2d/local_access_mapsize", local_access_mapsize_, 4.0);
global_map_accessor_ = new CostMapAccessor(*costMap_);
local_map_accessor_ = new CostMapAccessor(*costMap_, local_access_mapsize_, 0.0, 0.0);
@@ -699,7 +704,7 @@
{
const unsigned char* costmap = getCostMap().getMap();
res = costmap_response_;
- copy(costmap, costmap+res.map.width*res.map.height, res.map.data.begin());
+ copy(costmap, costmap+res.map.info.width*res.map.info.height, res.map.data.begin());
return true;
}
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -400,15 +400,15 @@
d.sleep();
}
ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- map->size_x = resp.map.width;
- map->size_y = resp.map.height;
- map->scale = resp.map.resolution;
- map->origin_x = resp.map.origin.x + (map->size_x / 2) * map->scale;
- map->origin_y = resp.map.origin.y + (map->size_y / 2) * map->scale;
+ map->size_x = resp.map.info.width;
+ map->size_y = resp.map.info.height;
+ map->scale = resp.map.info.resolution;
+ map->origin_x = resp.map.info.origin.position.x + (map->size_x / 2) * map->scale;
+ map->origin_y = resp.map.info.origin.position.y + (map->size_y / 2) * map->scale;
// Convert to player format
map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
ROS_ASSERT(map->cells);
Modified: pkg/trunk/nav/map_server/manifest.xml
===================================================================
--- pkg/trunk/nav/map_server/manifest.xml 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/manifest.xml 2009-04-27 21:34:34 UTC (rev 14484)
@@ -12,6 +12,7 @@
<depend package="roscpp"/>
<depend package="rospy" />
<depend package="robot_srvs"/>
+ <depend package="bullet"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread`"/>
</export>
Modified: pkg/trunk/nav/map_server/src/image_loader.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/image_loader.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/src/image_loader.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -74,17 +74,21 @@
}
// Copy the image data into the map structure
- resp->map.width = img->w;
- resp->map.height = img->h;
- resp->map.resolution = res;
+ resp->map.info.width = img->w;
+ resp->map.info.height = img->h;
+ resp->map.info.resolution = res;
/// @todo Make the map's origin configurable, probably from within the
/// comment section of the image file.
- resp->map.origin.x = 0.0;
- resp->map.origin.y = 0.0;
- resp->map.origin.th = 0.0;
+ resp->map.info.origin.position.x = 0.0;
+ resp->map.info.origin.position.y = 0.0;
+ resp->map.info.origin.position.z = 0.0;
+ resp->map.info.origin.orientation.x = 0.0;
+ resp->map.info.origin.orientation.y = 0.0;
+ resp->map.info.origin.orientation.z = 0.0;
+ resp->map.info.origin.orientation.w = 1.0;
// Allocate space to hold the data
- resp->map.set_data_size(resp->map.width * resp->map.height);
+ resp->map.set_data_size(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
@@ -92,9 +96,9 @@
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
- for(j = 0; j < resp->map.height; j++)
+ for(j = 0; j < resp->map.info.height; j++)
{
- for (i = 0; i < resp->map.width; i++)
+ for (i = 0; i < resp->map.info.width; i++)
{
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i*n_channels;
@@ -117,11 +121,11 @@
/// @todo Make the color thresholds configurable, probably from
/// within the comments section of the image file.
if(occ > 0.65)
- resp->map.data[MAP_IDX(resp->map.width,i,resp->map.height - j - 1)] = +100;
+ resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = +100;
else if(occ < 0.1)
- resp->map.data[MAP_IDX(resp->map.width,i,resp->map.height - j - 1)] = 0;
+ resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
else
- resp->map.data[MAP_IDX(resp->map.width,i,resp->map.height - j - 1)] = -1;
+ resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
}
}
Modified: pkg/trunk/nav/map_server/src/main.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/main.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/src/main.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -146,15 +146,15 @@
printf("[map_server] loading map from image \"%s\"\n", fname);
map_server::loadMapFromFile(&ms.map_resp_,fname,res,negate);
printf("[map_server] read a %d X %d map @ %.3lf m/cell\n",
- ms.map_resp_.map.width,
- ms.map_resp_.map.height,
- ms.map_resp_.map.resolution);
-
+ ms.map_resp_.map.info.width,
+ ms.map_resp_.map.info.height,
+ ms.map_resp_.map.info.resolution);
+ ///\todo This could be optimzed regarding ticket:937
ms.meta_data_message_.map_load_time = ros::Time::now();
- ms.meta_data_message_.resolution = ms.map_resp_.map.resolution;
- ms.meta_data_message_.width = ms.map_resp_.map.width;
- ms.meta_data_message_.height = ms.map_resp_.map.height;
- ms.meta_data_message_.origin = ms.map_resp_.map.origin;
+ ms.meta_data_message_.resolution = ms.map_resp_.map.info.resolution;
+ ms.meta_data_message_.width = ms.map_resp_.map.info.width;
+ ms.meta_data_message_.height = ms.map_resp_.map.info.height;
+ ms.meta_data_message_.origin = ms.map_resp_.map.info.origin;
ms.advertiseService("static_map", &MapServer::mapCallback);
ms.advertise("map_metadata", ms.meta_data_message_, &MapServer::metadataSubscriptionCallback, 1);
Modified: pkg/trunk/nav/map_server/src/map_saver.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/map_saver.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/src/map_saver.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -57,7 +57,7 @@
#include "ros/node.h"
#include "robot_srvs/StaticMap.h"
-
+#include "LinearMath/btMatrix3x3.h"
using namespace std;
/**
@@ -82,18 +82,18 @@
usleep(1000000);
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
FILE* out = fopen("map.pgm", "w");
fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n",
- resp.map.resolution, resp.map.width, resp.map.height);
- for(unsigned int y = 0; y < resp.map.height; y++) {
- for(unsigned int x = 0; x < resp.map.width; x++) {
- unsigned int i = x + (resp.map.height - y - 1) * resp.map.width;
+ resp.map.info.resolution, resp.map.info.width, resp.map.info.height);
+ for(unsigned int y = 0; y < resp.map.info.height; y++) {
+ for(unsigned int x = 0; x < resp.map.info.width; x++) {
+ unsigned int i = x + (resp.map.info.height - y - 1) * resp.map.info.width;
if (resp.map.data[i] == 0) { //occ [0,0.1)
fputc(254, out);
} else if (resp.map.data[i] == +100) { //occ (0.65,1]
@@ -120,9 +120,13 @@
unknown: [129]
*/
+ robot_msgs::Quaternion & orientation = resp.map.info.origin.orientation;
+ btMatrix3x3 mat(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
+ double yaw, pitch, roll;
+ mat.getEulerZYX(yaw, pitch, roll);
fprintf(yaml, "resolution: %f\norigin: [%f, %f, %f]\n#\nvacant: [254]\noccupied: [0]\ninterpolate: 0\nunknown: [206]\n",
- resp.map.resolution, resp.map.origin.x, resp.map.origin.y, resp.map.origin.th);
+ resp.map.info.resolution, resp.map.info.origin.position.x, resp.map.info.origin.position.y, yaw);
fclose(yaml);
Modified: pkg/trunk/nav/map_server/test/rtest.cpp
===================================================================
--- pkg/trunk/nav/map_server/test/rtest.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/test/rtest.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -94,10 +94,10 @@
}
}
ASSERT_TRUE(call_result);
- ASSERT_FLOAT_EQ(resp.map.resolution, g_valid_image_res);
- ASSERT_EQ(resp.map.width, g_valid_image_width);
- ASSERT_EQ(resp.map.height, g_valid_image_height);
- for(unsigned int i=0; i < resp.map.width * resp.map.height; i++)
+ ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res);
+ ASSERT_EQ(resp.map.info.width, g_valid_image_width);
+ ASSERT_EQ(resp.map.info.height, g_valid_image_height);
+ for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++)
ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]);
}
catch(...)
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -31,6 +31,8 @@
#include "ogre_tools/wx_ogre_render_window.h"
+#include "robot_srvs/StaticMap.h"
+
#include "ros/common.h"
#include "ros/node.h"
#include <tf/transform_listener.h>
@@ -207,15 +209,15 @@
return;
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- map_resolution_ = resp.map.resolution;
+ map_resolution_ = resp.map.info.resolution;
// Pad dimensions to power of 2
- map_width_ = resp.map.width;//(int)pow(2,ceil(log2(resp.map.width)));
- map_height_ = resp.map.height;//(int)pow(2,ceil(log2(resp.map.height)));
+ map_width_ = resp.map.info.width;//(int)pow(2,ceil(log2(resp.map.info.width)));
+ map_height_ = resp.map.info.height;//(int)pow(2,ceil(log2(resp.map.info.height)));
//printf("Padded dimensions to %d X %d\n", map_width_, map_height_);
@@ -224,14 +226,14 @@
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
- for(unsigned int j=0;j<resp.map.height;j++)
+ for(unsigned int j=0;j<resp.map.info.height;j++)
{
- for(unsigned int i=0;i<resp.map.width;i++)
+ for(unsigned int i=0;i<resp.map.info.width;i++)
{
unsigned char val;
- if(resp.map.data[j*resp.map.width+i] == 100)
+ if(resp.map.data[j*resp.map.info.width+i] == 100)
val = 0;
- else if(resp.map.data[j*resp.map.width+i] == 0)
+ else if(resp.map.data[j*resp.map.info.width+i] == 0)
val = 255;
else
val = 127;
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-27 21:34:34 UTC (rev 14484)
@@ -36,7 +36,6 @@
#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/Polyline.h"
#include "robot_msgs/PoseWithCovariance.h"
-#include "robot_srvs/StaticMap.h"
#include <OGRE/OgreTexture.h>
#include <OGRE/OgreMaterial.h>
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -314,23 +314,27 @@
/// @todo Dynamically determine bounding box for map
GMapping::Point wmin(-100.0, -100.0);
GMapping::Point wmax(100.0, 100.0);
- map_.map.resolution = delta_;
- map_.map.origin.x = wmin.x;
- map_.map.origin.y = wmin.y;
- map_.map.origin.th = 0.0;
+ map_.map.info.resolution = delta_;
+ map_.map.info.origin.position.x = wmin.x;
+ map_.map.info.origin.position.y = wmin.y;
+ map_.map.info.origin.position.z = 0.0;
+ map_.map.info.origin.orientation.x = 0.0;
+ map_.map.info.origin.orientation.y = 0.0;
+ map_.map.info.origin.orientation.z = 0.0;
+ map_.map.info.origin.orientation.w = 1.0;
GMapping::Point center;
center.x=(wmin.x + wmax.x) / 2.0;
center.y=(wmin.y + wmax.y) / 2.0;
GMapping::ScanMatcherMap smap(center, wmin.x, wmin.y, wmax.x, wmax.y,
- map_.map.resolution);
+ map_.map.info.resolution);
GMapping::IntPoint imin = smap.world2map(wmin);
GMapping::IntPoint imax = smap.world2map(wmax);
- map_.map.width = imax.x - imin.x;
- map_.map.height = imax.y - imin.y;
- map_.map.data.resize(map_.map.width * map_.map.height);
+ map_.map.info.width = imax.x - imin.x;
+ map_.map.info.height = imax.y - imin.y;
+ map_.map.data.resize(map_.map.info.width * map_.map.info.height);
ROS_DEBUG("Trajectory tree:");
for(GMapping::GridSlamProcessor::TNode* n = best.node;
@@ -360,11 +364,11 @@
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
- map_.map.data[MAP_IDX(map_.map.width, x, y)] = -1;
+ map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > 0.1)
- map_.map.data[MAP_IDX(map_.map.width, x, y)] = (int)round(occ*100.0);
+ map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
else
- map_.map.data[MAP_IDX(map_.map.width, x, y)] = 0;
+ map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
got_map_ = true;
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -304,13 +304,13 @@
d.sleep();
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
char* mapdata;
int sx, sy;
- sx = resp.map.width;
- sy = resp.map.height;
+ sx = resp.map.info.width;
+ sy = resp.map.info.height;
// Convert to player format
mapdata = new char[sx*sy];
for(int i=0;i<sx*sy;i++)
@@ -342,7 +342,7 @@
}
delete[] mapdata;
- this->plan->scale = resp.map.resolution;
+ this->plan->scale = resp.map.info.resolution;
this->plan->size_x = sx;
this->plan->size_y = sy;
this->plan->origin_x = 0.0;
Modified: pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg 2009-04-27 21:34:34 UTC (rev 14484)
@@ -8,4 +8,4 @@
uint32 height
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
-deprecated_msgs/Pose2DFloat32 origin
\ No newline at end of file
+robot_msgs/Pose origin
\ No newline at end of file
Added: pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg 2009-04-27 21:34:34 UTC (rev 14484)
@@ -0,0 +1,10 @@
+# A 2-D grid map, in which each cell represents the probability of
+# occupancy. Occupancy values are integers in the range [0,100], or -1
+# for unknown.
+
+#MetaData for the map
+MapMetaData info
+
+# The map data, in row-major order, starting with (0,0). Occupancy
+# probabilities are in the range [0,100]. Unknown is -1.
+byte[] data
Deleted: pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg 2009-04-27 21:34:34 UTC (rev 14484)
@@ -1,16 +0,0 @@
-# A 2-D grid map, in which each cell represents the probability of
-# occupancy. Occupancy values are integers in the range [0,100], or -1
-# for unknown.
-
-# The map resolution [m/cell]
-float32 resolution
-# Map width [cells]
-uint32 width
-# Map height [cells]
-uint32 height
-# The origin of the map [m, m, rad]. This is the real-world pose of the
-# cell (0,0) in the map.
-deprecated_msgs/Pose2DFloat32 origin
-# The map data, in row-major order, starting with (0,0). Occupancy
-# probabilities are in the range [0,100]. Unknown is -1.
-byte[] data
Modified: pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv 2009-04-27 21:34:34 UTC (rev 14484)
@@ -1,2 +1,2 @@
---
-robot_msgs/OccMap2D map
+robot_msgs/OccGrid map
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -194,15 +194,15 @@
clear();
ROS_DEBUG("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- resolution_ = resp.map.resolution;
+ resolution_ = resp.map.info.resolution;
// Pad dimensions to power of 2
- width_ = resp.map.width;//(int)pow(2,ceil(log2(resp.map.width)));
- height_ = resp.map.height;//(int)pow(2,ceil(log2(resp.map.height)));
+ width_ = resp.map.info.width;//(int)pow(2,ceil(log2(resp.map.info.width)));
+ height_ = resp.map.info.height;//(int)pow(2,ceil(log2(resp.map.info.height)));
//printf("Padded dimensions to %d X %d\n", width_, height_);
@@ -211,14 +211,14 @@
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
- for(unsigned int j=0;j<resp.map.height;j++)
+ for(unsigned int j=0;j<resp.map.info.height;j++)
{
- for(unsigned int i=0;i<resp.map.width;i++)
+ for(unsigned int i=0;i<resp.map.info.width;i++)
{
unsigned char val;
- if(resp.map.data[j*resp.map.width+i] == 100)
+ if(resp.map.data[j*resp.map.info.width+i] == 100)
val = 0;
- else if(resp.map.data[j*resp.map.width+i] == 0)
+ else if(resp.map.data[j*resp.map.info.width+i] == 0)
val = 255;
else
val = 127;
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-27 21:34:34 UTC (rev 14484)
@@ -41,7 +41,6 @@
#include <ros/console.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/observation_buffer.h>
-#include <robot_srvs/StaticMap.h>
#include <robot_msgs/Polyline.h>
#include <map>
#include <vector>
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -36,6 +36,9 @@
*********************************************************************/
#include <costmap_2d/costmap_2d_ros.h>
+#include <robot_srvs/StaticMap.h>
+
+
using namespace std;
using namespace tf;
using namespace robot_msgs;
@@ -141,7 +144,7 @@
usleep(1000000);
}
ROS_INFO("Received a %d X %d map at %f m/pix\n",
- map_resp.map.width, map_resp.map.height, map_resp.map.resolution);
+ map_resp.map.info.width, map_resp.map.info.height, map_resp.map.info.resolution);
//check if the user has set any parameters that will be overwritten
bool user_map_params = false;
@@ -156,16 +159,16 @@
// We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
// our planner and controller do not reason about the no obstacle case
- unsigned int numCells = map_resp.map.width * map_resp.map.height;
+ unsigned int numCells = map_resp.map.info.width * map_resp.map.info.height;
for(unsigned int i = 0; i < numCells; i++){
input_data.push_back((unsigned char) map_resp.map.data[i]);
}
- map_width = (unsigned int)map_resp.map.width;
- map_height = (unsigned int)map_resp.map.height;
- map_resolution = map_resp.map.resolution;
- map_origin_x = map_resp.map.origin.x;
- map_origin_y = map_resp.map.origin.y;
+ map_width = (unsigned int)map_resp.map.info.width;
+ map_height = (unsigned int)map_resp.map.info.height;
+ map_resolution = map_resp.map.info.resolution;
+ map_origin_x = map_resp.map.info.origin.position.x;
+ map_origin_y = map_resp.map.info.origin.position.y;
}
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -80,12 +80,12 @@
usleep(1000000);
}
ROS_INFO("Received a %d X %d map at %f m/pix\n",
- map_resp.map.width, map_resp.map.height, map_resp.map.resolution);
+ map_resp.map.info.width, map_resp.map.info.height, map_resp.map.info.resolution);
// We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
// our planner and controller do not reason about the no obstacle case
std::vector<unsigned char> input_data;
- unsigned int numCells = map_resp.map.width * map_resp.map.height;
+ unsigned int numCells = map_resp.map.info.width * map_resp.map.info.height;
for(unsigned int i = 0; i < numCells; i++){
input_data.push_back((unsigned char) map_resp.map.data[i]);
}
@@ -93,8 +93,8 @@
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
- new_costmap_ = new Costmap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
- map_resp.map.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
+ new_costmap_ = new Costmap2D((unsigned int)map_resp.map.info.width, (unsigned int)map_resp.map.info.height,
+ map_resp.map.info.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
Modified: pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_map.h
===================================================================
--- pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_map.h 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_map.h 2009-04-27 21:34:34 UTC (rev 14484)
@@ -40,7 +40,6 @@
#include <ros/node.h>
#include <string>
#include <ros/console.h>
-#include <robot_srvs/StaticMap.h>
#include "topological_map.h"
using std::string;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
