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

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

Revision: 4011
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4011&view=rev
Author:   isucan
Date:     2008-09-05 23:34:27 +0000 (Fri, 05 Sep 2008)

Log Message:
-----------
maintaing two models of the robot: one in MAP frame the other in ROBOT frame

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-09-05 23:28:08 UTC (rev 4010)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h	2008-09-05 23:34:27 UTC (rev 4011)
@@ -92,7 +92,9 @@
 	{
 	    m_urdf = NULL;
 	    m_kmodel = NULL;
+	    m_kmodelSimple = NULL;
 	    m_robotState = NULL;
+	    m_robotStateSimple = NULL;
 	    m_node = node;
 	    m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
 	    m_robotModelName = robot_model;
@@ -110,8 +112,12 @@
 		delete m_urdf;
 	    if (m_robotState)
 		delete m_robotState;
+	    if (m_robotStateSimple)
+		delete m_robotStateSimple;
 	    if (m_kmodel)
 		delete m_kmodel;
+	    if (m_kmodelSimple)
+		delete m_kmodelSimple;
 	}
 	
 	void setRobotDescriptionFromData(const char *data)
@@ -138,14 +144,22 @@
 		delete m_urdf;
 	    if (m_kmodel)
 		delete m_kmodel;
+	    if (m_kmodelSimple)
+		delete m_kmodelSimple;
 	    
 	    m_urdf = file;
 	    m_kmodel = new planning_models::KinematicModel();
 	    m_kmodel->setVerbose(false);
 	    m_kmodel->build(*file);
+
+	    m_kmodelSimple = new planning_models::KinematicModel();
+	    m_kmodelSimple->setVerbose(false);
+	    m_kmodelSimple->build(*file);
 	    
 	    m_robotState = m_kmodel->newStateParams();
 	    m_robotState->setAll(0.0);
+	    m_robotStateSimple = m_kmodelSimple->newStateParams();
+	    m_robotStateSimple->setAll(0.0);
 	}
 	
 	virtual void loadRobotDescription(void)
@@ -164,6 +178,8 @@
 	{
 	    if (m_kmodel)
 		m_kmodel->defaultState();
+	    if (m_kmodelSimple)
+		m_kmodelSimple->defaultState();
 	}
 	
 	bool loadedRobot(void) const
@@ -232,6 +248,15 @@
 		    double pos = m_mechanismState.joint_states[i].position;
 		    m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
 		}
+	    }	 
+	    if (m_robotStateSimple)
+	    {
+		unsigned int n = m_mechanismState.get_joint_states_size();
+		for (unsigned int i = 0 ; i < n ; ++i)
+		{
+		    double pos = m_mechanismState.joint_states[i].position;
+		    m_robotStateSimple->setParams(&pos, m_mechanismState.joint_states[i].name);
+		}
 	    }
 	    m_haveMechanismState = true;
 	    stateUpdate();
@@ -264,13 +289,18 @@
 	bool                                          m_haveMechanismState;
 	
 	robot_desc::URDF                             *m_urdf;
-	planning_models::KinematicModel              *m_kmodel;
+	planning_models::KinematicModel              *m_kmodel;         // MAP frame
+	planning_models::KinematicModel              *m_kmodelSimple;   // ROBOT frame
 	
 	std::string                                   m_robotModelName;
 	double                                        m_basePos[3];
 	
+	/** The complete robot state (MAP frame) */
 	planning_models::KinematicModel::StateParams *m_robotState;
 	bool                                          m_haveState;
+
+	/** The robot state without the base position. The robot is at the origin (ROBOT frame) */
+	planning_models::KinematicModel::StateParams *m_robotStateSimple;
 	
     };
      

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-09-05 23:28:08 UTC (rev 4010)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp	2008-09-05 23:34:27 UTC (rev 4011)
@@ -182,6 +182,8 @@
 	planning_node_util::NodeRobotModel::stateUpdate();
 	if (m_kmodel)
 	    m_kmodel->computeTransforms(m_robotState->getParams());
+	if (m_kmodelSimple)
+	    m_kmodelSimple->computeTransforms(m_robotStateSimple->getParams());
     }
     
     void pointCloudCallback(void)
@@ -414,8 +416,9 @@
 	return cloudF;
     }
     
-    /* Remove invalid floating point values and strip channel iformation.
-     * Also keep a certain ratio of the cloud information only */
+    /** Remove invalid floating point values and strip channel
+     *  iformation.  Also keep a certain ratio of the cloud information
+     *  only. Works with pointclouds in FRAMEID_ROBOT or FRAMEID_MAP */
     std_msgs::PointCloudFloat32* filter0(const std_msgs::PointCloudFloat32 &cloud, double frac = 1.0)
     {
 	std_msgs::PointCloudFloat32 *copy = new std_msgs::PointCloudFloat32();
@@ -435,6 +438,9 @@
 	return copy;	
     }    
     
+    /** Remove points from the cloud if the robot sees parts of
+	itself. Works for pointclouds in FRAMEID_ROBOT \todo make the
+	comment true, separate function in 2*/
     std_msgs::PointCloudFloat32* filter1(const std_msgs::PointCloudFloat32 &cloud)
     {
 	std_msgs::PointCloudFloat32 *copy = new std_msgs::PointCloudFloat32();


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