[Personalrobots-commit] SF.net SVN: personalrobots:[7721] pkg/trunk

[<tpr...@us...>]

Revision: 7721
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7721&view=rev
Author:   tpratkanis
Date:     2008-12-06 03:28:44 +0000 (Sat, 06 Dec 2008)

Log Message:
-----------
Make world_3d_map create point clouds: a change needed for fixing arm demos.

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
    pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp

Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h	2008-12-06 02:51:27 UTC (rev 7720)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h	2008-12-06 03:28:44 UTC (rev 7721)
@@ -88,7 +88,7 @@
 
     public:
 	
-        NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1 * 1000000000ULL, 0ULL)
+        NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1 * 1000000000ULL, 1000000000ULL)
 	{
 	    m_urdf = NULL;
 	    m_kmodel = NULL;

Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp	2008-12-06 02:51:27 UTC (rev 7720)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp	2008-12-06 03:28:44 UTC (rev 7721)
@@ -147,7 +147,7 @@
     /* Set up the transform client */
     double laser_x_offset;
     param("laser_x_offset", laser_x_offset, 0.275);
-    m_tf.setWithEulers("base_laser", "base", laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
+    m_tf.setWithEulers("base_laser", "base_link", laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
 
     /* create a thread that handles the publishing of the data */	
     m_publishingThread = ros::thread::member_thread::startMemberFunctionThread<World3DMap>(this, &World3DMap::publishDataThread);
@@ -193,7 +193,7 @@
   {
     if(!m_robotState)
       {
-	//ROS_WARN("Ignoring state update because I haven't yet received the robot description");
+	ROS_WARN("Ignoring state update because I haven't yet received the robot description");
 	return;
       }
     //m_robotState->print();
@@ -224,17 +224,64 @@
     m_projector.projectLaser(m_baseScanMsg, local_cloud, m_baseLaserMaxRange);
     processData(local_cloud);
   }
-    
+
+
   void publishDataThread(void)
   {
     ros::Duration *d = new ros::Duration();
     d->fromSec(1.0/m_maxPublishFrequency);
+    
+    /* while everything else is running (map building) check if
+       there are any updates to send, but do so at most at the
+       maximally allowed frequency of sending data */
+    while (m_active)
+    {
+      d->sleep();
 	
-    /* Pump out buffered, filtered point clouds and clear the buffer */
+      m_worldDataMutex.lock();
+      if (m_active && m_currentWorld.size() > 0)
+      {
+	std_msgs::PointCloud toPublish;
+	toPublish.header = m_currentWorld.back()->header;
+	
+	unsigned int      npts  = 0;
+	for (unsigned int i = 0 ; i < m_currentWorld.size() ; ++i)
+	  npts += m_currentWorld[i]->get_pts_size();
+	
+	toPublish.set_pts_size(npts);
+	
+	unsigned int j = 0;
+	for (unsigned int i = 0 ; i < m_currentWorld.size() ; ++i)
+	{
+	  unsigned int n = m_currentWorld[i]->get_pts_size();
+	  for (unsigned int k = 0 ; k < n ; ++k)
+	    toPublish.pts[j++] =  m_currentWorld[i]->pts[k];
+	}
+	
+	toPublish.set_pts_size(j);
+	if (ok())
+	{
+	  if (m_verbose)
+	    ROS_INFO("Publishing a point cloud with %u points\n", toPublish.get_pts_size());
+	  publish("world_3d_map", toPublish);
+	}
+      }
+      m_worldDataMutex.unlock();
+    }
+    delete d;
+  }
+
+  /*void publishDataThread(void)
+  {
+    ros::Duration *d = new ros::Duration(1.0/m_maxPublishFrequency);
+	
+    /* Pump out buffered, filtered point clouds and clear the buffer 
     while (m_active)
       {
+	ROS_INFO("Loop\n");
 	for(unsigned int i = 0; i < m_currentWorld.size(); i++){
 	  std_msgs::PointCloud* p = m_currentWorld[i];
+	  ROS_INFO("Publish\n");
 	  publish("world_3d_map", *p);
 	  delete p;
 	}
@@ -245,7 +292,7 @@
       }
 
     delete d;
-  }
+  }*/
     
   void addSelfSeeBodies(void)
   {
@@ -309,6 +356,7 @@
   void processData(const std_msgs::PointCloud& local_cloud)
   {
     if (!m_acceptScans){
+      ROS_INFO("Rejecting scans\n");
       return;
     }
 
@@ -320,11 +368,6 @@
 
       const std_msgs::PointCloud& point_cloud = point_clouds_.front();
 
-      //make sure that we don't fall to far in the past
-      if(ros::Time::now() - point_cloud.header.stamp > ros::Duration(9, 0)){
-	      point_clouds_.pop_front();
-	      continue;
-      }
 
       std_msgs::PointCloud map_cloud;
 	
@@ -340,7 +383,7 @@
 	}
       catch(libTF::TransformReference::ExtrapolateException& ex)
 	{
-	  //ROS_ERROR("Extrapolation exception: %s\n", ex.what());
+	  ROS_ERROR("Extrapolation exception: %s\n", ex.what());
 	  break;
 	}
       catch(libTF::TransformReference::ConnectivityException& ex)
@@ -406,7 +449,7 @@
 	  copy->pts[j++] = cloud.pts[k];
     copy->set_pts_size(j);
 	
-    ROS_DEBUG("Filter 0 discarded %d points (%d left) \n", n - j, j);
+    ROS_INFO("Filter 0 discarded %d points (%d left) \n", n - j, j);
 
     return copy;	
   }    
@@ -443,7 +486,7 @@
 	  }
       }
 
-    ROS_DEBUG("Filter 1 discarded %d points (%d left) \n", n - j, j);
+    ROS_INFO("Filter 1 discarded %d points (%d left) \n", n - j, j);
 	
     copy->set_pts_size(j);
 


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