Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13215] pkg/trunk
|
From: <ei...@us...> - 2009-04-01 02:13:46
|
Revision: 13215
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13215&view=rev
Author: eitanme
Date: 2009-04-01 02:13:41 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
New costmap merge
r20509@cib: eitan | 2009-03-31 13:25:01 -0700
Wrote a ROS node that wraps a costmap. Also, wrote observation buffers that pass information to a costmap
r20581@cib: eitan | 2009-03-31 16:46:35 -0700
Added more parameters
r20582@cib: eitan | 2009-03-31 16:48:25 -0700
Added non-templated base class for MessageNotifier to allow for common storage of notifiers listening to different message types
r20583@cib: eitan | 2009-03-31 16:55:01 -0700
Don't need to raytrace as far
Modified Paths:
--------------
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
pkg/trunk/prcore/tf/include/tf/message_notifier.h
pkg/trunk/world_models/new_costmap/CMakeLists.txt
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/costmap_2d.cpp
pkg/trunk/world_models/new_costmap/src/costmap_test.cpp
Added Paths:
-----------
pkg/trunk/prcore/tf/include/tf/message_notifier_base.h
pkg/trunk/world_models/new_costmap/costmap_test.xml
pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h
pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h
pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h
pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h
pkg/trunk/world_models/new_costmap/launch_map.xml
pkg/trunk/world_models/new_costmap/src/costmap_2d_ros.cpp
pkg/trunk/world_models/new_costmap/src/observation_buffer.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:/costmap_rework_branch:20298
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:20583
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
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -26,8 +26,8 @@
<param name="costmap_2d/inflation_radius" value="0.55"/>
<param name="costmap_2d/circumscribed_radius" value="0.46"/>
<param name="costmap_2d/inscribed_radius" value="0.325"/>
- <param name="costmap_2d/raytrace_window" value="10.0"/>
- <param name="costmap_2d/raytrace_range" value="10.0"/>
+ <param name="costmap_2d/raytrace_window" value="4.0"/>
+ <param name="costmap_2d/raytrace_range" value="4.0"/>
<param name="costmap_2d/weight" value="0.1"/>
<param name="costmap_2d/zLB" value="0.10"/>
<param name="costmap_2d/zUB" value="0.30"/>
Modified: pkg/trunk/prcore/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/message_notifier.h 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/prcore/tf/include/tf/message_notifier.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -35,6 +35,7 @@
#include <ros/node.h>
#include <tf/tf.h>
#include <tf/tfMessage.h>
+#include <tf/message_notifier_base.h>
#include <string>
#include <list>
@@ -95,7 +96,7 @@
\endverbatim
*/
template<class Message>
-class MessageNotifier
+class MessageNotifier : public MessageNotifierBase
{
public:
typedef boost::shared_ptr<Message> MessagePtr;
Added: pkg/trunk/prcore/tf/include/tf/message_notifier_base.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/message_notifier_base.h (rev 0)
+++ pkg/trunk/prcore/tf/include/tf/message_notifier_base.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,92 @@
+/*********************************************************************
+*
+* 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 MESSAGE_NOTIFIER_BASE_H_
+#define MESSAGE_NOTIFIER_BASE_H_
+#include <ros/node.h>
+#include <tf/tf.h>
+#include <tf/tfMessage.h>
+#include <tf/message_notifier_base.h>
+
+#include <string>
+#include <list>
+#include <vector>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread.hpp>
+
+namespace tf {
+ /**
+ * @class MessageNotifierBase
+ * @brief An abstract base class for the various types of message notifiers
+ */
+ class MessageNotifierBase
+ {
+ public:
+
+ /**
+ * \brief Destructor
+ */
+ virtual ~MessageNotifierBase(){}
+
+ /**
+ * \brief Set the frame you need to be able to transform to before getting a message callback
+ */
+ virtual void setTargetFrame(const std::string& target_frame) = 0;
+
+ /**
+ * \brief Set the topic to listen on
+ */
+ virtual void setTopic(const std::string& topic) = 0;
+
+ /**
+ * \brief Set the required tolerance for the notifier to return true
+ */
+ virtual void setTolerance(const ros::Duration& tolerance) = 0;
+
+ /**
+ * \brief Clear any messages currently in the queue
+ */
+ virtual void clear() = 0;
+
+
+ protected:
+ MessageNotifierBase(){}
+
+ };
+};
+#endif
Modified: pkg/trunk/world_models/new_costmap/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 02:13:41 UTC (rev 13215)
@@ -6,8 +6,8 @@
#rospack_add_boost_directories()
-rospack_add_library(new_costmap_2d src/costmap_2d.cpp)
+rospack_add_library(new_costmap_2d src/costmap_2d.cpp src/observation_buffer.cpp src/costmap_2d_ros.cpp)
#rospack_link_boost(costmap_2d thread)
-rospack_add_executable(costmap_test src/costmap_test.cpp)
+rospack_add_executable(costmap_test src/costmap_2d_ros.cpp)
target_link_libraries(costmap_test new_costmap_2d)
Added: pkg/trunk/world_models/new_costmap/costmap_test.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/costmap_test.xml (rev 0)
+++ pkg/trunk/world_models/new_costmap/costmap_test.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,12 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <node pkg="new_costmap" type="costmap_test" respawn="false" name="new_cost_map" output="screen">
+ <param name="costmap/observation_topics" value="base_scan" />
+ <!--<param name="costmap/base_scan/sensor_frame" value="base_link" />-->
+ <param name="costmap/base_scan/observation_persistance" value="0.0" />
+ <param name="costmap/base_scan/expected_update_rate" value="0.2" />
+ <param name="costmap/base_scan/data_type" value="LaserScan" />
+ </node>
+ </group>
+</launch>
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,72 @@
+/*********************************************************************
+*
+* 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_CELL_DATA_H_
+#define COSTMAP_CELL_DATA_H_
+namespace costmap_2d {
+ /**
+ * @class CellData
+ * @brief Storage for cell information used during obstacle inflation
+ */
+ class CellData {
+ public:
+ /**
+ * @brief Constructor for a CellData object
+ * @param d The distance to the nearest obstacle, used for ordering in the priority queue
+ * @param i The index of the cell in the cost map
+ * @param x The x coordinate of the cell in the cost map
+ * @param y The y coordinate of the cell in the cost map
+ * @param sx The x coordinate of the closest obstacle cell in the costmap
+ * @param sy The y coordinate of the closest obstacle cell in the costmap
+ * @return
+ */
+ CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : distance_(d),
+ index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
+ double distance_;
+ unsigned int index_;
+ unsigned int x_, y_;
+ unsigned int src_x_, src_y_;
+ };
+
+ /**
+ * @brief Provide an ordering between CellData objects in the priority queue
+ * @return We want the lowest distance to have the highest priority... so this returns true if a has higher priority than b
+ */
+ inline bool operator<(const CellData &a, const CellData &b){
+ return a.distance_ > b.distance_;
+ }
+};
+#endif
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,45 @@
+/*********************************************************************
+*
+* 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_COST_VALUES_H_
+#define COSTMAP_COST_VALUES_H_
+/** Provides a mapping for often used cost values */
+namespace costmap_2d {
+ static const unsigned char NO_INFORMATION = 255;
+ static const unsigned char LETHAL_OBSTACLE = 254;
+ static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
+};
+#endif
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -40,28 +40,11 @@
#include <vector>
#include <queue>
#include <new_costmap/observation.h>
+#include <new_costmap/cell_data.h>
+#include <new_costmap/cost_values.h>
#include <robot_msgs/PointCloud.h>
namespace costmap_2d {
- static const unsigned char NO_INFORMATION = 255;
- static const unsigned char LETHAL_OBSTACLE = 254;
- static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
-
- //for priority queue propagation
- class CellData {
- public:
- CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : distance_(d),
- index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
- double distance_;
- unsigned int index_;
- unsigned int x_, y_;
- unsigned int src_x_, src_y_;
- };
-
- inline bool operator<(const CellData &a, const CellData &b){
- return a.distance_ > b.distance_;
- }
-
/**
* @class Costmap
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
@@ -122,6 +105,14 @@
unsigned char getCost(unsigned int mx, unsigned int my) const;
/**
+ * @brief Set the cost of a cell in the costmap
+ * @param mx The x coordinate of the cell
+ * @param my The y coordinate of the cell
+ * @param cost The cost to set the cell to
+ */
+ void setCost(unsigned int mx, unsigned int my, unsigned char cost);
+
+ /**
* @brief Convert from map coordinates to world coordinates
* @param mx The x map coordinate
* @param my The y map coordinate
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,100 @@
+/*********************************************************************
+ *
+ * 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_ROS_H_
+#define COSTMAP_COSTMAP_2D_ROS_H_
+
+#include <ros/node.h>
+#include <ros/console.h>
+#include <new_costmap/costmap_2d.h>
+#include <new_costmap/observation_buffer.h>
+#include <robot_srvs/StaticMap.h>
+#include <robot_msgs/Polyline2D.h>
+#include <map>
+#include <vector>
+#include <string>
+#include <sstream>
+
+#include <tf/transform_datatypes.h>
+#include <tf/message_notifier_base.h>
+#include <tf/message_notifier.h>
+#include <tf/transform_listener.h>
+
+#include <laser_scan/LaserScan.h>
+#include <laser_scan/laser_scan.h>
+
+#include <robot_msgs/PointCloud.h>
+
+// Thread suppport
+#include <boost/thread.hpp>
+
+using namespace costmap_2d;
+using namespace tf;
+using namespace robot_msgs;
+
+namespace costmap_2d {
+
+ class Costmap2DROS {
+ public:
+ Costmap2DROS(ros::Node& ros_node);
+ ~Costmap2DROS();
+
+ void laserScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message, ObservationBuffer* buffer);
+ void pointCloudCallback(const tf::MessageNotifier<robot_msgs::PointCloud>::MessagePtr& message, ObservationBuffer* buffer);
+ void spin();
+ void updateMap();
+ void resetWindow();
+ void publishCostMap();
+
+ private:
+ ros::Node& ros_node_;
+ tf::TransformListener tf_; ///< @brief Used for transforming point clouds
+ laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
+ boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
+ Costmap2D* new_costmap_;
+ std::string global_frame_;
+ double freq_;
+ boost::thread* visualizer_thread_;
+ boost::thread* window_reset_thread_;
+
+ std::vector<tf::MessageNotifierBase*> observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
+ std::vector<ObservationBuffer*> observation_buffers_; ///< @brief Used to store observations from various sensors
+
+ };
+};
+
+#endif
+
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,58 @@
+/*
+ * 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, Inc. 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.
+ *
+ * Authors: Conor McGann
+ */
+
+#ifndef COSTMAP_OBSERVATION_H_
+#define COSTMAP_OBSERVATION_H_
+
+
+#include <robot_msgs/Point.h>
+#include <robot_msgs/PointCloud.h>
+
+namespace costmap_2d {
+
+ /**
+ * @brief Stores an observation in terms of a point cloud and the origin of the source
+ * @note Tried to make members and constructor arguments const but the compiler would not accept the default
+ * assignment operator for vector insertion!
+ */
+ class Observation {
+ public:
+ // Structors
+ Observation() : cloud_(){}
+ Observation(robot_msgs::Point& p, robot_msgs::PointCloud cloud): origin_(p), cloud_(cloud) {}
+ Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
+
+ robot_msgs::Point origin_;
+ robot_msgs::PointCloud cloud_;
+ };
+
+}
+#endif
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,80 @@
+/*********************************************************************
+*
+* 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_OBSERVATION_BUFFER_H_
+#define COSTMAP_OBSERVATION_BUFFER_H_
+
+#include <vector>
+#include <list>
+#include <string>
+#include <ros/time.h>
+#include <new_costmap/observation.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_datatypes.h>
+#include <robot_msgs/PointCloud.h>
+
+namespace costmap_2d {
+ /**
+ * @class ObservationBuffer
+ * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
+ */
+ class ObservationBuffer {
+ public:
+ ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
+ tf::TransformListener& tf, std::string global_frame, std::string sensor_frame);
+
+ ~ObservationBuffer();
+
+ //burden is on the user to make sure the transform is available... ie they should use a MessageNotifier
+ void bufferCloud(const robot_msgs::PointCloud& cloud);
+
+ void getObservations(std::vector<Observation>& observations);
+
+ bool isCurrent() const;
+
+ private:
+ void purgeStaleObservations();
+ tf::TransformListener& tf_;
+ const ros::Duration observation_keep_time_;
+ const ros::Duration expected_update_rate_;
+ ros::Time last_updated_;
+ std::string global_frame_;
+ std::string sensor_frame_;
+ std::list<Observation> observation_list_;
+ std::string topic_name_;
+ };
+};
+#endif
Added: pkg/trunk/world_models/new_costmap/launch_map.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/launch_map.xml (rev 0)
+++ pkg/trunk/world_models/new_costmap/launch_map.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,21 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <!--
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm-fake.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
+ -->
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
+ <node pkg="fake_localization" type="fake_localization" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false"/>
+ </group>
+</launch>
Modified: pkg/trunk/world_models/new_costmap/manifest.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/manifest.xml 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/manifest.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -12,7 +12,6 @@
<depend package="tf" />
<depend package="robot_filter" />
<depend package="robot_srvs" />
-<depend package="costmap_2d" />
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap_2d"/>
</export>
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 02:13:41 UTC (rev 13215)
@@ -125,9 +125,14 @@
unsigned char Costmap2D::getCost(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];
+ return cost_map_[getIndex(mx, my)];
}
+ void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) {
+ ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot set the cost of a cell that is outside the bounds of the costmap");
+ cost_map_[getIndex(mx, my)] = cost;
+ }
+
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_;
Added: pkg/trunk/world_models/new_costmap/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d_ros.cpp (rev 0)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d_ros.cpp 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,360 @@
+/*********************************************************************
+ *
+ * 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_ros.h>
+
+using namespace std;
+
+namespace costmap_2d {
+
+ Costmap2DROS::Costmap2DROS(ros::Node& ros_node) : ros_node_(ros_node),
+ tf_(ros_node, true, ros::Duration(10)){
+ ros_node_.advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
+ ros_node_.advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+
+
+ string topics_string;
+ //get the topics that we'll subscribe to from the parameter server
+ ros_node_.param("~costmap/observation_topics", topics_string, string(""));
+ ROS_INFO("Topics: %s", topics_string.c_str());
+
+ ros_node_.param("~costmap/update_frequency", freq_, 5.0);
+ ros_node_.param("~costmap/global_frame", global_frame_, string("map"));
+
+ //now we need to split the topics based on whitespace which we can use a stringstream for
+ stringstream ss(topics_string);
+
+ string topic;
+ while(ss >> topic){
+ //get the parameters for the specific topic
+ double observation_keep_time, expected_update_rate;
+ string sensor_frame, data_type;
+ ros_node_.param("~costmap/" + topic + "/sensor_frame", sensor_frame, string("frame_from_message"));
+ ros_node_.param("~costmap/" + topic + "/observation_persistance", observation_keep_time, 0.0);
+ ros_node_.param("~costmap/" + topic + "/expected_update_rate", expected_update_rate, 0.0);
+ ros_node_.param("~costmap/" + topic + "/data_type", data_type, string("PointCloud"));
+
+ ROS_ASSERT_MSG(data_type == "PointCloud" || data_type == "LaserScan", "Only topics that use point clouds or laser scans are currently supported");
+
+ //create an observation buffer
+ observation_buffers_.push_back(new ObservationBuffer(topic, observation_keep_time, expected_update_rate, tf_, global_frame_, sensor_frame));
+
+ ROS_INFO("Created an observation buffer for topic %s, expected update rate: %.2f, observation persistance: %.2f", topic.c_str(), expected_update_rate, observation_keep_time);
+
+ //create a callback for the topic
+ if(data_type == "LaserScan"){
+ observation_notifiers_.push_back(new MessageNotifier<laser_scan::LaserScan>(&tf_, &ros_node_,
+ boost::bind(&Costmap2DROS::laserScanCallback, this, _1, observation_buffers_.back()), topic, global_frame_, 50));
+ }
+ else{
+ observation_notifiers_.push_back(new MessageNotifier<robot_msgs::PointCloud>(&tf_, &ros_node_,
+ boost::bind(&Costmap2DROS::pointCloudCallback, this, _1, observation_buffers_.back()), topic, global_frame_, 50));
+ }
+
+ }
+
+
+ bool static_map;
+ unsigned int map_width, map_height;
+ double map_resolution;
+ double map_origin_x, map_origin_y;
+
+ ros_node_.param("~costmap/static_map", static_map, true);
+ std::vector<unsigned char> input_data;
+
+ if(static_map){
+ robot_srvs::StaticMap::Request map_req;
+ robot_srvs::StaticMap::Response map_resp;
+ ROS_INFO("Requesting the map...\n");
+ while(!ros::service::call("static_map", map_req, map_resp))
+ {
+ ROS_INFO("Request failed; trying again...\n");
+ 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);
+
+ // 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;
+ 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;
+
+ }
+ else{
+ double map_width_meters, map_height_meters;
+ ros_node_.param("~costmap/map_width", map_width_meters, 10.0);
+ ros_node_.param("~costmap/map_height", map_height_meters, 10.0);
+ ros_node_.param("~costmap/map_resolution", map_resolution, 0.05);
+ ros_node_.param("~costmap/map_origin_x", map_origin_x, 0.0);
+ ros_node_.param("~costmap/map_origin_y", map_origin_y, 0.0);
+ map_width = (unsigned int)(map_width_meters / map_resolution);
+ map_height = (unsigned int)(map_height_meters / map_resolution);
+
+ }
+
+ double inscribed_radius, circumscribed_radius, inflation_radius;
+ ros_node_.param("~costmap/inscribed_radius", inscribed_radius, 0.325);
+ ros_node_.param("~costmap/circumscribed_radius", circumscribed_radius, 0.46);
+ ros_node_.param("~costmap/inflation_radius", inflation_radius, 0.55);
+
+ double obstacle_range, max_obstacle_height, raytrace_range;
+ ros_node_.param("~costmap/obstacle_range", obstacle_range, 2.5);
+ ros_node_.param("~costmap/max_obstacle_height", max_obstacle_height, 2.0);
+ ros_node_.param("~costmap/raytrace_range", raytrace_range, 3.0);
+
+ double cost_scale;
+ ros_node_.param("~costmap/cost_scaling_factor", cost_scale, 1.0);
+
+ int temp_lethal_threshold;
+ ros_node_.param("~costmap/lethal_cost_threshold", temp_lethal_threshold, int(100));
+
+ unsigned char lethal_threshold = max(min(temp_lethal_threshold, 255), 0);
+
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+ new_costmap_ = new Costmap2D(map_width, map_height,
+ map_resolution, map_origin_x, map_origin_y, inscribed_radius, circumscribed_radius, inflation_radius,
+ obstacle_range, max_obstacle_height, raytrace_range, cost_scale, input_data, lethal_threshold);
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_INFO("New map construction time: %.9f", t_diff);
+
+
+ //create a separate thread to publish cost data to the visualizer
+ visualizer_thread_ = new boost::thread(boost::bind(&Costmap2DROS::publishCostMap, this));
+ window_reset_thread_ = new boost::thread(boost::bind(&Costmap2DROS::resetWindow, this));
+
+ }
+
+ Costmap2DROS::~Costmap2DROS(){
+ if(new_costmap_ != NULL)
+ delete new_costmap_;
+
+ if(visualizer_thread_ != NULL)
+ delete visualizer_thread_;
+
+ if(window_reset_thread_ != NULL)
+ delete window_reset_thread_;
+
+ //clean up observation buffers
+ for(unsigned int i = 0; i < observation_buffers_.size(); ++i){
+ if(observation_buffers_[i] != NULL)
+ delete observation_buffers_[i];
+ }
+
+ //clean up message notifiers
+ for(unsigned int i = 0; i < observation_notifiers_.size(); ++i){
+ if(observation_notifiers_[i] != NULL)
+ delete observation_notifiers_[i];
+ }
+ }
+
+ void Costmap2DROS::laserScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message, ObservationBuffer* buffer){
+ //project the laser into a point cloud
+ PointCloud base_cloud;
+ base_cloud.header = message->header;
+ //we want all values... even those out of range
+ projector_.projectLaser(*message, base_cloud, -1.0, true);
+
+ //buffer the point cloud
+ lock_.lock();
+ buffer->bufferCloud(base_cloud);
+ lock_.unlock();
+ }
+
+ void Costmap2DROS::pointCloudCallback(const tf::MessageNotifier<robot_msgs::PointCloud>::MessagePtr& message, ObservationBuffer* buffer){
+ //buffer the point cloud
+ lock_.lock();
+ buffer->bufferCloud(*message);
+ lock_.unlock();
+ }
+
+ void Costmap2DROS::spin(){
+ while(ros_node_.ok()){
+ updateMap();
+ usleep(1e6/freq_);
+ }
+ }
+
+ void Costmap2DROS::updateMap(){
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ global_pose.setIdentity();
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time();
+ try{
+ tf_.transformPose(global_frame_, robot_pose, global_pose);
+ }
+ catch(tf::LookupException& ex) {
+ ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException& ex) {
+ ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ }
+
+ double wx = global_pose.getOrigin().x();
+ double wy = global_pose.getOrigin().y();
+
+ bool current = true;
+ std::vector<Observation> observations;
+ lock_.lock();
+ for(unsigned int i = 0; i < observation_buffers_.size(); ++i){
+ observation_buffers_[i]->getObservations(observations);
+ current = current && observation_buffers_[i]->isCurrent();
+ }
+ lock_.unlock();
+
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+ new_costmap_->updateWorld(wx, wy, observations, observations);
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_INFO("Map update time: %.9f", t_diff);
+ }
+
+ void Costmap2DROS::resetWindow(){
+ while(ros_node_.ok()){
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ global_pose.setIdentity();
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time();
+ try{
+ tf_.transformPose(global_frame_, robot_pose, global_pose);
+ }
+ catch(tf::LookupException& ex) {
+ ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException& ex) {
+ ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ }
+
+ double wx = global_pose.getOrigin().x();
+ double wy = global_pose.getOrigin().y();
+ lock_.lock();
+ ROS_INFO("Resetting map outside window");
+ new_costmap_->resetMapOutsideWindow(wx, wy, 5.0, 5.0);
+ lock_.unlock();
+
+ usleep(1e6/0.2);
+ }
+ }
+
+ void Costmap2DROS::publishCostMap(){
+ while(ros_node_.ok()){
+ ROS_INFO("publishing map");
+ lock_.lock();
+ std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles;
+ for(unsigned int i = 0; i<new_costmap_->cellSizeX(); i++){
+ for(unsigned int j = 0; j<new_costmap_->cellSizeY();j++){
+ double wx, wy;
+ new_costmap_->mapToWorld(i, j, wx, wy);
+ std::pair<double, double> p(wx, wy);
+
+ if(new_costmap_->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+ raw_obstacles.push_back(p);
+ else if(new_costmap_->getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+ inflated_obstacles.push_back(p);
+ }
+ }
+ lock_.unlock();
+
+ // First publish raw obstacles in red
+ robot_msgs::Polyline2D obstacle_msg;
+ obstacle_msg.header.frame_id = global_frame_;
+ unsigned int pointCount = raw_obstacles.size();
+ obstacle_msg.set_points_size(pointCount);
+ obstacle_msg.color.a = 0.0;
+ obstacle_msg.color.r = 1.0;
+ obstacle_msg.color.b = 0.0;
+ obstacle_msg.color.g = 0.0;
+
+ for(unsigned int i=0;i<pointCount;i++){
+ obstacle_msg.points[i].x = raw_obstacles[i].first;
+ obstacle_msg.points[i].y = raw_obstacles[i].second;
+ }
+
+ ros::Node::instance()->publish("raw_obstacles", obstacle_msg);
+
+ // Now do inflated obstacles in blue
+ pointCount = inflated_obstacles.size();
+ obstacle_msg.set_points_size(pointCount);
+ obstacle_msg.color.a = 0.0;
+ obstacle_msg.color.r = 0.0;
+ obstacle_msg.color.b = 1.0;
+ obstacle_msg.color.g = 0.0;
+
+ for(unsigned int i=0;i<pointCount;i++){
+ obstacle_msg.points[i].x = inflated_obstacles[i].first;
+ obstacle_msg.points[i].y = inflated_obstacles[i].second;
+ }
+
+ ros::Node::instance()->publish("inflated_obstacles", obstacle_msg);
+ usleep(1e6/2.0);
+ }
+ }
+
+};
+
+int main(int argc, char** argv){
+ ros::init(argc, argv);
+ ros::Node ros_node("new_costmap_tester");
+ Costmap2DROS tester(ros_node);
+ tester.spin();
+
+ return(0);
+
+}
Modified: pkg/trunk/world_models/new_costmap/src/costmap_test.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_test.cpp 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/src/costmap_test.cpp 2009-04-01 02:13:41 UTC (rev 13215)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <ros/console.h>
#include <new_costmap/costmap_2d.h>
-#include <costmap_2d/costmap_2d.h>
+#include <new_costmap/observation_buffer.h>
#include <robot_srvs/StaticMap.h>
#include <robot_msgs/Polyline2D.h>
#include <map>
@@ -61,12 +61,14 @@
class CostmapTester {
public:
- CostmapTester(ros::Node& ros_node) : ros_node_(ros_node), base_scan_notifier_(NULL), tf_(ros_node, true, ros::Duration(10)), observations_(1), global_frame_("map"), freq_(5) {
+ CostmapTester(ros::Node& ros_node) : ros_node_(ros_node), base_scan_notifier_(NULL), tf_(ros_node, true, ros::Duration(10)), global_frame_("map"), freq_(5), base_scan_buffer_(NULL) {
ros_node.advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
ros_node.advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ base_scan_buffer_ = new ObservationBuffer(0.0, 0.2, tf_, "map", "base_laser");
+
base_scan_notifier_ = new MessageNotifier<laser_scan::LaserScan>(&tf_, &ros_node,
- boost::bind(&CostmapTester::baseScanCallback, this, _1),
+ boost::bind(&CostmapTester::baseScanCallback, this, _1, (int) 1),
"base_scan", global_frame_, 50);
robot_srvs::StaticMap::Request map_req;
@@ -99,17 +101,6 @@
t_diff = end_t - start_t;
ROS_INFO("New map construction time: %.9f", t_diff);
- gettimeofday(&start, NULL);
- old_costmap_ = new CostMap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
- input_data , map_resp.map.resolution,
- 100, 2.0, 15, 25,
- 0.55, 0.46, 0.325, 1.0,
- 10.0, 10.0, 10.0);
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- ROS_INFO("Old map construction time: %.9f", t_diff);
//create a separate thread to publish cost data to the visualizer
visualizer_thread_ = new boost::thread(boost::bind(&CostmapTester::publishCostMap, this));
@@ -119,37 +110,22 @@
~CostmapTester(){
delete new_costmap_;
- delete old_costmap_;
delete base_scan_notifier_;
delete visualizer_thread_;
delete window_reset_thread_;
+ delete base_scan_buffer_;
}
- void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message){
+ void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message, int i){
//project the laser into a point cloud
PointCloud base_cloud;
base_cloud.header = message->header;
//we want all values... even those out of range
projector_.projectLaser(*message, base_cloud, -1.0, true);
- Stamped<btVector3> global_origin;
+ //buffer the point cloud
lock_.lock();
- //we know the transform is available from the laser frame to the global frame
- try{
- //transform the origin for the sensor
- Stamped<btVector3> local_origin(btVector3(0, 0, 0), base_cloud.header.stamp, base_cloud.header.frame_id);
- tf_.transformPoint(global_frame_, local_origin, global_origin);
- observations_[0].origin_.x = global_origin.getX();
- observations_[0].origin_.y = global_origin.getY();
- observations_[0].origin_.z = global_origin.getZ();
-
- //transform the point cloud
- tf_.transformPointCloud(global_frame_, base_cloud, observations_[0].cloud_);
- }
- catch(tf::TransformException& ex){
- ROS_ERROR("TF Exception that should never happen %s", ex.what());
- return;
- }
+ base_scan_buffer_->bufferCloud(base_cloud);
lock_.unlock();
}
@@ -187,7 +163,9 @@
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
- new_costmap_->updateWorld(wx, wy, observations_, observations_);
+ std::vector<Observation> observations;
+ base_scan_buffer_->getObservations(observations);
+ new_costmap_->updateWorld(wx, wy, observations, observations);
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
@@ -286,13 +264,12 @@
tf::TransformListener tf_; ///< @brief Used for transforming point clouds
laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
- std::vector<Observation> observations_;
Costmap2D* new_costmap_;
- CostMap2D* old_costmap_;
std::string global_frame_;
double freq_;
- boost::thread *visualizer_thread_;
- boost::thread *window_reset_thread_;
+ boost::thread* visualizer_thread_;
+ boost::thread* window_reset_thread_;
+ ObservationBuffer* base_scan_buffer_;
};
Added: pkg/trunk/world_models/new_costmap/src/observation_buffer.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/observation_buffer.cpp (rev 0)
+++ pkg/trunk/world_models/new_costmap/src/observation_buffer.cpp 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,134 @@
+/*********************************************************************
+*
+* 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/observation_buffer.h>
+
+using namespace std;
+using namespace tf;
+using namespace robot_msgs;
+
+namespace costmap_2d {
+ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
+ TransformListener& tf, string global_frame, string sensor_frame) : tf_(tf),
+ observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate), last_updated_(ros::Time::now()),
+ global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name)
+ {
+ }
+
+ ObservationBuffer::~ObservationBuffer(){}
+
+ void ObservationBuffer::bufferCloud(const PointCloud& cloud){
+ Stamped<btVector3> global_origin;
+
+ //create a new observation on the list to be populated
+ observation_list_.push_front(Observation());
+
+ //check whether the origin frame has been set explicitly or whether we should get it from the cloud
+ string origin_frame = sensor_frame_ == "frame_from_message" ? cloud.header.frame_id : sensor_frame_;
+
+ try{
+ //given these observations come from sensors... we'll need to store the origin pt of the sensor
+ Stamped<btVector3> local_origin(btVector3(0, 0, 0), cloud.header.stamp, origin_frame);
+ tf_.transformPoint(global_frame_, local_origin, global_origin);
+ observation_list_.front().origin_.x = global_origin.getX();
+ observation_list_.front().origin_.y = global_origin.getY();
+ observation_list_.front().origin_.z = global_origin.getZ();
+
+ //transform the point cloud
+ tf_.transformPointCloud(global_frame_, cloud, observation_list_.front().cloud_);
+ }
+ catch(TransformException& ex){
+ //if an exception occurs, we need to remove the empty observation from the list
+ observation_list_.pop_front();
+ ROS_ERROR("TF Exception that should never happen %s", ex.what());
+ return;
+ }
+
+ //if the update was successful, we want to update the last updated time
+ last_updated_ = ros::Time::now();
+
+ //we'll also remove any stale observations from the list
+ purgeStaleObservations();
+
+ }
+
+ //returns a copy of the observations
+ void ObservationBuffer::getObservations(vector<Observation>& observations){
+ //first... let's make sure that we don't have any stale observations
+ purgeStaleObservations();
+
+ //now we'll just copy the observations for the caller
+ list<Observation>::iterator obs_it;
+ for(obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
+ observations.push_back(*obs_it);
+ }
+
+ }
+
+ void ObservationBuffer::purgeStaleObservations(){
+ if(!observation_list_.empty()){
+ list<Observation>::iterator obs_it = observation_list_.begin();
+ //if we're keeping observations for no time... then we'll only keep one observation
+ if(observation_keep_time_ == ros::Duration(0.0)){
+ observation_list_.erase(++obs_it, observation_list_.end());
+ return;
+ }
+
+ //otherwise... we'll have to loop through the observations to see which ones are stale
+ for(obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
+ Observation& obs = *obs_it;
+ //check if the observation is out of date... and if it is, remove it and those that follow from the list
+ if((last_updated_ - obs.cloud_.header.stamp) > observation_keep_time_){
+ observation_list_.erase(obs_it, observation_list_.end());
+ return;
+ }
+ }
+ }
+ }
+
+ bool ObservationBuffer::isCurrent() const {
+ if(expected_update_rate_ == ros::Duration(0.0))
+ return true;
+
+ bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
+ if(!current){
+ ROS_WARN("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", topic_name_.c_str(),
+ (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
+ }
+ return current;
+ }
+
+};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
