|
From: <is...@us...> - 2009-06-19 03:22:48
|
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.
|