Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7986] pkg/trunk
|
From: <mc...@us...> - 2008-12-11 20:17:53
|
Revision: 7986
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7986&view=rev
Author: mcgann
Date: 2008-12-11 20:17:46 +0000 (Thu, 11 Dec 2008)
Log Message:
-----------
Added ObservationBuffer specific watchdogs to make sure the robot does not go forward without all expected sensor updates. Solves trac #739
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-12-11 20:17:46 UTC (rev 7986)
@@ -195,16 +195,10 @@
bool withinDistance(double x1, double y1, double th1, double x2, double y2, double th2) const ;
/**
- * @brief Watchdog Handling
+ * @brief Tests if all the buffers are appropriately up to date
*/
- void petTheWatchDog(const ros::Time& t);
-
bool checkWatchDog() const;
- // Time stamp for laser data. Should go away when ros::Time::now() works in simulation
- struct timeval last_updated_;
- ros::Time last_time_stamp_;
-
// Callback messages
std_msgs::LaserScan baseScanMsg_; /**< Filled by subscriber with new base laser scans */
std_msgs::LaserScan tiltScanMsg_; /**< Filled by subscriber with new tilte laser scans */
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-12-11 20:17:46 UTC (rev 7986)
@@ -88,7 +88,6 @@
param("/costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
// Unsigned chars cannot be stored in parameter server
-
int tmpLethalObstacleThreshold;
param("/costmap_2d/lethal_obstacle_threshold", tmpLethalObstacleThreshold, int(lethalObstacleThreshold));
if (tmpLethalObstacleThreshold > 255)
@@ -116,10 +115,23 @@
robotWidth_ = inscribedRadius * 2;
- // Allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ // Obtain parameters for sensors and allocate observation buffers accordingly. Rates are in Hz.
+ double base_laser_update_rate(2.0);
+ double tilt_laser_update_rate(2.0);
+ double stereo_update_rate(2.0);
+ param("/costmap_2d/base_laser_update_rate", base_laser_update_rate , base_laser_update_rate);
+ param("/costmap_2d/tilt_laser_update_rate", tilt_laser_update_rate , tilt_laser_update_rate);
+ param("/costmap_2d/stereo_update_rate", stereo_update_rate , stereo_update_rate);
+ // Then allocate observation buffers
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0),
+ costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
+ inscribedRadius, minZ_, maxZ_);
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_, ros::Duration(1, 0),
+ costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
+ inscribedRadius, minZ_, maxZ_);
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0),
+ costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
+ inscribedRadius, minZ_, maxZ_);
// get map via RPC
std_srvs::StaticMap::request req;
@@ -348,7 +360,6 @@
std_msgs::PointCloud local_cloud;
local_cloud.header = baseScanMsg_.header;
projector_.projectLaser(baseScanMsg_, local_cloud, baseLaserMaxRange_);
- petTheWatchDog(local_cloud.header.stamp);
lock();
baseScanBuffer_->buffer_cloud(local_cloud);
unlock();
@@ -545,25 +556,14 @@
return false;
}
- void MoveBase::petTheWatchDog(const ros::Time& t){
- last_time_stamp_ = t;
- gettimeofday(&last_updated_, NULL);
- }
-
/**
- * At most one second between updates
+ * The conjunction of all observation buffers must be current
*/
bool MoveBase::checkWatchDog() const {
- struct timeval curr;
- gettimeofday(&curr,NULL);
- double curr_t, last_t, t_diff;
- last_t = last_updated_.tv_sec + last_updated_.tv_usec / 1e6;
- curr_t = curr.tv_sec + curr.tv_usec / 1e6;
- t_diff = curr_t - last_t;
- bool ok = t_diff < 1.0;
+ bool ok = baseScanBuffer_->isCurrent() && tiltScanBuffer_->isCurrent() && stereoCloudBuffer_->isCurrent();
if(!ok)
- ROS_DEBUG("Missed required cost map update. Should not allow commanding now. Check cost map data source.\n");
+ ROS_INFO("Missed required cost map update. Should not allow commanding now. Check cost map data source.\n");
return ok;
}
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2008-12-11 20:17:46 UTC (rev 7986)
@@ -19,6 +19,15 @@
<param name="/costmap_2d/circumscribed_radius" value="0.46"/>
<param name="/costmap_2d/inscribed_radius" value="0.325"/>
<param name="/costmap_2d/weight" value="0.1"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" respawn="false" />
+
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 5 Hz -->
+ <param name="/costmap_2d/base_laser_update_rate" value="5.0"/>
+
+ <!-- Setting these parameters to 0.0 disables the watchdo on them. For stage this is required since we
+ are not getting any data -->
+ <param name="/costmap_2d/tilt_laser_update_rate" value="0.0"/>
+ <param name="/costmap_2d/stereo_update_rate" value="0.0"/>
+
+ <node pkg="highlevel_controllers" type="move_base_sbpl" respawn="true"/>
</group>
</launch>
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 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2008-12-11 20:17:46 UTC (rev 7986)
@@ -42,7 +42,7 @@
*/
class BasicObservationBuffer: public ObservationBuffer {
public:
- BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, 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);
virtual void buffer_cloud(const std_msgs::PointCloud& local_cloud);
@@ -60,8 +60,6 @@
* filter based on the min and max z values
*/
std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
-
- const std::string frame_id_;
tf::TransformListener& tf_;
std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
ros::thread::mutex buffer_mutex_;
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h 2008-12-11 20:17:46 UTC (rev 7986)
@@ -47,7 +47,7 @@
*/
class ObservationBuffer {
public:
- ObservationBuffer(ros::Duration keep_alive):keep_alive_(keep_alive){}
+ ObservationBuffer(const std::string& frame_id, ros::Duration keep_alive, ros::Duration refresh_interval);
virtual ~ObservationBuffer();
@@ -63,9 +63,27 @@
*/
virtual void get_observations(std::vector<Observation>& observations);
+ /**
+ * @brief Checks if the buffered observations are up to date.
+ *
+ * In order to avoid working with stale data, we wish to check if the buffer has been updated
+ * within its refresh interval
+ */
+ bool isCurrent() const;
+
+
+ /**
+ * @brief Translates a rate to an interval in ros duration
+ */
+ static ros::Duration computeRefreshInterval(double rate);
+
+ protected:
+ const std::string frame_id_;
+
private:
std::list<Observation> buffer_;
const ros::Duration keep_alive_;
+ const ros::Duration refresh_interval_;
ros::Time last_updated_;
};
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2008-12-11 20:17:46 UTC (rev 7986)
@@ -2,9 +2,9 @@
namespace costmap_2d {
- BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive,
+ BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
double robotRadius, double minZ, double maxZ)
- : ObservationBuffer(keepAlive), frame_id_(frame_id), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
+ : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
void BasicObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
{
Modified: pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2008-12-11 20:17:46 UTC (rev 7986)
@@ -33,6 +33,20 @@
namespace costmap_2d {
+ ros::Duration ObservationBuffer::computeRefreshInterval(double rate){
+ if(rate <= 0)
+ return ros::Duration(0, 0);
+
+ double period = 1 / rate;
+ return ros::Duration((int) rint(period), (int) rint((period - rint(period)) * pow(10.0, 9.0)));
+ }
+
+ ObservationBuffer::ObservationBuffer(const std::string& frame_id, ros::Duration keep_alive, ros::Duration refresh_interval)
+ :frame_id_(frame_id), keep_alive_(keep_alive), refresh_interval_(refresh_interval){
+ ROS_INFO("Initializing observation buffer for %s with keepAlive = %f and refresh_interval = %f\n",
+ frame_id_.c_str(), keep_alive.toSec(), refresh_interval_.toSec());
+ }
+
// Just clean up outstanding observations
ObservationBuffer::~ObservationBuffer(){
while(!buffer_.empty()){
@@ -46,7 +60,8 @@
// 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;
+ // Basically petting the watchdog here
+ last_updated_ = ros::Time::now();
if(observation.cloud_->header.frame_id != "map")
return false;
@@ -75,4 +90,15 @@
observations.push_back(*it);
}
}
+
+ bool ObservationBuffer::isCurrent() const {
+ static const ros::Duration FOREVER(0, 0);
+ bool ok = refresh_interval_ == FOREVER || (ros::Time::now() - last_updated_ <= refresh_interval_);
+
+ if(!ok){
+ ROS_INFO("Observation Buffer %s is not up to date. It has not been updated for %f seconds.", frame_id_.c_str(), (ros::Time::now() - last_updated_).toSec());
+ }
+
+ return ok;
+ }
}
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-12-11 20:16:45 UTC (rev 7985)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-12-11 20:17:46 UTC (rev 7986)
@@ -132,7 +132,7 @@
// Update the cost map for this observation
map.updateDynamicObstacles(wx, wy, obsBuf);
- // Verify that we know have only 8 * 4 + 4 cells with lethal cost, thus provong that we have correctly cleared
+ // Verify that we now have only 8 * 4 + 4 cells with lethal cost, thus provong that we have correctly cleared
// free space
unsigned int hitCount = 0;
for(unsigned int i=0; i <100; i++){
@@ -165,8 +165,55 @@
* Basic testing for observation buffer
*/
TEST(costmap, test15){
+ // Rate calculations
+ ASSERT_EQ(ObservationBuffer::computeRefreshInterval(-100), ros::Duration(0, 0));
+ ASSERT_EQ(ObservationBuffer::computeRefreshInterval(100), ros::Duration(0, 10000000));
+ ASSERT_EQ(ObservationBuffer::computeRefreshInterval(0), ros::Duration(0, 0));
+ ASSERT_EQ(ObservationBuffer::computeRefreshInterval(5), ros::Duration(0, (int)(0.2 * pow(10.0, 9.0))));
+ ASSERT_EQ(ObservationBuffer::computeRefreshInterval(0.3), ros::Duration(3, (int)((1.0/3.0) * pow(10.0, 9.0))));
+
ros::Duration keep_alive(10, 0);
- ObservationBuffer buffer(keep_alive);
+ ros::Duration refresh_interval(0, 200000000); // 200 ms
+ ObservationBuffer buffer("Foo", keep_alive, refresh_interval);
+
+ // Initially it should be false
+ ASSERT_EQ(buffer.isCurrent(), false);
+
+ std_msgs::Point origin; // Map origin
+ origin.x = 0;
+ origin.y = 0;
+ origin.z = 0;
+
+ ros::Time epoch; // Beginning of time
+
+ // Buffer a point cloud with a time stamp that is very old. It should still not be current
+ std_msgs::PointCloud* p0 = new std_msgs::PointCloud();
+ p0->set_pts_size(1);
+ p0->pts[0].x = 50;
+ p0->pts[0].y = 50;
+ p0->pts[0].z = MAX_Z;
+ p0->header.stamp = epoch;
+ Observation o0(origin, p0);
+ buffer.buffer_observation(o0);
+ // Up to date - ignores the time stamp.
+ ASSERT_EQ(buffer.isCurrent(), true);
+
+ // Now buffer another which has a current time stamp
+ std_msgs::PointCloud* p1 = new std_msgs::PointCloud();
+ p1->set_pts_size(1);
+ p1->pts[0].x = 50;
+ p1->pts[0].y = 50;
+ p1->pts[0].z = MAX_Z;
+ p1->header.stamp = ros::Time::now();
+ Observation o1(origin, p1);
+ buffer.buffer_observation(o1);
+ ASSERT_EQ(buffer.isCurrent(), true);
+
+ // Now go again after sleeping for a bit too long - 300 ms
+ ros::Time oldValue = ros::Time::now();
+ ros::Duration excessiveSleep(0, 300000000);
+ excessiveSleep.sleep();
+ ASSERT_EQ(buffer.isCurrent(), false);
}
/**
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
