xml_testdata_loader.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2018 Pilz GmbH & Co. KG
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "pilz_industrial_motion_testutils/xml_testdata_loader.h"

#include <iostream>

#include <boost/property_tree/xml_parser.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/optional.hpp>

#include "pilz_industrial_motion_testutils/default_values.h"
#include "pilz_industrial_motion_testutils/exception_types.h"
#include "pilz_industrial_motion_testutils/xml_constants.h"

namespace pt = boost::property_tree;
namespace pilz_industrial_motion_testutils
{
class CmdReader
{
public:
  CmdReader(const pt::ptree::value_type& node) : cmd_node_(node)
  {
  }

public:
  std::string getPlanningGroup() const;
  std::string getTargetLink() const;
  std::string getStartPoseName() const;
  std::string getEndPoseName() const;

  double getVelocityScale() const;
  double getAccelerationScale() const;

  CmdReader& setDefaultVelocityScale(double scale);
  CmdReader& setDefaultAccelerationScale(double scale);

private:
  const pt::ptree::value_type& cmd_node_;

  double default_velocity_scale_{ DEFAULT_VEL };
  double default_acceleration_scale_{ DEFAULT_ACC };
};

inline std::string CmdReader::getPlanningGroup() const
{
  return cmd_node_.second.get<std::string>(PLANNING_GROUP_STR);
}

inline std::string CmdReader::getTargetLink() const
{
  return cmd_node_.second.get<std::string>(TARGET_LINK_STR);
}

inline std::string CmdReader::getStartPoseName() const
{
  return cmd_node_.second.get<std::string>(START_POS_STR);
}

inline std::string CmdReader::getEndPoseName() const
{
  return cmd_node_.second.get<std::string>(END_POS_STR);
}

inline double CmdReader::getVelocityScale() const
{
  return cmd_node_.second.get<double>(VEL_STR, default_velocity_scale_);
}

inline double CmdReader::getAccelerationScale() const
{
  return cmd_node_.second.get<double>(ACC_STR, default_acceleration_scale_);
}

inline CmdReader& CmdReader::setDefaultVelocityScale(double scale)
{
  default_velocity_scale_ = scale;
  return *this;
}

inline CmdReader& CmdReader::setDefaultAccelerationScale(double scale)
{
  default_acceleration_scale_ = scale;
  return *this;
}

template <class CmdType>
class CmdGetterAdapter : public XmlTestdataLoader::AbstractCmdGetterAdapter
{
public:
  using FuncType = std::function<CmdType(const std::string&)>;

  CmdGetterAdapter(FuncType func) : AbstractCmdGetterAdapter(), func_(func)
  {
  }

public:
  CmdVariant getCmd(const std::string& cmd_name) const override
  {
    return CmdVariant(func_(cmd_name));
  }

private:
  FuncType func_;
};

XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename) : TestdataLoader(), path_filename_(path_filename)
{
  // Parse the XML into the property tree.
  pt::read_xml(path_filename_, tree_, pt::xml_parser::no_comments);

  using std::placeholders::_1;
  cmd_getter_funcs_["ptp"] =
      AbstractCmdGetterUPtr(new CmdGetterAdapter<PtpJoint>(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1)));
  cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr(
      new CmdGetterAdapter<PtpJointCart>(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1)));
  cmd_getter_funcs_["ptp_cart_cart"] =
      AbstractCmdGetterUPtr(new CmdGetterAdapter<PtpCart>(std::bind(&XmlTestdataLoader::getPtpCart, this, _1)));

  cmd_getter_funcs_["lin"] =
      AbstractCmdGetterUPtr(new CmdGetterAdapter<LinJoint>(std::bind(&XmlTestdataLoader::getLinJoint, this, _1)));
  cmd_getter_funcs_["lin_cart"] =
      AbstractCmdGetterUPtr(new CmdGetterAdapter<LinCart>(std::bind(&XmlTestdataLoader::getLinCart, this, _1)));

  cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr(
      new CmdGetterAdapter<CircCenterCart>(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1)));
  cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr(
      new CmdGetterAdapter<CircInterimCart>(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1)));
  cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr(
      new CmdGetterAdapter<CircJointInterimCart>(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1)));

  cmd_getter_funcs_["gripper"] =
      AbstractCmdGetterUPtr(new CmdGetterAdapter<Gripper>(std::bind(&XmlTestdataLoader::getGripper, this, _1)));
}

XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename,
                                     const moveit::core::RobotModelConstPtr& robot_model)
  : XmlTestdataLoader(path_filename)
{
  setRobotModel(robot_model);
}

XmlTestdataLoader::~XmlTestdataLoader()
{
}

