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

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

Revision: 3677
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3677&view=rev
Author:   isucan
Date:     2008-08-27 03:55:37 +0000 (Wed, 27 Aug 2008)

Log Message:
-----------
fixed bug in world_3d_map (correct order of initialization), added more things in the collision group, removed extra messages in collision space

Modified Paths:
--------------
    pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
    pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
    pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
    pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
    pkg/trunk/world_models/world_3d_map/run.xml
    pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp

Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml	2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml	2008-08-27 03:55:37 UTC (rev 3677)
@@ -1,13 +1,11 @@
 <launch>
   <group name="wg">
-    <include file="$(find wg_robot_description)/send.xml"/>
-    <node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_rosgazebo.world" respawn="true" />
-    <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
-    <node pkg="pr2_gazebo" type="run-pr2_gazebo.sh" args="" respawn="true" />
-    <node pkg="fake_localization" type="fake_localization" respawn="false" />
-    <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" />
-    <node pkg="nav_view" type="nav_view" respawn="true" />
-    <node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" />
+    <include file="$(find gazebo_robot_description)/pr2_test.xml"/>
+    <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
+    <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
+    <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
+    <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+    <node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" />
   </group>
 </launch>
 

Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp	2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp	2008-08-27 03:55:37 UTC (rev 3677)
@@ -250,16 +250,14 @@
 		    dReal aabb1[6], aabb2[6];		    
 		    dGeomGetAABB(g1, aabb1);
 		    dGeomGetAABB(g2, aabb2);
+		    
 		    if (!(aabb1[2] > aabb2[3] ||
 			  aabb1[3] < aabb2[2] ||
 			  aabb1[4] > aabb2[5] ||
-			  aabb1[5] < aabb2[4]))
+			  aabb1[5] < aabb2[4])) 
 			dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
 		    if (cdata.collides)
-		    {
-			std::cout << m_kgeoms[model_id].geom[vec[j]]->link->name << " intersects with " << m_kgeoms[model_id].geom[vec[k]]->link->name  << std::endl;
 			goto OUT1;
-		    }
 		}
 	}
     }
@@ -280,11 +278,13 @@
 		dGeomID g2 = m_odeGeoms[j];
 		dReal aabb2[6];
 		dGeomGetAABB(g2, aabb2);
+
 		if (!(aabb1[2] > aabb2[3] ||
 		      aabb1[3] < aabb2[2] ||
 		      aabb1[4] > aabb2[5] ||
 		      aabb1[5] < aabb2[4]))
 		    dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+		
 		if (cdata.collides)
 		    goto OUT2;
 	    }
@@ -299,8 +299,6 @@
 	m_collide2.setup();
 	for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
 	    m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
-	if (cdata.collides)
-	    std::cout << "Pointcloud intersection"  << std::endl;
     }
     
     return cdata.collides;

Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-08-27 03:55:37 UTC (rev 3677)
@@ -78,6 +78,7 @@
 	
 	req.params.model_id = "pr2::base";
 	req.params.distance_metric = "L2Square";
+	req.params.planner_id = "RRT";
 	req.threshold = 0.01;
 	req.interpolate = 1;
 	req.times = 10;
@@ -89,7 +90,7 @@
 	    req.goal_state.vals[i] = m_basePos[i];
 	req.goal_state.vals[0] += 3.5;
 	
-	req.allowed_time = 30.0;
+	req.allowed_time = 10.0;
 	
 	req.params.volumeMin.x = -5.0 + m_basePos[0];	req.params.volumeMin.y = -5.0 + m_basePos[1];	req.params.volumeMin.z = 0.0;
 	req.params.volumeMax.x = 5.0 + m_basePos[0];	req.params.volumeMax.y = 5.0 + m_basePos[1];	req.params.volumeMax.z = 0.0;

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-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h	2008-08-27 03:55:37 UTC (rev 3677)
@@ -96,6 +96,9 @@
 	    m_node = node;
 	    m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
 	    m_robotModelName = robot_model;
+	    m_haveState = false;
+	    m_haveMechanismState = false;
+	    m_haveBasePos = false;
 	    
 	    m_node->subscribe("localizedpose",   m_localizedPose,  &NodeRobotModel::localizedPoseCallback,  this, 1);
 	    m_node->subscribe("mechanism_state", m_mechanismState, &NodeRobotModel::mechanismStateCallback, this, 1);
@@ -174,6 +177,12 @@
 	    return m_kmodel != NULL;
 	}
 	
+	void waitForState(void)
+	{
+	    while (m_node->ok() && !(m_haveState ^ loadedRobot()))
+		ros::Duration(0.05).sleep();
+	}
+	
     protected:
 	
 	void localizedPoseCallback(void)
@@ -214,7 +223,7 @@
 		    m_basePos[1] = pose.y;
 		if (isfinite(pose.yaw))
 		    m_basePos[2] = pose.yaw;
-		
+		m_haveBasePos = true;
 		baseUpdate();
 	    }
 	}
