attached_body.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Ioan Sucan */
 
#pragma once
 
#include <moveit/robot_model/link_model.h>
#include <moveit/transforms/transforms.h>
#include <geometric_shapes/check_isometry.h>
#include <eigen_stl_containers/eigen_stl_containers.h>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <set>
#include <functional>
 
namespace moveit
{
namespace core
{
class AttachedBody;
typedef std::function<void(AttachedBody* body, bool attached)> AttachedBodyCallback;
 
class AttachedBody
{
public:
  AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose,
               const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
               const std::set<std::string>& touch_links, const trajectory_msgs::msg::JointTrajectory& detach_posture,
               const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap());
 
  ~AttachedBody();
 
  const std::string& getName() const
  {
    return id_;
  }
 
  const Eigen::Isometry3d& getPose() const
  {
    return pose_;
  }
 
  const Eigen::Isometry3d& getGlobalPose() const
  {
    return global_pose_;
  }
 
  const std::string& getAttachedLinkName() const
  {
    return parent_link_model_->getName();
  }
 
  const LinkModel* getAttachedLink() const
  {
    return parent_link_model_;
  }
 
  const std::vector<shapes::ShapeConstPtr>& getShapes() const
  {
    return shapes_;
  }
 
  const EigenSTL::vector_Isometry3d& getShapePoses() const
  {
    return shape_poses_;
  }
 
  const std::set<std::string>& getTouchLinks() const
  {
    return touch_links_;
  }
 
  const trajectory_msgs::msg::JointTrajectory& getDetachPosture() const
  {
    return detach_posture_;
  }
 
  const EigenSTL::vector_Isometry3d& getShapePosesInLinkFrame() const
  {
    return shape_poses_in_link_frame_;
  }
 
  const moveit::core::FixedTransformsMap& getSubframes() const
  {
    return subframe_poses_;
  }
 
  const moveit::core::FixedTransformsMap& getGlobalSubframeTransforms() const
  {
    return global_subframe_poses_;
  }
 
  void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses)
  {
    for (const auto& t : subframe_poses)
    {
      ASSERT_ISOMETRY(t.second)  // unsanitized input, could contain a non-isometry
    }
    subframe_poses_ = subframe_poses;
  }
 
  const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
 
  const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const;
 
  const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
 
  bool hasSubframeTransform(const std::string& frame_name) const;
 
  const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const
  {
    return global_collision_body_transforms_;
  }
 
  void setPadding(double padding);
 
  void setScale(double scale);
 
  void computeTransform(const Eigen::Isometry3d& parent_link_global_transform);
 
private:
  const LinkModel* parent_link_model_;
 
  std::string id_;
 
  Eigen::Isometry3d pose_;
 
  Eigen::Isometry3d global_pose_;
 
  std::vector<shapes::ShapeConstPtr> shapes_;
 
  EigenSTL::vector_Isometry3d shape_poses_;
 
  EigenSTL::vector_Isometry3d shape_poses_in_link_frame_;
 
  EigenSTL::vector_Isometry3d global_collision_body_transforms_;
 
  std::set<std::string> touch_links_;
 
  trajectory_msgs::msg::JointTrajectory detach_posture_;
 
  moveit::core::FixedTransformsMap subframe_poses_;
 
  moveit::core::FixedTransformsMap global_subframe_poses_;
};
}  // namespace core
}  // namespace moveit