const pt::ptree::value_type& XmlTestdataLoader::findNodeWithName(const boost::property_tree::ptree& tree,
                                                                 const std::string& name,
                                                                 const std::string& key,
                                                                 const std::string& path) const
{
  std::string path_str{ (path.empty() ? NAME_PATH_STR : path) };

  // Search for node with given name.
  for (const pt::ptree::value_type& val : tree)
  {
    // Ignore attributes which are always the first element of a tree.
    if (val.first == XML_ATTR_STR)
    {
      continue;
    }

    if (val.first != key)
    {
      continue;
    }

    const auto& node{ val.second.get_child(path_str, empty_tree_) };
    if (node == empty_tree_)
    {
      break;
    }
    if (node.data() == name)
    {
      return val;
    }
  }

  std::string msg;
  msg.append("Node of type \"")
      .append(key)
      .append("\" with ")
      .append(path_str)
      .append("=\"")
      .append(name)
      .append("\" not found.");
  throw TestDataLoaderReadingException(msg);
}

JointConfiguration XmlTestdataLoader::getJoints(const std::string& pos_name, const std::string& group_name) const
{
  // Search for node with given name.
  const auto& poses_tree{ tree_.get_child(POSES_PATH_STR, empty_tree_) };
  if (poses_tree == empty_tree_)
  {
    throw TestDataLoaderReadingException("No poses found.");
  }
  return getJoints(findNodeWithName(poses_tree, pos_name, POSE_STR).second, group_name);
}

JointConfiguration XmlTestdataLoader::getJoints(const boost::property_tree::ptree& joints_tree,
                                                const std::string& group_name) const
{
  // Search joints node with given group_name.
  if (joints_tree == empty_tree_)
  {
    throw TestDataLoaderReadingException("No joints found.");
  }
  const auto& joint_node{ findNodeWithName(joints_tree, group_name, JOINT_STR, GROUP_NAME_PATH_STR) };

  std::vector<std::string> strs;
  boost::split(strs, joint_node.second.data(), boost::is_any_of(" "));
  return JointConfiguration(group_name, strVec2doubleVec(strs), robot_model_);
}

CartesianConfiguration XmlTestdataLoader::getPose(const std::string& pos_name, const std::string& group_name) const
{
  const auto& all_poses_tree{ tree_.get_child(POSES_PATH_STR, empty_tree_) };
  if (all_poses_tree == empty_tree_)
  {
    throw TestDataLoaderReadingException("No poses found.");
  }
  const auto& pose_tree{ findNodeWithName(all_poses_tree, pos_name, POSE_STR).second };
  const auto& xyz_quat_tree{ findNodeWithName(pose_tree, group_name, XYZ_QUAT_STR, GROUP_NAME_PATH_STR).second };
  const boost::property_tree::ptree& link_name_attr{ xyz_quat_tree.get_child(LINK_NAME_PATH_STR, empty_tree_) };
  if (link_name_attr == empty_tree_)
  {
    throw TestDataLoaderReadingException("No link name found.");
  }

  // Get rid of things like "\n", etc.
  std::string data{ xyz_quat_tree.data() };
  boost::trim(data);

  std::vector<std::string> pos_ori_str;
  boost::split(pos_ori_str, data, boost::is_any_of(" "));
  CartesianConfiguration cart_config{ CartesianConfiguration(
      group_name, link_name_attr.data(), strVec2doubleVec(pos_ori_str), robot_model_) };

  const auto& seeds_tree{ xyz_quat_tree.get_child(SEED_STR, empty_tree_) };
  if (seeds_tree != empty_tree_)
  {
    cart_config.setSeed(getJoints(seeds_tree, group_name));
  }
  return cart_config;
}