@@ -227,6 +236,7 @@
 		double pos = m_mechanismState.joint_states[i].position;
 		m_robotState->setValue(m_mechanismState.joint_states[i].name, &pos);
 	    }
+	    m_haveMechanismState = true;
 	    stateUpdate();
 	}
 	
@@ -243,13 +253,16 @@
 	
 	virtual void stateUpdate(void)
 	{
+	    m_haveState = m_haveBasePos && m_haveMechanismState;
 	}
 	
 	rosTFClient                                   m_tf; 
 	ros::node                                    *m_node;
 	std_msgs::RobotBase2DOdom                     m_localizedPose;
+	bool                                          m_haveBasePos;	
 	
 	mechanism_control::MechanismState             m_mechanismState; // this message should be moved to robot_msgs
+	bool                                          m_haveMechanismState;
 	
 	robot_desc::URDF                             *m_urdf;
 	planning_models::KinematicModel              *m_kmodel;
@@ -258,7 +271,8 @@
 	double                                        m_basePos[3];
 	
 	planning_models::KinematicModel::StateParams *m_robotState;
-
+	bool                                          m_haveState;
+	
     };
      
 }

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml	2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml	2008-08-27 03:55:37 UTC (rev 3677)
@@ -19,6 +19,12 @@
     finger_r_left
     finger_l_right
     finger_r_right
+    shoulder_pan_right
+    shoulder_pitch_right 
+    shoulder_pan_left
+    shoulder_pitch_left
+    torso
+    base
   </group>
   
   <!-- Define the parts of the robot that the collision checker should care about -->

Modified: pkg/trunk/world_models/world_3d_map/run.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/run.xml	2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/world_models/world_3d_map/run.xml	2008-08-27 03:55:37 UTC (rev 3677)
@@ -2,8 +2,8 @@
 
   <param name="world_3d_map/max_publish_frequency" type="double" value="0.3" /> <!-- Hz -->
   <param name="world_3d_map/retain_pointcloud_duration" type="double" value="60.0" /> <!-- seconds -->
-  <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.05" /> <!-- percentage (between 0 and 1) -->
-  <param name="world_3d_map/retain_above_ground_threshold" type="double" value="0.02" /> <!-- double value -->
+  <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.25" /> <!-- percentage (between 0 and 1) -->
+  <param name="world_3d_map/retain_above_ground_threshold" type="double" value="0.03" /> <!-- double value -->
   <param name="world_3d_map/verbosity_level" type="int" value="1" /> <!-- integer value -->
   
   <node pkg="world_3d_map" type="world_3d_map" args="robotdesc/pr2 scan:=tilt_scan" output="screen" respawn="false" />  

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-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp	2008-08-27 03:55:37 UTC (rev 3677)
@@ -125,7 +125,8 @@
 
 	param("world_3d_map/max_publish_frequency", m_maxPublishFrequency, 0.5);
 	param("world_3d_map/retain_pointcloud_duration", m_retainPointcloudDuration, 60.0);
-	param("world_3d_map/retain_pointcloud_fraction", m_retainPointcloudFraction, 0.02);
+	param("world_3d_map/retain_pointcloud_fraction", m_retainPointcloudFraction, 0.25);
+	
 	param("world_3d_map/retain_above_ground_threshold", m_retainAboveGroundThreshold, 0.01);
 	param("world_3d_map/verbosity_level", m_verbose, 1);
 	
@@ -191,7 +192,7 @@
 	   postpone processing latest data just because it is not done
 	   with older data. */
 	if (m_verbose) 
-          fprintf(stdout, "Received laser scan with %d points in frame %s\n", m_inputScan.get_ranges_size(), m_inputScan.header.frame_id.c_str());
+	    fprintf(stdout, "Received laser scan with %d points in frame %s\n", m_inputScan.get_ranges_size(), m_inputScan.header.frame_id.c_str());
 	
 	/* copy data to a place where incoming messages do not affect it */
 	bool success = false;
@@ -388,7 +389,9 @@
 		if (isfinite(cloud.pts[k].x) && isfinite(cloud.pts[k].y) && isfinite(cloud.pts[k].z))
 		    copy->pts[j++] = cloud.pts[k];
 	copy->set_pts_size(j);
-
+	
+	if (m_verbose)
+	    printf("Filter 0 discarded %d points (%d left) \n", n - j, j);
 	return copy;	
     }    
     
@@ -420,7 +423,7 @@
 	    }
 	}
 	if (m_verbose)
-	    printf("Discarded %d points (%d left) \n", n - j, j);
+	    printf("Filter 1 discarded %d points (%d left) \n", n - j, j);
 	
 	copy->set_pts_size(j);
 
@@ -463,6 +466,7 @@
     {
 	World3DMap *map = new World3DMap(argv[1]);
 	map->loadRobotDescription();
+	map->waitForState();
 	map->setAcceptScans(true);
 	map->spin();
 	map->shutdown();


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