svn diff old1

svn diff https://code.ros.org/svn/wg-ros-pkg/stacks/tabletop_object_perception/branches/0.4-branch-old1/tabletop_collision_map_processing/ https://code.ros.org/svn/wg-ros-pkg/stacks/tabletop_object_perception/branches/0.4-branch/tabletop_collision_map_processing/
Index: include/tabletop_collision_map_processing/collision_map_interface.h
===================================================================
--- include/tabletop_collision_map_processing/collision_map_interface.h	(.../0.4-branch-old1/tabletop_collision_map_processing)	(revision 55195)
+++ include/tabletop_collision_map_processing/collision_map_interface.h	(.../0.4-branch/tabletop_collision_map_processing)	(revision 55195)
@@ -93,6 +93,9 @@
   //! Client for getting the mesh for a database object
   ros::ServiceClient get_model_mesh_srv_;

+  //! The topic for the point cloud used for static maps
+  std::string static_map_cloud_name_;
+
   //! Counts the number of objects added to the map since the last reset
   int collision_object_current_id_;

@@ -113,6 +116,12 @@
   //! Connects all the services that it uses to their appropriate service names
   void connectServices();

+  //! Only keep every point_skip_num_ points when adding a point cloud collision object
+  int point_skip_num_;
+
+  //! Only keep every point_skip_num_ points when adding a point cloud collision object
+  int collision_box_size_;
+
  public:
   //! Connects and waits for all needed services and action servers
   CollisionMapInterface();
@@ -132,7 +141,7 @@

   //! Sends collision geometry for a point cloud into the collision map
   void processCollisionGeometryForCluster(const sensor_msgs::PointCloud &cluster,
-					  std::string &collision_name);
+					  std::string &collision_name, float collision_size = -1.0);

   //! Sends collision geometry for a bounding box into the collision map
   void processCollisionGeometryForBoundingBox(const object_manipulation_msgs::ClusterBoundingBox &box,
Index: src/collision_map_interface.cpp
===================================================================
--- src/collision_map_interface.cpp	(.../0.4-branch-old1/tabletop_collision_map_processing)	(revision 55195)
+++ src/collision_map_interface.cpp	(.../0.4-branch/tabletop_collision_map_processing)	(revision 55195)
@@ -69,6 +69,9 @@
   collision_map_reset_client_ = root_nh_.serviceClient<std_srvs::Empty>(RESET_COLLISION_MAP_NAME, true);
   cluster_bounding_box_client_ = root_nh_.serviceClient<object_manipulation_msgs::FindClusterBoundingBox>
     (CLUSTER_BOUNDING_BOX_NAME, true);
+  priv_nh_.param<std::string>("static_map_cloud_name", static_map_cloud_name_, "full_cloud_filtered");
+  priv_nh_.param<int>("point_skip_num", point_skip_num_, 1);
+  priv_nh_.param<int>("collision_box_size", collision_box_size_, 0.003);
 }

 bool CollisionMapInterface::connectionsPresent()
@@ -148,7 +151,7 @@
 void CollisionMapInterface::takeStaticMap()
 {
   collision_environment_msgs::MakeStaticCollisionMapGoal static_map_goal;
-  static_map_goal.cloud_source = "full_cloud_filtered";
+  static_map_goal.cloud_source = static_map_cloud_name_;
   static_map_goal.number_of_clouds = 2;

   make_static_collision_map_client_.sendGoal(static_map_goal);
@@ -245,7 +248,7 @@
 }

 void CollisionMapInterface::processCollisionGeometryForCluster(const sensor_msgs::PointCloud &cluster,
-							       std::string &collision_name)
+							       std::string &collision_name, float collision_size)
 {
   ROS_INFO("Adding cluster with %u points to collision map", (unsigned int)cluster.points.size());

@@ -253,21 +256,25 @@
   many_boxes.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
   many_boxes.header = cluster.header;
   many_boxes.header.stamp = ros::Time::now();
-  unsigned int num_to_use = (unsigned int)(cluster.points.size()/100.0);
+  unsigned int num_to_use = (unsigned int)(cluster.points.size()/point_skip_num_);
   many_boxes.shapes.resize(num_to_use);
   many_boxes.poses.resize(num_to_use);
+
+  if(collision_size < 0)
+    collision_size = collision_box_size_;
+
   for(unsigned int i = 0; i < num_to_use; i++) {
     geometric_shapes_msgs::Shape shape;
     shape.type = geometric_shapes_msgs::Shape::BOX;
     shape.dimensions.resize(3);
-    shape.dimensions[0] = .005;
-    shape.dimensions[1] = .005;
-    shape.dimensions[2] = .005;
+    shape.dimensions[0] = collision_size;
+    shape.dimensions[1] = collision_size;
+    shape.dimensions[2] = collision_size;
     many_boxes.shapes[i]=shape;
     geometry_msgs::Pose pose;
-    pose.position.x = cluster.points[i*100].x;
-    pose.position.y = cluster.points[i*100].y;
-    pose.position.z = cluster.points[i*100].z;
+    pose.position.x = cluster.points[i*point_skip_num_].x;
+    pose.position.y = cluster.points[i*point_skip_num_].y;
+    pose.position.z = cluster.points[i*point_skip_num_].z;
     pose.orientation.x = 0;
     pose.orientation.y = 0;
     pose.orientation.z = 0;
@@ -293,7 +300,7 @@
     {
       ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
     }
-    if (!root_nh_.ok()) exit(0);
+    if (!root_nh_.ok()) return false;
     get_model_mesh_srv_ = root_nh_.serviceClient<household_objects_database_msgs::GetModelMesh>
       (get_model_mesh_srv_name, true);
     service_initialized = true;