Anonymous avatar Anonymous committed 9707810

UPdates

Comments (0)

Files changed (11)

gazebo/physics/CollisionState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+bool CollisionState::operator==(const CollisionState &_state) const
+{
+  return this->name == _state.name && this->pose == _state.pose;
+}

gazebo/physics/CollisionState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Equality operator.
+      /// \param[in] _state State to compare against.
+      /// \return True if this state equals the given state.
+      public: bool operator==(const CollisionState &_state) const;
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return Reference to this

gazebo/physics/JointState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+bool JointState::operator==(const JointState &_state) const
+{
+  return this->name == _state.name &&
+    this->angles.size() == _state.angles.size() &&
+    std::equal(this->angles.begin(), this->angles.end(), _state.angles.begin());
+}

gazebo/physics/JointState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Equality operator.
+      /// \param[in] _state State to compare against.
+      /// \return True if this state equals the given state.
+      public: bool operator==(const JointState &_state) const;
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return this

gazebo/physics/LinkState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+bool LinkState::operator==(const LinkState &_state) const
+{
+  bool equal = this->name == _state.name && this->pose == _state.pose &&
+               this->velocity == _state.velocity &&
+               this->acceleration == _state.acceleration &&
+               this->wrench == _state.wrench &&
+               this->collisionStates.size() == _state.collisionStates.size();
+
+  // Check all the collision states.
+  for (std::vector<CollisionState>::const_iterator iter =
+       this->collisionStates.begin(); iter != this->collisionStates.end() &&
+       equal; ++iter)
+  {
+    equal = (*iter) == _state.GetCollisionState((*iter).GetName());
+  }
+
+  return equal;
+}

gazebo/physics/LinkState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Equality operator.
+      /// \param[in] _state State to compare against.
+      /// \return True if this state equals the given state.
+      public: bool operator==(const LinkState &_state) const;
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return this

gazebo/physics/ModelState.cc

 }
 
 /////////////////////////////////////////////////
+bool ModelState::operator==(const ModelState &_state) const
+{
+  bool equal = this->name == _state.name && this->pose == _state.pose &&
+               this->linkStates.size() == _state.linkStates.size() &&
+               this->jointStates.size() == _state.jointStates.size();
+
+  // Check for equality between the links
+  for (std::vector<LinkState>::const_iterator iter = this->linkStates.begin();
+       iter != this->linkStates.end() && equal; ++iter)
+  {
+    equal = (*iter) == _state.GetLinkState((*iter).GetName());
+  }
+
+  // Check for equality between the joints
+  for (std::vector<JointState>::const_iterator iter = this->jointStates.begin();
+       iter != this->jointStates.end() && equal; ++iter)
+  {
+    equal = (*iter) == _state.GetJointState((*iter).GetName());
+  }
+
+  return equal;
+}
+
+/////////////////////////////////////////////////
 ModelState &ModelState::operator=(const ModelState &_state)
 {
   State::operator=(_state);

gazebo/physics/ModelState.hh

       /// \return A vector of joint states.
       public: const std::vector<JointState> &GetJointStates() const;
 
+      /// \brief Equality operator.
+      /// \param[in] _state State to compare against.
+      /// \return True if this state equals the given state.
+      public: bool operator==(const ModelState &_state) const;
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return this

gazebo/physics/WorldState.cc

     if (this->HasModelState((*iter).GetName()))
     {
       ModelState state = this->GetModelState((*iter).GetName()) - *iter;
+      std::cout << "STATE[" << state << "]\n";
       if (!state.IsZero())
         result.modelStates.push_back(state);
     }
 
   return result;
 }
+
+/////////////////////////////////////////////////
+bool WorldState::operator==(const WorldState &_state) const
+{
+  bool equal = this->name == _state.name &&
+               this->simTime == _state.simTime &&
+               this->realTime == _state.realTime &&
+               this->wallTime == _state.wallTime &&
+               this->modelStates.size() == _state.modelStates.size();
+
+  for (std::vector<ModelState>::const_iterator iter = this->modelStates.begin();
+       iter != this->modelStates.end() && equal; ++iter)
+  {
+    equal = (*iter) == _state.GetModelState((*iter).GetName());
+  }
+
+  return equal;
+}

gazebo/physics/WorldState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Equality operator.
+      /// \param[in] _state State to compare against.
+      /// \return True if this state equals the given state.
+      public: bool operator==(const WorldState &_state) const;
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return Reference to this

test/unit/state.cc

 
 TEST_F(StateTest, WorldStateEmptyWorld)
 {
+  std::vector<physics::ModelState> modelStates;
+
   Load("worlds/empty.world");
   physics::WorldState state(physics::get_world("default"));
-  EPPECT_TRUE(state.GetSimTime(), commont::Time());
+  EXPECT_EQ(state.GetSimTime(), common::Time());
 
-  std::vector<physics::ModelState> modelStates = state.GetModelStates();
-  EXPECT_EQ(modelStates.size(), 1);
+  // Test WorldState::GetModelStates and WorldState::GetModelStateCount
+  {
+    modelStates = state.GetModelStates();
+    EXPECT_EQ(static_cast<int>(modelStates.size()), 1);
+    EXPECT_EQ(static_cast<int>(state.GetModelStateCount()), 1);
+  }
+
+  // Test WorldState::GetModelState
+  {
+    EXPECT_EQ(state.GetModelState(0), modelStates[0]);
+    EXPECT_EQ(state.GetModelState("ground_plane"), modelStates[0]);
+
+    EXPECT_THROW(state.GetModelState("bad_model_name"), common::Exception);
+    EXPECT_EQ(state.GetModelState(2), physics::ModelState());
+  }
+
+  // Test WorldState::HasModelState
+  {
+    EXPECT_TRUE(state.HasModelState("ground_plane"));
+    EXPECT_FALSE(state.HasModelState("bad_name"));
+  }
+
+  // Test WorldState::IsZero
+  {
+    EXPECT_TRUE(state.IsZero());
+  }
+
+  // Test WorldState::operator= adn WorldState::operator-
+  {
+    physics::WorldState newState = state;
+    EXPECT_EQ(newState, state);
+
+    newState = newState - state;
+    EXPECT_EQ(newState.GetSimTime(), common::Time());
+    EXPECT_EQ(newState.GetRealTime(), common::Time());
+    EXPECT_EQ(newState.GetWallTime(), common::Time());
+
+    EXPECT_EQ(static_cast<int>(state.GetModelStateCount()), 0);
+  }
+
 }
 
 TEST_F(StateTest, State)
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.