scene_state.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/access.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/unordered_map.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/eigen_serialization.h>
#include <tesseract_common/utils.h>
#include <tesseract_scene_graph/scene_state.h>
 
namespace tesseract_scene_graph
{
Eigen::VectorXd SceneState::getJointValues(const std::vector<std::string>& joint_names) const
{
  Eigen::VectorXd jv;
  jv.resize(static_cast<long int>(joint_names.size()));
  for (auto j = 0U; j < joint_names.size(); ++j)
    jv(j) = joints.at(joint_names[j]);
 
  return jv;
}
 
tesseract_common::TransformMap SceneState::getFloatingJointValues(const std::vector<std::string>& joint_names) const
{
  tesseract_common::TransformMap fjv;
  for (const auto& joint_name : joint_names)
    fjv[joint_name] = floating_joints.at(joint_name);
 
  return fjv;
}
 
bool SceneState::operator==(const SceneState& rhs) const
{
  auto isometry_equal = [](const Eigen::Isometry3d& iso_1, const Eigen::Isometry3d& iso_2) {
    return iso_1.isApprox(iso_2, 1e-5);
  };
 
  using namespace tesseract_common;
  bool equal = true;
  equal &= isIdenticalMap<std::unordered_map<std::string, double>, double>(joints, rhs.joints);
  equal &= isIdenticalMap<TransformMap, Eigen::Isometry3d>(floating_joints, rhs.floating_joints, isometry_equal);
  equal &= isIdenticalMap<TransformMap, Eigen::Isometry3d>(link_transforms, rhs.link_transforms, isometry_equal);
  equal &= isIdenticalMap<TransformMap, Eigen::Isometry3d>(joint_transforms, rhs.joint_transforms, isometry_equal);
 
  return equal;
}
bool SceneState::operator!=(const SceneState& rhs) const { return !operator==(rhs); }
 
template <class Archive>
void SceneState::serialize(Archive& ar, const unsigned int /*version*/)
{
  ar& BOOST_SERIALIZATION_NVP(joints);
  ar& BOOST_SERIALIZATION_NVP(floating_joints);
  ar& BOOST_SERIALIZATION_NVP(link_transforms);
  ar& BOOST_SERIALIZATION_NVP(joint_transforms);
}
 
}  // namespace tesseract_scene_graph
 
#include <tesseract_common/serialization.h>
TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_scene_graph::SceneState)
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_scene_graph::SceneState)