[Personalrobots-commit] SF.net SVN: personalrobots:[17340] pkg/trunk/motion_planning/ planning_models

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

Revision: 17340
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17340&view=rev
Author:   isucan
Date:     2009-06-19 02:02:25 +0000 (Fri, 19 Jun 2009)

Log Message:
-----------
operators for ease of use

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
    pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
    pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h	2009-06-19 01:35:31 UTC (rev 17339)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h	2009-06-19 02:02:25 UTC (rev 17340)
@@ -56,8 +56,13 @@
     public:
 	
 	StateParams(KinematicModel *model);
+	StateParams(const StateParams &sp);
 	virtual ~StateParams(void);
 	
+	StateParams &operator=(const StateParams &rhs);
+	
+	bool operator==(const StateParams &rhs) const;
+	
 	/** \brief Mark all values as unseen */
 	void reset(void);
 	
@@ -166,11 +171,14 @@
 	
     protected:
 	
+	/** \brief Copy data from another instance of this class */
+	void copyFrom(const StateParams &sp);
+	
 	KinematicModel                      *m_owner;
-	msg::Interface                       m_msg;
 	KinematicModel::ModelInfo           &m_mi;
 	double                              *m_params;
 	std::map<unsigned int, bool>         m_seen;
+	msg::Interface                       m_msg;
     };
     
 }

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-06-19 01:35:31 UTC (rev 17339)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp	2009-06-19 02:02:25 UTC (rev 17340)
@@ -40,7 +40,7 @@
 #include <sstream>
 #include <cmath>
 
-planning_models::StateParams::StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo())
+planning_models::StateParams::StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo()), m_params(NULL)
 {
     assert(model->isBuilt());
     m_params = m_mi.stateDimension > 0 ? new double[m_mi.stateDimension] : NULL;
@@ -50,12 +50,47 @@
 	setInRobotFrame();
 }
 
+planning_models::StateParams::StateParams(const StateParams &sp) : m_owner(sp.m_owner), m_mi(sp.m_mi), m_params(NULL)
+{
+    copyFrom(sp);
+}
+
 planning_models::StateParams::~StateParams(void)
 {
     if (m_params)
 	delete[] m_params;
 }
 
+planning_models::StateParams& planning_models::StateParams::operator=(const StateParams &rhs) 
+{
+    if (this != &rhs)
+	copyFrom(rhs);
+    return *this;
+}
+
+bool planning_models::StateParams::operator==(const StateParams &rhs) const
+{
+    if (m_mi.stateDimension != rhs.m_mi.stateDimension)
+	return false;
+    for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+	if (fabs(m_params[i] - rhs.m_params[i]) > DBL_MIN)
+	    return false;
+    return true;
+}
+
+void planning_models::StateParams::copyFrom(const StateParams &sp)
+{
+    m_owner = sp.m_owner;
+    m_mi = sp.m_mi;
+    if (m_params)
+	delete[] m_params;    
+    m_params = m_mi.stateDimension > 0 ? new double[m_mi.stateDimension] : NULL;
+    if (m_params)
+	for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+	    m_params[i] = sp.m_params[i];
+    m_seen = sp.m_seen;
+}
+
 void planning_models::StateParams::reset(void)
 {
     for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)

Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp	2009-06-19 01:35:31 UTC (rev 17339)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp	2009-06-19 02:02:25 UTC (rev 17340)
@@ -436,6 +436,7 @@
     tmpParam[0] = 0.1;
     sp->setParamsJoint(tmpParam, "link_a_joint");
     EXPECT_FALSE(sp->seenAll());
+    EXPECT_TRUE(sp->seenJoint("link_a_joint"));
 
     tmpParam[0] = -1.0;    
     sp->setParamsJoint(tmpParam, "link_c_joint");
@@ -451,6 +452,9 @@
     EXPECT_EQ(0.1, sp->getParams()[3]);
     EXPECT_EQ(-1.0, sp->getParams()[4]);
     
+    planning_models::StateParams sp_copy = *sp;
+    EXPECT_TRUE(sp_copy == *sp);
+    
     delete sp;
     delete model;
 }


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