link_to_collision.hpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * 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 <stdint.h>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include <ros/ros.h>
#include <urdf/model.h>

#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/pointer_cast.hpp>

#include <fcl/collision_object.h>
#include <fcl/collision.h>
#include <fcl/distance.h>
#include <fcl/collision_data.h>

#include <std_msgs/ColorRGBA.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>


#include "cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp"
#include "cob_obstacle_distance/marker_shapes/marker_shapes.hpp"
#include "cob_obstacle_distance/shapes_manager.hpp"
#include "cob_obstacle_distance/obstacle_distance_data_types.hpp"

class LinkToCollision
{
    private:
        typedef boost::shared_ptr<const urdf::Link> PtrConstLink_t;
        typedef boost::shared_ptr<urdf::Link> PtrLink_t;
        typedef std::vector<boost::shared_ptr<urdf::Link> > VecPtrLink_t;
        typedef boost::shared_ptr<urdf::Collision> PtrCollision_t;
        typedef boost::shared_ptr<urdf::Geometry> PtrGeometry_t;
        typedef boost::shared_ptr<urdf::Mesh> PtrMesh_t;
        typedef boost::shared_ptr<urdf::Box> PtrBox_t;
        typedef boost::shared_ptr<urdf::Sphere> PtrSphere_t;
        typedef boost::shared_ptr<urdf::Cylinder> PtrCylinder_t;
        typedef std::unordered_map<std::string, std::vector<std::string> >::iterator MapIter_t;

        urdf::Model model_;
        bool success_;
        std::string root_frame_id_;
        std::unordered_map<std::string, std::vector<std::string> > self_collision_map_; 

        void createSpecificMarkerShape(const std::string& link_of_interest,
                                       const geometry_msgs::Pose& pose,
                                       const std_msgs::ColorRGBA& col,
                                       const PtrGeometry_t& geometry,
                                       PtrIMarkerShape_t& segment_of_interest_marker_shape);


        void poseURDFToMsg(const urdf::Pose& urdf_pose, geometry_msgs::Pose& msg_pose);

    public:
        typedef std::unordered_map<std::string, std::vector<std::string> > MapSelfCollisions_t;

        LinkToCollision();
        ~LinkToCollision();

        inline MapSelfCollisions_t::iterator getSelfCollisionsIterBegin()
        {
            return this->self_collision_map_.begin();
        }

        inline MapSelfCollisions_t::iterator getSelfCollisionsIterEnd()
        {
            return this->self_collision_map_.end();
        }

        bool ignoreSelfCollisionPart(const std::string& link_of_interest, const std::string& self_collision_obstacle_link);

        bool initFile(const std::string& root_frame_id, const std::string& urdf_file_name);

        bool initParameter(const std::string& root_frame_id, const std::string& urdf_param);

        bool initSelfCollision(XmlRpc::XmlRpcValue& self_collision_params, boost::scoped_ptr<ShapesManager>& sm);


        bool getMarkerShapeFromUrdf(const Eigen::Vector3d& abs_pos,
                                    const Eigen::Quaterniond& quat_pos,
                                    const std::string& link_of_interest,
                                    PtrIMarkerShape_t& segment_of_interest_marker_shape);

        bool getMarkerShapeFromType(const uint32_t& shape_type,
                                    const Eigen::Vector3d& abs_pos,
                                    const Eigen::Quaterniond& quat_pos,
                                    const std::string& link_of_interest,
                                    const Eigen::Vector3d& dimension,
                                    PtrIMarkerShape_t& segment_of_interest_marker_shape);

        bool getMarkerShapeFromType(const uint32_t& shape_type,
                                    const geometry_msgs::Pose& pose,
                                    const std::string& link_of_interest,
                                    const Eigen::Vector3d& dimension,
                                    PtrIMarkerShape_t& segment_of_interest_marker_shape);

};