Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9286] pkg/trunk
|
From: <tpr...@us...> - 2009-01-13 00:04:06
|
Revision: 9286
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9286&view=rev
Author: tpratkanis
Date: 2009-01-13 00:03:55 +0000 (Tue, 13 Jan 2009)
Log Message:
-----------
Add obstacle filtering to the costmap.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-13 00:03:55 UTC (rev 9286)
@@ -57,10 +57,12 @@
#include <std_msgs/LaserScan.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/PointCloud.h>
// For transform support
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
+#include <tf/message_notifier.h>
// Laser projection
#include "laser_scan/laser_scan.h"
@@ -70,6 +72,10 @@
#include <list>
+namespace robot_filter {
+ class RobotFilter;
+}
+
namespace ros {
namespace highlevel_controllers {
@@ -167,6 +173,7 @@
void baseScanCallback();
void tiltScanCallback();
void tiltCloudCallback();
+ void tiltCloudCallbackTransform(const tf::MessageNotifier<std_msgs::PointCloud>::MessagePtr& message);
void groundPlaneCloudCallback();
void stereoCloudCallback();
void groundPlaneCallback();
@@ -260,7 +267,12 @@
// Tolerances for determining if goal has been reached
double yaw_goal_tolerance_;
double xy_goal_tolerance_;
+
+ //Robot filter
+ robot_filter::RobotFilter* filter_;
+ tf::MessageNotifier<std_msgs::PointCloud>* tiltLaserNotifier_;
+
//ground plane extraction
ransac_ground_plane_extraction::RansacGroundPlaneExtraction ground_plane_extractor_;
pr2_msgs::PlaneStamped groundPlaneMsg_;
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-01-13 00:03:55 UTC (rev 9286)
@@ -27,6 +27,7 @@
<depend package="rostime"/>
<depend package="navfn"/>
<depend package="angles"/>
+ <depend package="robot_filter"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-13 00:03:55 UTC (rev 9286)
@@ -45,6 +45,7 @@
#include <iterator>
#include <angles/angles.h>
#include <boost/thread.hpp>
+#include <robot_filter/RobotFilter.h>
namespace ros {
namespace highlevel_controllers {
@@ -141,23 +142,40 @@
param("/costmap_2d/tilt_laser_keepalive", tilt_laser_keepalive, tilt_laser_keepalive);
param("/costmap_2d/low_obstacle_keepalive", low_obstacle_keepalive, low_obstacle_keepalive);
param("/costmap_2d/stereo_keepalive", stereo_keepalive, stereo_keepalive);
+
+ //Create robot filter
+ std::string robotName = "/robotdesc/pr2";
+ double bodypartScale = 2.4;
+ bool useFilter = false;
+ param("/costmap_2d/body_part_scale", bodypartScale, bodypartScale);
+ param("/costmap_2d/robot_name", robotName, robotName);
+ param("/costmap_2d/filter_robot_points", useFilter, useFilter);
+
+ if (useFilter) {
+ filter_ = new robot_filter::RobotFilter((ros::node*)this, robotName, true, bodypartScale);
+ filter_->loadRobotDescription();
+ filter_->waitForState();
+ } else {
+ filter_ = NULL;
+ }
+
// Then allocate observation buffers
baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_,
ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
- inscribedRadius, minZ_, maxZ_);
+ inscribedRadius, minZ_, maxZ_, filter_);
tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_,
ros::Duration().fromSec(tilt_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
- inscribedRadius, minZ_, maxZ_);
+ inscribedRadius, minZ_, maxZ_, filter_);
lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), tf_,
ros::Duration().fromSec(low_obstacle_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
- inscribedRadius, -10.0, maxZ_);
+ inscribedRadius, -10.0, maxZ_, filter_);
stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), tf_,
ros::Duration().fromSec(stereo_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
- inscribedRadius, minZ_, maxZ_);
+ inscribedRadius, minZ_, maxZ_, filter_);
// get map via RPC
@@ -286,6 +304,9 @@
subscribe("base_scan", baseScanMsg_, &MoveBase::baseScanCallback, 1);
//subscribe("tilt_scan", tiltScanMsg_, &MoveBase::tiltScanCallback, 1);
subscribe("tilt_laser_cloud_filtered", tiltCloudMsg_, &MoveBase::tiltCloudCallback, 1);
+ tiltLaserNotifier_ = new tf::MessageNotifier<std_msgs::PointCloud>(&tf_, this,
+ boost::bind(&MoveBase::tiltCloudCallbackTransform, this, _1),
+ "tilt_laser_cloud_filtered", "map", 1);
subscribe("dcam/cloud", stereoCloudMsg_, &MoveBase::stereoCloudCallback, 1);
subscribe("ground_plane", groundPlaneMsg_, &MoveBase::groundPlaneCallback, 1);
subscribe("obstacle_cloud", groundPlaneCloudMsg_, &MoveBase::groundPlaneCloudCallback, 1);
@@ -318,6 +339,8 @@
delete lowObstacleBuffer_;
delete tiltScanBuffer_;
delete stereoCloudBuffer_;
+ delete filter_;
+ delete tiltLaserNotifier_;
}
void MoveBase::updateGlobalPose(){
@@ -425,8 +448,12 @@
void MoveBase::tiltCloudCallback()
{
+ }
+
+ void MoveBase::tiltCloudCallbackTransform(const tf::MessageNotifier<std_msgs::PointCloud>::MessagePtr& message)
+ {
lock();
- tiltScanBuffer_->buffer_cloud(tiltCloudMsg_);
+ tiltScanBuffer_->buffer_cloud(*message);
unlock();
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-13 00:03:55 UTC (rev 9286)
@@ -34,15 +34,19 @@
#include <costmap_2d/observation_buffer.h>
+namespace robot_filter {
+ class RobotFilter;
+}
+
namespace costmap_2d {
/**
* @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
* access
*/
- class BasicObservationBuffer: public ObservationBuffer {
+ class BasicObservationBuffer : public ObservationBuffer {
public:
- BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ);
+ BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
virtual void buffer_cloud(const std_msgs::PointCloud& local_cloud);
@@ -64,6 +68,7 @@
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 */
+ robot_filter::RobotFilter* filter_;
};
}
#endif
Modified: pkg/trunk/world_models/costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/world_models/costmap_2d/manifest.xml 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/world_models/costmap_2d/manifest.xml 2009-01-13 00:03:55 UTC (rev 9286)
@@ -8,6 +8,7 @@
<depend package="std_msgs" />
<depend package="pr2_msgs" />
<depend package="tf" />
+<depend package="robot_filter" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
</export>
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-13 00:03:55 UTC (rev 9286)
@@ -1,10 +1,11 @@
#include <costmap_2d/basic_observation_buffer.h>
+#include <robot_filter/RobotFilter.h>
namespace costmap_2d {
BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
- double robotRadius, double minZ, double maxZ)
- : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
+ double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter)
+ : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ), filter_(filter) {}
void BasicObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
{
@@ -72,12 +73,34 @@
newData = NULL;
}
+ if (filter_) {
+ newData = filter_->filter(*map_cloud);
+
+ if (map_cloud != NULL){
+ delete map_cloud;
+ map_cloud = NULL;
+ }
+ map_cloud = newData;
+ newData = NULL;
+ }
+
// Get the point from whihc we ray trace
std_msgs::Point o;
o.x = map_origin.getX();
o.y = map_origin.getY();
o.z = map_origin.getZ();
+ if (!map_cloud) {
+ ROS_ERROR("Cloud is null.");
+ continue;
+ }
+
+ if (map_cloud->pts.empty()) {
+ ROS_ERROR("Cloud is empty.");
+ delete map_cloud;
+ continue;
+ }
+
// Allocate and buffer the observation
Observation obs(o, map_cloud);
buffer_observation(obs);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
