Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7089] pkg/trunk
|
From: <mc...@us...> - 2008-11-20 23:33:43
|
Revision: 7089
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7089&view=rev
Author: mcgann
Date: 2008-11-20 23:33:37 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
Moved Observation Buffer stuff to the costmap package
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
pkg/trunk/world_models/costmap_2d/CMakeLists.txt
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2008-11-20 23:33:37 UTC (rev 7089)
@@ -10,13 +10,6 @@
genmsg()
-
-#uncomment for profiling
-#set(ROS_COMPILE_FLAGS "-g -pg" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g -pg" ${ROS_LINK_FLAGS})
-#set(ROS_COMPILE_FLAGS "-g" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g" ${ROS_LINK_FLAGS})
-
# Library
rospack_add_library(highlevel_controllers src/MoveBase.cpp src/VelocityControllers.cc)
target_link_libraries(highlevel_controllers trajectory_rollout costmap_2d sbpl_util)
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-20 23:33:37 UTC (rev 7089)
@@ -40,6 +40,7 @@
// Costmap used for the map representation
#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/basic_observation_buffer.h>
// Message structures used
#include <std_msgs/Planner2DState.h>
@@ -63,38 +64,6 @@
namespace ros {
namespace highlevel_controllers {
- /**
- * @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
- * access
- */
- class ObservationBuffer: public costmap_2d::ObservationBuffer {
- public:
- ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive, double robotRadius, double minZ, double maxZ);
-
- void buffer_cloud(const std_msgs::PointCloud& local_cloud);
-
- virtual void get_observations(std::vector<Observation>& observations);
-
- private:
-
- /**
- * @brief Test if point in the square footprint of the robot
- */
- bool inFootprint(double x, double y) const;
-
- /**
- * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
- * filter based on the min and max z values
- */
- std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
-
- const std::string frame_id_;
- rosTFClient& tf_;
- std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
- ros::thread::mutex buffer_mutex_;
- const double robotRadius_, minZ_, maxZ_; /**< Constraints for filtering points */
- };
-
class MoveBase : public HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal> {
public:
@@ -228,10 +197,14 @@
rosTFClient tf_; /**< Used to do transforms */
- // Observation Buffers
- ObservationBuffer* baseScanBuffer_;
- ObservationBuffer* tiltScanBuffer_;
- ObservationBuffer* stereoCloudBuffer_;
+ // Observation Buffers are dynamically allocated since their constructors take
+ // arguments bound by lookup to the param server. This could be chnaged with some reworking of how paramaters
+ // are looked up. If we wanted to generalize this node further, we could use a factory pattern to dynamically
+ // load specific derived classes. For that we would need a hand-shaking pattern to register subscribers for them
+ // with this node
+ costmap_2d::BasicObservationBuffer* baseScanBuffer_;
+ costmap_2d::BasicObservationBuffer* tiltScanBuffer_;
+ costmap_2d::BasicObservationBuffer* stereoCloudBuffer_;
/** Should encapsulate as a controller wrapper that is not resident in the trajectory rollout package */
VelocityController* controller_;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-20 23:33:37 UTC (rev 7089)
@@ -34,159 +34,17 @@
#include <MoveBase.hh>
+#include <costmap_2d/basic_observation_buffer.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/PointCloud.h>
#include <std_msgs/Pose2DFloat32.h>
#include <std_msgs/Polyline2D.h>
#include <std_srvs/StaticMap.h>
#include <std_msgs/PointStamped.h>
-#include <deque>
-#include <set>
namespace ros {
namespace highlevel_controllers {
- ObservationBuffer::ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive,
- double robotRadius, double minZ, double maxZ)
- : costmap_2d::ObservationBuffer(keepAlive), frame_id_(frame_id), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
-
- void ObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
- {
- static const ros::Duration max_transform_delay(10, 0); // max time we will wait for a transform before chucking out the data
- point_clouds_.push_back(local_cloud);
-
- std_msgs::PointCloud * newData = NULL;
- std_msgs::PointCloud * map_cloud = NULL;
-
- while(!point_clouds_.empty()){
- const std_msgs::PointCloud& point_cloud = point_clouds_.front();
- std_msgs::Point origin;
-
- // Throw out point clouds that are old.
- if((local_cloud.header.stamp - point_cloud.header.stamp) > max_transform_delay){
- ROS_DEBUG("Discarding stale point cloud\n");
- point_clouds_.pop_front();
- continue;
- }
-
- libTF::TFPoint map_origin;
- std_msgs::PointCloud base_cloud;
-
- /* Transform to the base frame */
- try
- {
- // First we want the origin for the sensor
- libTF::TFPoint local_origin;
- local_origin.x = 0;
- local_origin.y = 0;
- local_origin.z = 0;
- local_origin.time = point_cloud.header.stamp.toNSec();
- local_origin.frame = frame_id_;
- map_origin = tf_.transformPoint("map", local_origin);
-
- tf_.transformPointCloud("base", base_cloud, point_cloud);
- newData = extractFootprintAndGround(base_cloud);
- map_cloud = new std_msgs::PointCloud();
- tf_.transformPointCloud("map", *map_cloud, *newData);
-
- ROS_DEBUG("Buffering cloud for %s at origin <%f, %f, %f>\n", frame_id_.c_str(), map_origin.x, map_origin.y, map_origin.z);
- }
- catch(libTF::TransformReference::LookupException& ex)
- {
- ROS_ERROR("Lookup exception for %s : %s\n", frame_id_.c_str(), ex.what());
- break;
- }
- catch(libTF::TransformReference::ExtrapolateException& ex)
- {
- ROS_DEBUG("No transform available yet for %s - have to try later: %s . Buffer size is %d\n",
- frame_id_.c_str(), ex.what(), point_clouds_.size());
- break;
- }
- catch(libTF::TransformReference::ConnectivityException& ex)
- {
- ROS_ERROR("Connectivity exception for %s: %s\n", frame_id_.c_str(), ex.what());
- break;
- }
- catch(...)
- {
- ROS_ERROR("Exception in point cloud computation\n");
- break;
- }
-
- point_clouds_.pop_front();
-
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- // Get the point from whihc we ray trace
- std_msgs::Point o;
- o.x = map_origin.x;
- o.y = map_origin.y;
- o.z = map_origin.z;
-
- // Allocate and buffer the observation
- Observation obs(o, map_cloud);
- buffer_observation(obs);
- map_cloud = NULL;
- }
-
- // In case we get thrown out on the second transform - clean up
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- if(map_cloud != NULL){
- delete map_cloud;
- map_cloud = NULL;
- }
- }
-
- void ObservationBuffer::get_observations(std::vector<costmap_2d::Observation>& observations){
- costmap_2d::ObservationBuffer::get_observations(observations);
- }
-
- /**
- * The point is in the footprint if its x and y values are in the range [0 robotWidth] in
- * the base frame.
- */
- bool ObservationBuffer::inFootprint(double x, double y) const {
- bool result = fabs(x) <= robotRadius_ && fabs(y) <= robotRadius_;
-
- if(result){
- ROS_DEBUG("Discarding point <%f, %f> in footprint\n", x, y);
- }
-
- return result;
- }
-
- std_msgs::PointCloud * ObservationBuffer::extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const {
- std_msgs::PointCloud *copy = new std_msgs::PointCloud();
- copy->header = baseFrameCloud.header;
-
- unsigned int n = baseFrameCloud.get_pts_size();
- unsigned int j = 0;
- copy->set_pts_size(n);
-
- for (unsigned int k = 0 ; k < n ; ++k){
- bool ok = baseFrameCloud.pts[k].z > minZ_ && baseFrameCloud.pts[k].z < maxZ_;
- if (ok && !inFootprint(baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y)){
- copy->pts[j++] = baseFrameCloud.pts[k];
- }
- else {
- ROS_DEBUG("Discarding <%f, %f, %f>\n", baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y, baseFrameCloud.pts[k].z);
- }
- }
-
- copy->set_pts_size(j);
-
- ROS_DEBUG("Filter discarded %d points (%d left) \n", n - j, j);
-
- return copy;
- }
-
MoveBase::MoveBase()
: HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal>("move_base", "state", "goal"),
tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
@@ -265,9 +123,9 @@
robotWidth_ = inscribedRadius * 2;
// Allocate observation buffers
- baseScanBuffer_ = new ObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
- tiltScanBuffer_ = new ObservationBuffer(std::string("tilt_laser"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
- stereoCloudBuffer_ = new ObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("tilt_laser"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
// get map via RPC
std_srvs::StaticMap::request req;
@@ -474,13 +332,11 @@
void MoveBase::baseScanCallback()
{
- ROS_DEBUG("Base Scan Callback");
// Project laser into point cloud
std_msgs::PointCloud local_cloud;
local_cloud.header = baseScanMsg_.header;
projector_.projectLaser(baseScanMsg_, local_cloud, baseLaserMaxRange_);
petTheWatchDog(local_cloud.header.stamp);
- ROS_DEBUG("Projected");
lock();
baseScanBuffer_->buffer_cloud(local_cloud);
unlock();
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-20 23:33:37 UTC (rev 7089)
@@ -12,7 +12,7 @@
<remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
<param name="max_publish_frequency" value="20.0"/>
<!--node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" /-->
- <node pkg="fake_localization" type="fake_localization" args="" respawn="false" />
+ <!--node pkg="fake_localization" type="fake_localization" args="" respawn="false" /-->
<!-- Load parameters for moving the base. -->
<param name="costmap_2d/inflation_radius" type="double" value="0.35" />
Modified: pkg/trunk/world_models/costmap_2d/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/costmap_2d/CMakeLists.txt 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/CMakeLists.txt 2008-11-20 23:33:37 UTC (rev 7089)
@@ -4,17 +4,15 @@
rospack(costmap_2d)
genmsg()
-#uncomment for profiling
-#set(ROS_COMPILE_FLAGS "-g -pg" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g -pg" ${ROS_LINK_FLAGS})
-#set(ROS_COMPILE_FLAGS "-g" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g" ${ROS_LINK_FLAGS})
+rospack_add_library(costmap_2d src/costmap_2d.cpp
+ src/obstacle_map_accessor.cc
+ src/observation_buffer.cc
+ src/basic_observation_buffer.cc)
-rospack_add_library(costmap_2d src/costmap_2d.cpp src/obstacle_map_accessor.cc)
-
# Test target for module tests to be included in gtest regression test harness
rospack_add_gtest(utest src/test/module-tests.cc)
target_link_libraries(utest costmap_2d)
# Target for benchmarking the costmap
-rospack_add_executable(benchmark src/test/benchmark.cc src/costmap_2d.cpp src/obstacle_map_accessor.cc)
+rospack_add_executable(benchmark src/test/benchmark.cc )
+target_link_libraries(benchmark costmap_2d)
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-20 23:33:37 UTC (rev 7089)
@@ -56,14 +56,12 @@
// Base class
#include "obstacle_map_accessor.h"
-#include <std_msgs/Point.h>
-#include <std_msgs/PointCloud.h>
+// Need observations
+#include "observation.h"
+
//c++
-#include <list>
#include <vector>
-#include <map>
-#include <set>
#include <string>
#include <queue>
@@ -96,50 +94,7 @@
typedef std::priority_queue< QueueElement*, std::vector<QueueElement*>, QueueElementComparator > QUEUE;
- /**
- * @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(std_msgs::Point& p, std_msgs::PointCloud* cloud): origin_(p), cloud_(cloud) {}
- Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
- std_msgs::Point origin_;
- std_msgs::PointCloud* cloud_;
- };
-
- /**
- * @brief Base class for buffering observations with a time to live property
- * The inputs are in the map frame
- */
- class ObservationBuffer {
- public:
- ObservationBuffer(ros::Duration keep_alive):keep_alive_(keep_alive){}
-
- virtual ~ObservationBuffer();
-
- /**
- * @brief Buffer a current observation
- * @return true if succeded, false if not (which might occur if there was no transform available for example)
- */
- bool buffer_observation(const Observation& observation);
-
- /**
- * @brief Queries for current observations. Any observations that are no longer
- * current will be deleted. Will append observations to the input vector
- */
- virtual void get_observations(std::vector<Observation>& observations);
-
- private:
- std::list<Observation> buffer_;
- const ros::Duration keep_alive_;
- ros::Time last_updated_;
- };
-
-
class CostMap2D: public ObstacleMapAccessor
{
public:
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-20 23:33:37 UTC (rev 7089)
@@ -68,49 +68,6 @@
return (unsigned int) a;
}
- // Just clean up outstanding observations
- ObservationBuffer::~ObservationBuffer(){
- while(!buffer_.empty()){
- std::list<Observation>::iterator it = buffer_.begin();
- const std_msgs::PointCloud* cloud = it->cloud_;
- delete cloud;
- buffer_.erase(it);
- }
- }
-
- // Only works if the observation is in the map frame - test for it. It should be transformed before
- // we enque it
- bool ObservationBuffer::buffer_observation(const Observation& observation){
- last_updated_ = observation.cloud_->header.stamp;
-
- if(observation.cloud_->header.frame_id != "map")
- return false;
-
- // If the duration is 0, then we just keep the latest one, so we clear out all existing observations
- while(!buffer_.empty()){
- std::list<Observation>::iterator it = buffer_.begin();
- // Get the current one, and check if still alive. if so
- Observation& obs = *it;
- if((last_updated_ - obs.cloud_->header.stamp) > keep_alive_){
- delete obs.cloud_;
- buffer_.erase(it);
- }
- else
- break;
- }
-
- // Otherwise just store it and indicate success
- buffer_.push_back(observation);
- return true;
- }
-
- void ObservationBuffer::get_observations(std::vector<Observation>& observations){
- // Add all remaining observations to the output
- for(std::list<Observation>::const_iterator it = buffer_.begin(); it != buffer_.end(); ++it){
- observations.push_back(*it);
- }
- }
-
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, unsigned char threshold, double maxZ, double zLB, double zUB,
double inflationRadius, double circumscribedRadius, double inscribedRadius, double weight,
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-20 23:33:37 UTC (rev 7089)
@@ -33,6 +33,7 @@
*/
#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/observation_buffer.h>
#include <set>
#include <gtest/gtest.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
