[Personalrobots-commit] SF.net SVN: personalrobots:[17428] pkg/trunk/motion_planning/ planning_environment

[<is...@us...>]

Revision: 17428
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17428&view=rev
Author:   isucan
Date:     2009-06-21 20:57:42 +0000 (Sun, 21 Jun 2009)

Log Message:
-----------
using message notifier for collision maps

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
    pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h	2009-06-21 19:27:22 UTC (rev 17427)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h	2009-06-21 20:57:42 UTC (rev 17428)
@@ -40,6 +40,7 @@
 #include "planning_environment/collision_models.h"
 #include "planning_environment/kinematic_model_state_monitor.h"
 
+#include <tf/message_notifier.h>
 #include <robot_msgs/CollisionMap.h>
 #include <robot_msgs/AttachedObject.h>
 
@@ -75,6 +76,10 @@
 	
 	virtual ~CollisionSpaceMonitor(void)
 	{
+	    if (collisionMapNotifier_)
+		delete collisionMapNotifier_;
+	    if (attachedBodyNotifier_)
+		delete attachedBodyNotifier_;
 	}
 
 	collision_space::EnvironmentModel* getEnvironmentModel(void) const
@@ -142,8 +147,8 @@
 	
 	bool                                                           haveMap_;
 	ros::Time                                                      lastMapUpdate_;	
-	ros::Subscriber                                                attachBodySubscriber_;
-	ros::Subscriber                                                collisionMapSubscriber_;
+	tf::MessageNotifier<robot_msgs::CollisionMap>                 *collisionMapNotifier_;
+	tf::MessageNotifier<robot_msgs::AttachedObject>               *attachedBodyNotifier_;
 	
 	boost::function<void(const robot_msgs::CollisionMapConstPtr)>  onBeforeMapUpdate_;
 	boost::function<void(const robot_msgs::CollisionMapConstPtr)>  onAfterMapUpdate_;

Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-21 19:27:22 UTC (rev 17427)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp	2009-06-21 20:57:42 UTC (rev 17428)
@@ -36,6 +36,7 @@
 
 #include "planning_environment/collision_space_monitor.h"
 #include <robot_msgs/PointStamped.h>
+#include <boost/bind.hpp>
 #include <climits>
 
 namespace planning_environment
@@ -52,6 +53,10 @@
     onBeforeMapUpdate_ = NULL;
     onAfterMapUpdate_  = NULL;
     onAfterAttachBody_ = NULL;
+    
+    attachedBodyNotifier_   = NULL;
+    collisionMapNotifier_ = NULL;
+    
     haveMap_ = false;
 
     if (!tf_)
@@ -59,12 +64,13 @@
     
     collisionSpace_ = cm_->getODECollisionModel().get();
     
-    collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
+    collisionMapNotifier_ = new tf::MessageNotifier<robot_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
     ROS_DEBUG("Listening to collision_map");
 
     if (cm_->loadedModels())
     {
-	attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+	attachedBodyNotifier_ = new tf::MessageNotifier<robot_msgs::AttachedObject>(*tf_, boost::bind(&CollisionSpaceMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
+	attachedBodyNotifier_->setTargetFrame(cm_->getCollisionCheckLinks());
 	ROS_DEBUG("Listening to attach_object");
     }
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.