Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23810] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-09-04 03:09:05
|
Revision: 23810
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23810&view=rev
Author: isucan
Date: 2009-09-04 03:08:58 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
switching to message filters instead of message notifier
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-09-04 03:08:58 UTC (rev 23810)
@@ -69,12 +69,18 @@
virtual ~CollisionSpaceMonitor(void)
{
- if (objectInMapNotifier_)
- delete objectInMapNotifier_;
- if (collisionMapNotifier_)
- delete collisionMapNotifier_;
- if (collisionMapUpdateNotifier_)
- delete collisionMapUpdateNotifier_;
+ if (objectInMapFilter_)
+ delete objectInMapFilter_;
+ if (objectInMapSubscriber_)
+ delete objectInMapSubscriber_;
+ if (collisionMapFilter_)
+ delete collisionMapFilter_;
+ if (collisionMapSubscriber_)
+ delete collisionMapSubscriber_;
+ if (collisionMapUpdateFilter_)
+ delete collisionMapUpdateFilter_;
+ if (collisionMapUpdateSubscriber_)
+ delete collisionMapUpdateSubscriber_;
}
/** \brief Start the environment monitor. By default, the monitor is started after creation */
@@ -188,10 +194,14 @@
bool haveMap_;
ros::Time lastMapUpdate_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
- tf::MessageNotifier<mapping_msgs::ObjectInMap> *objectInMapNotifier_;
+ message_filters::Subscriber<mapping_msgs::CollisionMap> *collisionMapSubscriber_;
+ tf::MessageFilter<mapping_msgs::CollisionMap> *collisionMapFilter_;
+ message_filters::Subscriber<mapping_msgs::CollisionMap> *collisionMapUpdateSubscriber_;
+ tf::MessageFilter<mapping_msgs::CollisionMap> *collisionMapUpdateFilter_;
+ message_filters::Subscriber<mapping_msgs::ObjectInMap> *objectInMapSubscriber_;
+ tf::MessageFilter<mapping_msgs::ObjectInMap> *objectInMapFilter_;
+
boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onBeforeMapUpdate_;
boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onAfterMapUpdate_;
boost::function<void(const mapping_msgs::ObjectInMapConstPtr)> onObjectInMapUpdate_;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-09-04 03:08:58 UTC (rev 23810)
@@ -41,7 +41,8 @@
#include "planning_models/kinematic_state.h"
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <tf/message_notifier.h>
+#include <tf/message_filter.h>
+#include <message_filters/subscriber.h>
#include <sensor_msgs/JointState.h>
#include <mapping_msgs/AttachedObject.h>
#include <boost/bind.hpp>
@@ -81,8 +82,10 @@
{
if (robotState_)
delete robotState_;
- if (attachedBodyNotifier_)
- delete attachedBodyNotifier_;
+ if (attachedBodyFilter_)
+ delete attachedBodyFilter_;
+ if (attachedBodySubscriber_)
+ delete attachedBodySubscriber_;
}
/** \brief Start the state monitor. By default, the monitor is started after creation */
@@ -208,8 +211,11 @@
ros::Subscriber jointStateSubscriber_;
tf::TransformListener *tf_;
- tf::MessageNotifier<mapping_msgs::AttachedObject>
- *attachedBodyNotifier_;
+ message_filters::Subscriber<mapping_msgs::AttachedObject>
+ *attachedBodySubscriber_;
+
+ tf::MessageFilter<mapping_msgs::AttachedObject>
+ *attachedBodyFilter_;
planning_models::KinematicState *robotState_;
double robotVelocity_;
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-09-04 03:08:58 UTC (rev 23810)
@@ -12,6 +12,7 @@
<depend package="roscpp" />
<depend package="rosconsole" />
<depend package="tf" />
+ <depend package="message_filters" />
<depend package="angles" />
<depend package="sensor_msgs" />
<depend package="motion_planning_msgs" />
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-09-04 03:08:58 UTC (rev 23810)
@@ -58,10 +58,14 @@
onAfterMapUpdate_ = NULL;
onObjectInMapUpdate_ = NULL;
- collisionMapNotifier_ = NULL;
- collisionMapUpdateNotifier_ = NULL;
- objectInMapNotifier_ = NULL;
+ collisionMapFilter_ = NULL;
+ collisionMapUpdateFilter_ = NULL;
+ objectInMapFilter_ = NULL;
+ collisionMapSubscriber_ = NULL;
+ collisionMapUpdateSubscriber_ = NULL;
+ objectInMapSubscriber_ = NULL;
+
haveMap_ = false;
collisionSpace_ = cm_->getODECollisionModel().get();
@@ -74,15 +78,21 @@
{
if (envMonitorStarted_)
return;
-
- collisionMapNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
- ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapNotifier_->getTargetFramesString().c_str());
- collisionMapUpdateNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1), "collision_map_update", getFrameId(), 1);
- ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
+ collisionMapSubscriber_ = new message_filters::Subscriber<mapping_msgs::CollisionMap>(nh_, "collision_map", 1);
+ collisionMapFilter_ = new tf::MessageFilter<mapping_msgs::CollisionMap>(*collisionMapSubscriber_, *tf_, getFrameId(), 1);
+ collisionMapFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1));
+ ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapFilter_->getTargetFramesString().c_str());
- objectInMapNotifier_ = new tf::MessageNotifier<mapping_msgs::ObjectInMap>(*tf_, boost::bind(&CollisionSpaceMonitor::objectInMapCallback, this, _1), "object_in_map", getFrameId(), 1024);
- ROS_DEBUG("Listening to object_in_map using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
+ collisionMapUpdateSubscriber_ = new message_filters::Subscriber<mapping_msgs::CollisionMap>(nh_, "collision_map_update", 1);
+ collisionMapUpdateFilter_ = new tf::MessageFilter<mapping_msgs::CollisionMap>(*collisionMapUpdateSubscriber_, *tf_, getFrameId(), 1);
+ collisionMapUpdateFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1));
+ ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateFilter_->getTargetFramesString().c_str());
+
+ objectInMapSubscriber_ = new message_filters::Subscriber<mapping_msgs::ObjectInMap>(nh_, "object_in_map", 1024);
+ objectInMapFilter_ = new tf::MessageFilter<mapping_msgs::ObjectInMap>(*objectInMapSubscriber_, *tf_, getFrameId(), 1024);
+ objectInMapFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::objectInMapCallback, this, _1));
+ ROS_DEBUG("Listening to object_in_map using message notifier with target frame %s", collisionMapUpdateFilter_->getTargetFramesString().c_str());
envMonitorStarted_ = true;
}
@@ -92,14 +102,20 @@
if (!envMonitorStarted_)
return;
- delete collisionMapUpdateNotifier_;
- collisionMapUpdateNotifier_ = NULL;
+ delete collisionMapUpdateFilter_;
+ collisionMapUpdateFilter_ = NULL;
+ delete collisionMapUpdateSubscriber_;
+ collisionMapUpdateSubscriber_ = NULL;
- delete collisionMapNotifier_;
- collisionMapNotifier_ = NULL;
+ delete collisionMapFilter_;
+ collisionMapFilter_ = NULL;
+ delete collisionMapSubscriber_;
+ collisionMapSubscriber_ = NULL;
- delete objectInMapNotifier_;
- objectInMapNotifier_ = NULL;
+ delete objectInMapFilter_;
+ objectInMapFilter_ = NULL;
+ delete objectInMapSubscriber_;
+ objectInMapSubscriber_ = NULL;
ROS_DEBUG("Environment state is no longer being monitored");
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-09-04 03:08:58 UTC (rev 23810)
@@ -47,7 +47,8 @@
robotState_ = NULL;
onStateUpdate_ = NULL;
onAfterAttachBody_ = NULL;
- attachedBodyNotifier_ = NULL;
+ attachedBodyFilter_ = NULL;
+ attachedBodySubscriber_ = NULL;
havePose_ = haveJointState_ = false;
robotVelocity_ = 0.0;
@@ -94,9 +95,12 @@
jointStateSubscriber_ = nh_.subscribe("joint_states", 1, &KinematicModelStateMonitor::jointStateCallback, this);
ROS_DEBUG("Listening to joint states");
- attachedBodyNotifier_ = new tf::MessageNotifier<mapping_msgs::AttachedObject>(*tf_, boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
- attachedBodyNotifier_->setTargetFrame(rm_->getCollisionCheckLinks());
- ROS_DEBUG("Listening to attach_object using message notifier with target frame %s", attachedBodyNotifier_->getTargetFramesString().c_str());
+ attachedBodySubscriber_ = new message_filters::Subscriber<mapping_msgs::AttachedObject>(nh_, "attach_object", 1);
+ attachedBodyFilter_ = new tf::MessageFilter<mapping_msgs::AttachedObject>(*attachedBodySubscriber_, *tf_, "", 1);
+ attachedBodyFilter_->setTargetFrames(rm_->getCollisionCheckLinks());
+ attachedBodyFilter_->registerCallback(boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1));
+
+ ROS_DEBUG("Listening to attach_object using message notifier with target frame %s", attachedBodyFilter_->getTargetFramesString().c_str());
}
stateMonitorStarted_ = true;
}
@@ -106,8 +110,11 @@
if (!stateMonitorStarted_)
return;
- delete attachedBodyNotifier_;
- attachedBodyNotifier_ = NULL;
+ delete attachedBodyFilter_;
+ attachedBodyFilter_ = NULL;
+
+ delete attachedBodySubscriber_;
+ attachedBodySubscriber_ = NULL;
jointStateSubscriber_.shutdown();
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-09-04 03:08:58 UTC (rev 23810)
@@ -46,7 +46,8 @@
#include "planning_environment/util/construct_object.h"
#include <geometric_shapes/bodies.h>
-#include <tf/message_notifier.h>
+#include <tf/message_filter.h>
+#include <message_filters/subscriber.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/PoseStamped.h>
#include <mapping_msgs/AttachedObject.h>
@@ -71,23 +72,35 @@
cloudPublisher_ = nh_.advertise<sensor_msgs::PointCloud>("cloud_out", 1);
kmsm_->setOnAfterAttachBodyCallback(boost::bind(&ClearKnownObjects::attachObjectEvent, this, _1));
- cloudNotifier_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&ClearKnownObjects::cloudCallback, this, _1), "cloud_in", fixed_frame_, 1);
- objectInMapNotifier_ = new tf::MessageNotifier<mapping_msgs::ObjectInMap>(tf_, boost::bind(&ClearKnownObjects::objectInMapCallback, this, _1), "object_in_map", fixed_frame_, 1024);
+
+ cloudSubscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud>(nh_, "cloud_in", 1);
+ cloudFilter_ = new tf::MessageFilter<sensor_msgs::PointCloud>(*cloudSubscriber_, tf_, fixed_frame_, 1);
+ cloudFilter_->registerCallback(boost::bind(&ClearKnownObjects::cloudCallback, this, _1));
+
+ objectInMapSubscriber_ = new message_filters::Subscriber<mapping_msgs::ObjectInMap>(nh_, "object_in_map", 1024);
+ objectInMapFilter_ = new tf::MessageFilter<mapping_msgs::ObjectInMap>(*objectInMapSubscriber_, tf_, fixed_frame_, 1024);
+ objectInMapFilter_->registerCallback(boost::bind(&ClearKnownObjects::objectInMapCallback, this, _1));
}
else
{
kmsm_ = NULL;
- cloudNotifier_ = NULL;
- objectInMapNotifier_ = NULL;
+ cloudFilter_ = NULL;
+ cloudSubscriber_ = NULL;
+ objectInMapFilter_ = NULL;
+ objectInMapSubscriber_ = NULL;
}
}
~ClearKnownObjects(void)
{
- if (cloudNotifier_)
- delete cloudNotifier_;
- if (objectInMapNotifier_)
- delete objectInMapNotifier_;
+ if (cloudFilter_)
+ delete cloudFilter_;
+ if (cloudSubscriber_)
+ delete cloudSubscriber_;
+ if (objectInMapFilter_)
+ delete objectInMapFilter_;
+ if (objectInMapSubscriber_)
+ delete objectInMapSubscriber_;
if (kmsm_)
delete kmsm_;
if (rm_)
@@ -361,8 +374,12 @@
tf::TransformListener tf_;
planning_environment::RobotModels *rm_;
planning_environment::KinematicModelStateMonitor *kmsm_;
- tf::MessageNotifier<sensor_msgs::PointCloud> *cloudNotifier_;
- tf::MessageNotifier<mapping_msgs::ObjectInMap> *objectInMapNotifier_;
+
+ message_filters::Subscriber<mapping_msgs::ObjectInMap> *objectInMapSubscriber_;
+ tf::MessageFilter<mapping_msgs::ObjectInMap> *objectInMapFilter_;
+ message_filters::Subscriber<sensor_msgs::PointCloud> *cloudSubscriber_;
+ tf::MessageFilter<sensor_msgs::PointCloud> *cloudFilter_;
+
std::string fixed_frame_;
boost::mutex updateObjects_;
ros::Publisher cloudPublisher_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
