Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13193] pkg/trunk
|
From: <ei...@us...> - 2009-03-31 23:57:05
|
Revision: 13193
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13193&view=rev
Author: eitanme
Date: 2009-03-31 23:57:01 +0000 (Tue, 31 Mar 2009)
Log Message:
-----------
r19709@cib: eitan | 2009-03-20 16:31:43 -0700
Initial commit of new costmap... still a lot of work to be done
Added Paths:
-----------
pkg/trunk/world_models/new_costmap/
pkg/trunk/world_models/new_costmap/CMakeLists.txt
pkg/trunk/world_models/new_costmap/Makefile
pkg/trunk/world_models/new_costmap/include/
pkg/trunk/world_models/new_costmap/include/new_costmap/
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/manifest.xml
pkg/trunk/world_models/new_costmap/src/
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19709
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Added: pkg/trunk/world_models/new_costmap/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/new_costmap/CMakeLists.txt (rev 0)
+++ pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+set(ROS_BUILD_TYPE Release)
+rospack(new_costmap)
+genmsg()
+
+#rospack_add_boost_directories()
+
+rospack_add_library(new_costmap_2d src/costmap_2d.cpp)
+#rospack_link_boost(costmap_2d thread)
Added: pkg/trunk/world_models/new_costmap/Makefile
===================================================================
--- pkg/trunk/world_models/new_costmap/Makefile (rev 0)
+++ pkg/trunk/world_models/new_costmap/Makefile 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,191 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef COSTMAP_COSTMAP_2D_H_
+#define COSTMAP_COSTMAP_2D_H_
+
+#include <vector>
+#include <new_costmap/observation.h>
+
+namespace costmap_2d {
+ /**
+ * @class Costmap
+ * @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
+ */
+ class Costmap2D {
+ public:
+ /**
+ * @brief Constructor for a costmap
+ * @param meters_size_x The x size of the map in meters
+ * @param meters_size_y The y size of the map in meters
+ * @param resolution The resolution of the map in meters/cell
+ * @param origin_x The x origin of the map
+ * @param origin_y The y origin of the map
+ */
+ Costmap2D(double meters_size_x, double meters_size_y,
+ double resolution, double origin_x, double origin_y);
+
+ /**
+ * @brief Constructor for a costmap
+ * @param cells_size_x The x size of the map in cells
+ * @param cells_size_y The y size of the map in cells
+ * @param resolution The resolution of the map in meters/cell
+ * @param origin_x The x origin of the map
+ * @param origin_y The y origin of the map
+ */
+ Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
+ double resolution, double origin_x, double origin_y);
+
+ /**
+ * @brief Get the cost of a cell in the costmap
+ * @param mx The x coordinate of the cell
+ * @param my The y coordinate of the cell
+ * @return The cost of the cell
+ */
+ unsigned char getCellCost(unsigned int mx, unsigned int my) const;
+
+ /**
+ * @brief Convert from map coordinates to world coordinates
+ * @param mx The x map coordinate
+ * @param my The y map coordinate
+ * @param wx Will be set to the associated world x coordinate
+ * @param wy Will be set to the associated world y coordinate
+ */
+ void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
+
+ /**
+ * @brief Convert from map coordinates to world coordinates
+ * @param wx The x world coordinate
+ * @param wy The y world coordinate
+ * @param mx Will be set to the associated map x coordinate
+ * @param my Will be set to the associated map y coordinate
+ * @return true if the conversion was successful (legal bounds) false otherwise
+ */
+ bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+
+ /**
+ * @brief Convert from map coordinates to world coordinates without checking for legal bounds
+ * @param wx The x world coordinate
+ * @param wy The y world coordinate
+ * @param mx Will be set to the associated map x coordinate
+ * @param my Will be set to the associated map y coordinate
+ */
+ void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+
+ /**
+ * @brief Revert to the static map outside of a specified window centered at a world coordinate
+ * @param wx The x coordinate of the center point of the window in world space (meters)
+ * @param wy The y coordinate of the center point of the window in world space (meters)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ */
+ void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
+
+ /**
+ * @brief Insert new obstacles into the cost map
+ * @param obstacles The point clouds of obstacles to insert into the map
+ */
+ void updateObstacles(const std::vector<Observation>& obstacles);
+
+ /**
+ * @brief Clear freespace based on any number of planar scans
+ * @param clearing_scans The scans used to raytrace
+ */
+ void raytraceFreespace(const std::vector<Observation>& clearing_scans);
+
+ /**
+ * @brief Inflate obstacles within a given window centered at a point in world coordinates
+ * @param wx The x coordinate of the center point of the window in world space (meters)
+ * @param wy The y coordinate of the center point of the window in world space (meters)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ */
+ void inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y);
+
+ /**
+ * @brief Accessor for the x size of the costmap in cells
+ * @return The x size of the costmap
+ */
+ unsigned int cellSizeX() const;
+
+ /**
+ * @brief Accessor for the y size of the costmap in cells
+ * @return The y size of the costmap
+ */
+ unsigned int cellSizeY() const;
+
+ /**
+ * @brief Accessor for the x size of the costmap in meters
+ * @return The x size of the costmap
+ */
+ double metersSizeX() const;
+
+ /**
+ * @brief Accessor for the y size of the costmap in meters
+ * @return The y size of the costmap
+ */
+ double metersSizeY() const;
+
+ /**
+ * @brief Accessor for the x origin of the costmap
+ * @return The x origin of the costmap
+ */
+ double originX() const;
+
+ /**
+ * @brief Accessor for the y origin of the costmap
+ * @return The y origin of the costmap
+ */
+ double originY() const;
+
+ /**
+ * @brief Accessor for the resolution of the costmap
+ * @return The resolution of the costmap
+ */
+ double resolution() const;
+
+ private:
+ unsigned int size_x_;
+ unsigned int size_y_;
+ double resolution_;
+ double origin_x_;
+ double origin_y_;
+ unsigned char* static_map_;
+ unsigned char* cost_map_;
+ };
+};
+
+#endif
Added: pkg/trunk/world_models/new_costmap/manifest.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/manifest.xml (rev 0)
+++ pkg/trunk/world_models/new_costmap/manifest.xml 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,18 @@
+<package>
+<description>Cost map maintenance library</description>
+<author>Eitan Marder-Eppstein</author>
+<license>BSD</license>
+<review status="unreviewed" notes=""/>
+<depend package="rosconsole"/>
+<depend package="roscpp" />
+<depend package="std_msgs" />
+<depend package="robot_msgs" />
+<depend package="laser_scan" />
+<depend package="pr2_msgs" />
+<depend package="tf" />
+<depend package="robot_filter" />
+<depend package="robot_srvs" />
+<export>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap"/>
+</export>
+</package>
Added: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp (rev 0)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,159 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <new_costmap/costmap_2d.h>
+
+using namespace std;
+
+namespace costmap_2d{
+
+ Costmap2D::Costmap2D(double meters_size_x, double meters_size_y,
+ double resolution, double origin_x, double origin_y) : resolution_(resolution),
+ origin_x_(origin_x), origin_y_(origin_y) {
+ //make sure that we have a legal sized cost_map
+ ROS_ASSERT_MSG(meters_size_x > 0 && meters_size_y > 0, "Both the x and y dimensions of the costmap must be greater than 0.");
+
+ size_x_ = (unsigned int) (meters_size_x / resolution);
+ size_y_ = (unsigned int) (meters_size_y / resolution);
+ }
+
+ Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
+ double resolution, double origin_x, double origin_y) : size_x_(cells_size_x),
+ size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y) {}
+
+ unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
+ ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
+ return cost_map_[my * size_x_ + mx];
+ }
+
+ void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const {
+ wx = origin_x_ + (mx + 0.5) * resolution_;
+ wy = origin_y_ + (my + 0.5) * resolution_;
+ }
+
+ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
+ if(wx < origin_x_ || wy < origin_y_)
+ return false;
+
+ mx = (int) ((wx - origin_x_) / resolution_);
+ my = (int) ((wy - origin_y_) / resolution_);
+
+ if(mx < size_x_ && my < size_y_)
+ return true;
+
+ return false;
+ }
+
+ void Costmap2D::worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const {
+ mx = (int) ((wx - origin_x_) / resolution_);
+ my = (int) ((wy - origin_y_) / resolution_);
+ }
+
+ void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
+ ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
+
+ double start_point_x = wx - w_size_x / 2;
+ double start_point_y = wy - w_size_y / 2;
+
+ unsigned int start_x, start_y, end_x, end_y;
+ //we'll do our own bounds checking for the window
+ worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+ worldToMapNoBounds(start_point_x + w_size_x, start_point_y + w_size_y, end_x, end_y);
+
+ //check start bounds
+ start_x = start_point_x < origin_x_ ? 0 : start_x;
+ start_y = start_point_y < origin_y_ ? 0 : start_y;
+
+ //check end bounds
+ end_x = end_x > size_x_ ? size_x_ : end_x;
+ end_y = end_y > size_y_ ? size_y_ : end_y;
+
+ unsigned int cell_size_x = end_x - start_x;
+ unsigned int cell_size_y = end_y - start_y;
+
+ //we need a map to store the obstacles in the window temporarily
+ unsigned char local_map[cell_size_x * cell_size_y];
+
+ //copy the local window in the costmap to the local map
+ unsigned int cost_map_index = start_y * size_x_ + start_x;
+ unsigned int local_map_index = 0;
+ for(unsigned int y = 0; y < cell_size_y; ++y){
+ for(unsigned int x = 0; x < cell_size_x; ++x){
+ local_map[local_map_index] = cost_map_[cost_map_index];
+ local_map_index++;
+ cost_map_index++;
+ }
+ local_map_index += cell_size_x;
+ cost_map_index += size_x_;
+ }
+
+ //now we'll reset the costmap to the static map
+ memcpy(cost_map_, static_map_, size_x_ * size_y_);
+
+ //now we want to copy the local map back into the costmap
+ cost_map_index = start_y * size_x_ + start_x;
+ local_map_index = 0;
+ for(unsigned int y = 0; y < cell_size_y; ++y){
+ for(unsigned int x = 0; x < cell_size_x; ++x){
+ cost_map_[cost_map_index] = local_map[local_map_index];
+ local_map_index++;
+ cost_map_index++;
+ }
+ local_map_index += cell_size_x;
+ cost_map_index += size_x_;
+ }
+ }
+
+ void Costmap2D::updateObstacles(const std::vector<Observation>& obstacles){}
+
+ void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
+
+ void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
+
+ unsigned int Costmap2D::cellSizeX() const{}
+
+ unsigned int Costmap2D::cellSizeY() const{}
+
+ double Costmap2D::metersSizeX() const{}
+
+ double Costmap2D::metersSizeY() const{}
+
+ double Costmap2D::originX() const{}
+
+ double Costmap2D::originY() const{}
+
+ double Costmap2D::resolution() const{}
+};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
