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

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

Revision: 18718
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18718&view=rev
Author:   isucan
Date:     2009-07-13 23:47:38 +0000 (Mon, 13 Jul 2009)

Log Message:
-----------
listening to objects_in_map topic

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
    pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.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-07-13 23:47:11 UTC (rev 18717)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h	2009-07-13 23:47:38 UTC (rev 18718)
@@ -41,7 +41,7 @@
 #include "planning_environment/monitors/kinematic_model_state_monitor.h"
 
 #include <mapping_msgs/CollisionMap.h>
-
+#include <mapping_msgs/ObjectInMap.h>
 #include <boost/thread/mutex.hpp>
 
 namespace planning_environment
@@ -68,6 +68,8 @@
 	
 	virtual ~CollisionSpaceMonitor(void)
 	{
+	    if (objectInMapNotifier_)
+		delete objectInMapNotifier_;
 	    if (collisionMapNotifier_)
 		delete collisionMapNotifier_;
 	    if (collisionMapUpdateNotifier_)
@@ -131,6 +133,7 @@
 	void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
 	void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
 	void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
+	void objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap);
 	virtual bool attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
 
 	CollisionModels                                                *cm_;
@@ -142,6 +145,7 @@
 	ros::Time                                                       lastMapUpdate_;	
 	tf::MessageNotifier<mapping_msgs::CollisionMap>                *collisionMapNotifier_;
 	tf::MessageNotifier<mapping_msgs::CollisionMap>                *collisionMapUpdateNotifier_;
+	tf::MessageNotifier<mapping_msgs::ObjectInMap>                 *objectInMapNotifier_;
 	
 	boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
 	boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;

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-07-13 23:47:11 UTC (rev 18717)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp	2009-07-13 23:47:38 UTC (rev 18718)
@@ -35,6 +35,8 @@
 /** \author Ioan Sucan */
 
 #include "planning_environment/monitors/collision_space_monitor.h"
+#include "planning_environment/util/construct_object.h"
+#include <robot_msgs/PointStamped.h>
 #include <robot_msgs/PoseStamped.h>
 #include <boost/bind.hpp>
 #include <sstream>
@@ -55,6 +57,8 @@
     onAfterMapUpdate_  = NULL;
 
     collisionMapNotifier_ = NULL;
+    collisionMapUpdateNotifier_ = NULL;
+    objectInMapNotifier_ = NULL;
     
     haveMap_ = false;
 
@@ -89,6 +93,9 @@
 
     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());
+
+    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());
 }
 
 bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
@@ -218,6 +225,51 @@
 	onAfterMapUpdate_(collisionMap);
 }
 
+void planning_environment::CollisionSpaceMonitor::objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap)
+{
+    if (objectInMap->action == mapping_msgs::ObjectInMap::ADD)
+    {
+	// add the object to the map
+	shapes::Shape *shape = construct_object(objectInMap->object);
+	if (shape)
+	{
+	    bool err = false;
+	    robot_msgs::PoseStamped psi;
+	    robot_msgs::PoseStamped pso;
+	    psi.pose = objectInMap->pose;
+	    psi.header = objectInMap->header;
+	    try
+	    {
+		tf_->transformPose(getFrameId(), psi, pso);
+	    }
+	    catch(...)
+	    {
+		err = true;
+	    }
+	    
+	    if (err)
+		ROS_ERROR("Unable to transform object '%s' in frame '%s' to frame '%s'", objectInMap->id.c_str(), objectInMap->header.frame_id.c_str(), getFrameId().c_str());
+	    else
+	    {
+		btTransform pose;
+		tf::poseMsgToTF(pso.pose, pose);
+		collisionSpace_->lock();
+		collisionSpace_->addObject(objectInMap->id, shape, pose);
+		collisionSpace_->unlock();
+		ROS_DEBUG("Added object '%s' to collision space", objectInMap->id.c_str());
+	    }
+	    delete shape;
+	}
+    }
+    else
+    {
+	// remove the object from the map
+	collisionSpace_->lock();
+	collisionSpace_->clearObstacles(objectInMap->id);
+	collisionSpace_->unlock();
+    }
+}
+
 bool planning_environment::CollisionSpaceMonitor::attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject)
 {
     collisionSpace_->lock();


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