manipulator_info.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/algorithm/string.hpp>
#include <boost/serialization/nvp.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/manipulator_info.h>
#include <tesseract_common/eigen_serialization.h>
#include <tesseract_common/utils.h>
 
namespace tesseract_common
{
ManipulatorInfo::ManipulatorInfo(std::string manipulator_,
                                 std::string working_frame_,
                                 std::string tcp_frame_,
                                 std::variant<std::string, Eigen::Isometry3d> tcp_offset_)
  : manipulator(std::move(manipulator_))
  , working_frame(std::move(working_frame_))
  , tcp_frame(std::move(tcp_frame_))
  , tcp_offset(std::move(tcp_offset_))
{
}
 
ManipulatorInfo ManipulatorInfo::getCombined(const ManipulatorInfo& manip_info_override) const
{
  ManipulatorInfo combined = *this;
 
  if (!manip_info_override.manipulator.empty())
    combined.manipulator = manip_info_override.manipulator;
 
  if (!manip_info_override.manipulator_ik_solver.empty())
    combined.manipulator_ik_solver = manip_info_override.manipulator_ik_solver;
 
  if (!manip_info_override.working_frame.empty())
    combined.working_frame = manip_info_override.working_frame;
 
  if (!manip_info_override.tcp_frame.empty())
  {
    combined.tcp_frame = manip_info_override.tcp_frame;
    combined.tcp_offset = manip_info_override.tcp_offset;
  }
 
  return combined;
}
 
bool ManipulatorInfo::empty() const
{
  // This struct is empty if either the manipulator or tcp_frame members are empty since they are required
  return manipulator.empty() || working_frame.empty() || tcp_frame.empty();
}
 
bool ManipulatorInfo::operator==(const ManipulatorInfo& rhs) const
{
  bool ret_val = true;
  ret_val &= (manipulator == rhs.manipulator);
  ret_val &= (manipulator_ik_solver == rhs.manipulator_ik_solver);
  ret_val &= (working_frame == rhs.working_frame);
  ret_val &= (tcp_frame == rhs.tcp_frame);
  ret_val &= (tcp_offset.index() == rhs.tcp_offset.index());
  if (ret_val)
  {
    if (tcp_offset.index() == 0)
      ret_val &= (std::get<std::string>(tcp_offset) == std::get<std::string>(rhs.tcp_offset));
    else
      ret_val &= std::get<Eigen::Isometry3d>(tcp_offset).isApprox(std::get<Eigen::Isometry3d>(rhs.tcp_offset));
  }
 
  return ret_val;
}
bool ManipulatorInfo::operator!=(const ManipulatorInfo& rhs) const { return !operator==(rhs); }
 
template <class Archive>
void ManipulatorInfo::serialize(Archive& ar, const unsigned int /*version*/)
{
  ar& boost::serialization::make_nvp("manipulator", manipulator);
  ar& boost::serialization::make_nvp("manipulator_ik_solver", manipulator_ik_solver);
  ar& boost::serialization::make_nvp("working_frame", working_frame);
  ar& boost::serialization::make_nvp("tcp_frame", tcp_frame);
  ar& boost::serialization::make_nvp("tcp_offset", tcp_offset);
}
 
}  // namespace tesseract_common
 
#include <tesseract_common/serialization.h>
TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ManipulatorInfo)
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ManipulatorInfo)
 
TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ManipulatorInfoAnyPoly)
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ManipulatorInfoAnyPoly)