[Personalrobots-commit] SF.net SVN: personalrobots:[10135] pkg/trunk/mapping/collision_map/src/ collision_map_buffer.cpp

[<ve...@us...>]

Revision: 10135
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10135&view=rev
Author:   veedee
Date:     2009-01-24 07:43:02 +0000 (Sat, 24 Jan 2009)

Log Message:
-----------
boo

Modified Paths:
--------------
    pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp

Modified: pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp	2009-01-24 07:00:47 UTC (rev 10134)
+++ pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp	2009-01-24 07:43:02 UTC (rev 10135)
@@ -116,9 +116,9 @@
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
     CollisionMapperBuffer () : ros::Node ("collision_map_buffer"), tf_(*this)
     {
-      param ("~leaf_width_x", leaf_width_.x, 0.015);       // 2.5cm diameter by default
-      param ("~leaf_width_y", leaf_width_.y, 0.015);       // 2.5cm diameter by default
-      param ("~leaf_width_z", leaf_width_.z, 0.015);       // 2.5cm diameter by default
+      param ("~leaf_width_x", leaf_width_.x, 0.02);       // 2cm diameter by default
+      param ("~leaf_width_y", leaf_width_.y, 0.02);       // 2cm diameter by default
+      param ("~leaf_width_z", leaf_width_.z, 0.02);       // 2cm diameter by default
 
       param ("~robot_max_x", robot_max_.x, 1.5);           // 1.5m radius by default
       param ("~robot_max_y", robot_max_.y, 1.5);           // 1.5m radius by default
@@ -136,7 +136,7 @@
       robot_max_.y = robot_max_.y * robot_max_.y;
       robot_max_.z = robot_max_.z * robot_max_.z;
 
-      string cloud_topic ("cloud_1_raw");
+      string cloud_topic ("full_cloud");
 
       vector<pair<string, string> > t_list;
       getPublishedTopics (&t_list);
@@ -234,7 +234,7 @@
     {
       PointStamped base_origin, torso_lift_origin;
       base_origin.point.x = base_origin.point.y = base_origin.point.z = 0.0;
-      base_origin.header.frame_id = "tilt_link_frame";
+      base_origin.header.frame_id = "torso_lift_link";
       base_origin.header.stamp = ros::Time();
 
       try
@@ -394,8 +394,12 @@
         else
           final_leaves_ = cur_leaves_;
 
-        computeCollisionMapFromLeaves (&final_leaves_, final_collision_map_);
+        // Include the static map in the reunion
+        set_union (final_leaves_.begin (), final_leaves_.end (), static_leaves_.begin (), static_leaves_.end (),
+                    inserter (model_reunion, model_reunion.begin ()), compareLeaf);
 
+        computeCollisionMapFromLeaves (&model_reunion, final_collision_map_);
+
         gettimeofday (&t2, NULL);
         time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
         ROS_INFO ("Collision map with %u boxes computed in %g seconds. Total maps in the queue %d.",
@@ -424,7 +428,7 @@
         tictoc.sleep ();
       }
 
-      resp.status = 0;      // success (!!
+      resp.status = 0;      // success (!)
 
       return (true);
     }


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