PtpJoint XmlTestdataLoader::getPtpJoint(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  PtpJoint cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

PtpJointCart XmlTestdataLoader::getPtpJointCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  PtpJointCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

PtpCart XmlTestdataLoader::getPtpCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  PtpCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

LinJoint XmlTestdataLoader::getLinJoint(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  LinJoint cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

LinCart XmlTestdataLoader::getLinCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  LinCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

LinJointCart XmlTestdataLoader::getLinJointCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  LinJointCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

const pt::ptree::value_type& XmlTestdataLoader::findCmd(const std::string& cmd_name,
                                                        const std::string& cmd_path,
                                                        const std::string& cmd_key) const
{
  // Search for node with given name.
  const boost::property_tree::ptree& cmds_tree{ tree_.get_child(cmd_path, empty_tree_) };
  if (cmds_tree == empty_tree_)
  {
    throw TestDataLoaderReadingException("No list of commands of type \"" + cmd_key + "\" found");
  }

  return findNodeWithName(cmds_tree, cmd_name, cmd_key);
}

CartesianCenter XmlTestdataLoader::getCartesianCenter(const std::string& cmd_name,
                                                      const std::string& planning_group) const
{
  const pt::ptree::value_type& cmd_node{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) };
  std::string aux_pos_name;
  try
  {
    aux_pos_name = cmd_node.second.get<std::string>(CENTER_POS_STR);
  }
  catch (...)
  {
    throw TestDataLoaderReadingException("Did not find center of circ");
  }

  CartesianCenter aux;
  aux.setConfiguration(getPose(aux_pos_name, planning_group));
  return aux;
}

CartesianInterim XmlTestdataLoader::getCartesianInterim(const std::string& cmd_name,
                                                        const std::string& planning_group) const
{
  const pt::ptree::value_type& cmd_node{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) };
  std::string aux_pos_name;
  try
  {
    aux_pos_name = cmd_node.second.get<std::string>(INTERMEDIATE_POS_STR);
  }
  catch (...)
  {
    throw TestDataLoaderReadingException("Did not find interim of circ");
  }

  CartesianInterim aux;
  aux.setConfiguration(getPose(aux_pos_name, planning_group));
  return aux;
}

CircCenterCart XmlTestdataLoader::getCircCartCenterCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  CircCenterCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group));
  cmd.setAuxiliaryConfiguration(getCartesianCenter(cmd_name, planning_group));
  cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

CircInterimCart XmlTestdataLoader::getCircCartInterimCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  CircInterimCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group));
  cmd.setAuxiliaryConfiguration(getCartesianInterim(cmd_name, planning_group));
  cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

CircJointInterimCart XmlTestdataLoader::getCircJointInterimCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  CircJointInterimCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setAuxiliaryConfiguration(getCartesianInterim(cmd_name, planning_group));
  cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

CircJointCenterCart XmlTestdataLoader::getCircJointCenterCart(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) };
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  CircJointCenterCart cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setAuxiliaryConfiguration(getCartesianCenter(cmd_name, planning_group));
  cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

Sequence XmlTestdataLoader::getSequence(const std::string& cmd_name) const
{
  Sequence seq;

  // Find sequence with given name and loop over all its cmds
  const auto& sequence_cmd_tree{ findCmd(cmd_name, SEQUENCE_PATH_STR, BLEND_STR).second };
  for (const pt::ptree::value_type& seq_cmd : sequence_cmd_tree)
  {
    // Ignore attributes which are always the first element of a tree.
    if (seq_cmd.first == XML_ATTR_STR)
    {
      continue;
    }

    // Get name of blend cmd.
    const boost::property_tree::ptree& cmd_name_attr = seq_cmd.second.get_child(NAME_PATH_STR, empty_tree_);
    if (cmd_name_attr == empty_tree_)
    {
      throw TestDataLoaderReadingException("Did not find name of sequence cmd");
    }

    const std::string& cmd_name{ cmd_name_attr.data() };

    // Get type of blend cmd
    const boost::property_tree::ptree& type_name_attr{ seq_cmd.second.get_child(CMD_TYPE_PATH_STR, empty_tree_) };
    if (type_name_attr == empty_tree_)
    {
      throw TestDataLoaderReadingException("Did not find type of sequence cmd \"" + cmd_name + "\"");
    }
    const std::string& cmd_type{ type_name_attr.data() };

    // Get blend radius of blend cmd.
    double blend_radius{ seq_cmd.second.get<double>(BLEND_RADIUS_PATH_STR, DEFAULT_BLEND_RADIUS) };

    // Read current command from test data + Add command to sequence
    seq.add(cmd_getter_funcs_.at(cmd_type)->getCmd(cmd_name), blend_radius);
  }

  return seq;
}

Gripper XmlTestdataLoader::getGripper(const std::string& cmd_name) const
{
  CmdReader cmd_reader{ findCmd(cmd_name, GRIPPERS_PATH_STR, GRIPPER_STR) };
  cmd_reader.setDefaultVelocityScale(DEFAULT_VEL_GRIPPER);
  cmd_reader.setDefaultAccelerationScale(DEFAULT_ACC_GRIPPER);
  std::string planning_group{ cmd_reader.getPlanningGroup() };

  Gripper cmd;
  cmd.setPlanningGroup(planning_group);
  cmd.setVelocityScale(cmd_reader.getVelocityScale());
  cmd.setAccelerationScale(cmd_reader.getAccelerationScale());

  cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group));
  cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group));

  return cmd;
}

}  // namespace pilz_industrial_motion_testutils