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

[<mc...@us...>]

Revision: 6910
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6910&view=rev
Author:   mcgann
Date:     2008-11-18 21:50:12 +0000 (Tue, 18 Nov 2008)

Log Message:
-----------
Fixed error where not updating a time stamp because of double buffering and deferred update

Modified Paths:
--------------
    pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
    pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
    pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h

Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp	2008-11-18 21:37:24 UTC (rev 6909)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp	2008-11-18 21:50:12 UTC (rev 6910)
@@ -63,8 +63,8 @@
 	std_msgs::Point origin;
 
         // Throw out point clouds that are old.
-        if((last_updated_ -  point_cloud.header.stamp) > max_transform_delay){
-          ROS_INFO("Discarding stale point cloud\n");
+        if((local_cloud.header.stamp -  point_cloud.header.stamp) > max_transform_delay){
+          ROS_DEBUG("Discarding stale point cloud\n");
           point_clouds_.pop_front();
           continue;
         }
@@ -93,17 +93,18 @@
         }
         catch(libTF::TransformReference::LookupException& ex)
         {
-          ROS_ERROR("Lookup exception: %s\n", ex.what());
+          ROS_ERROR("Lookup exception for %s : %s\n", frame_id_.c_str(), ex.what());
           break;
         }
         catch(libTF::TransformReference::ExtrapolateException& ex)
         {
-          ROS_DEBUG("No transform available yet - have to try later: %s . Buffer size is %d\n", ex.what(), point_clouds_.size());
+          ROS_INFO("No transform available yet for %s - have to try later: %s . Buffer size is %d\n", 
+		    frame_id_.c_str(), ex.what(), point_clouds_.size());
           break;
         }
         catch(libTF::TransformReference::ConnectivityException& ex)
         {
-          ROS_ERROR("Connectivity exception: %s\n", ex.what());
+          ROS_ERROR("Connectivity exception for %s: %s\n", frame_id_.c_str(), ex.what());
           break;
         }
         catch(...)
@@ -723,7 +724,7 @@
         start_t = start.tv_sec + double(start.tv_usec) / 1e6;
         end_t = end.tv_sec + double(end.tv_usec) / 1e6;
         t_diff = end_t - start_t;
-        ROS_INFO("Cycle Time: %.3f\n", t_diff);
+        ROS_DEBUG("Cycle Time: %.3f\n", t_diff);
 
         if(!planOk){
           // Zero out the velocities
@@ -736,8 +737,6 @@
         }
       }
 
-      ROS_INFO("Dispatching velocity vector: (%f, %f, %f)\n", cmdVel.vx, cmdVel.vy, cmdVel.vw);
-
       publish("cmd_vel", cmdVel);
       publishFootprint(global_pose_.x, global_pose_.y, global_pose_.yaw);
 
@@ -911,7 +910,7 @@
 	//Avoids laser race conditions.
 	if (isInitialized()) {
 
-	  ROS_INFO("Starting cost map update/n");
+	  ROS_DEBUG("Starting cost map update/n");
 	  lock();
 	  // Aggregate buffered observations across 3 sources
 	  std::vector<costmap_2d::Observation> observations;
@@ -919,7 +918,7 @@
 	  tiltScanBuffer_->get_observations(observations);
 	  stereoCloudBuffer_->get_observations(observations);
 
-	  ROS_INFO("Applying update with %d observations/n", observations.size());
+	  ROS_DEBUG("Applying update with %d observations/n", observations.size());
 	  // Apply to cost map
 	  struct timeval curr;
 	  gettimeofday(&curr,NULL);
@@ -931,7 +930,7 @@
 	  t_diff = last_t - curr_t;
 	  publishLocalCostMap();
 	  unlock();
-	  ROS_INFO("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+	  ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
 	}
 
         d->sleep();

Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml	2008-11-18 21:37:24 UTC (rev 6909)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml	2008-11-18 21:50:12 UTC (rev 6910)
@@ -8,14 +8,12 @@
     <include file="$(find pr2_gazebo)/pr2_floorobj.launch" />
 
 
-    <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
+    <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
     <remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/> 
     <param name="max_publish_frequency" value="20.0"/>
-    <node pkg="fake_localization" type="fake_localization" respawn="false" />
+    <!--node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" /-->
+    <node pkg="fake_localization" type="fake_localization" args="" respawn="false" />
 
-
-    <!--include file="$(find highlevel_controllers)/test/launch_world_3d_map.xml"/-->
-
     <!-- Load parameters for moving the base. -->
     <param name="costmap_2d/inflation_radius" type="double" value="0.35" />
 

Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h	2008-11-18 21:37:24 UTC (rev 6909)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h	2008-11-18 21:50:12 UTC (rev 6910)
@@ -133,7 +133,7 @@
      */
     virtual void get_observations(std::vector<Observation>& observations);
 
-  protected:
+  private:
     std::list<Observation> buffer_;
     const ros::Duration keep_alive_;
     ros::Time last_updated_;


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