00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #ifndef DISTANCE_MANAGER_HPP_ 00019 #define DISTANCE_MANAGER_HPP_ 00020 00021 #include <vector> 00022 #include <thread> 00023 #include <mutex> 00024 #include <boost/scoped_ptr.hpp> 00025 #include <cob_obstacle_distance/link_to_collision.hpp> 00026 00027 #include <ros/ros.h> 00028 00029 #include <kdl_parser/kdl_parser.hpp> 00030 #include <kdl/tree.hpp> 00031 #include <kdl/frames.hpp> 00032 00033 #include <Eigen/Dense> 00034 00035 #include <tf/tf.h> 00036 #include <tf/transform_listener.h> 00037 #include <tf_conversions/tf_kdl.h> 00038 #include <tf_conversions/tf_eigen.h> 00039 00040 #include <sensor_msgs/JointState.h> 00041 #include <moveit_msgs/CollisionObject.h> 00042 #include "cob_srvs/SetString.h" 00043 00044 #include "cob_obstacle_distance/marker_shapes/marker_shapes.hpp" 00045 #include "cob_obstacle_distance/shapes_manager.hpp" 00046 #include "cob_obstacle_distance/chainfk_solvers/advanced_chainfksolver_recursive.hpp" 00047 #include "cob_obstacle_distance/obstacle_distance_data_types.hpp" 00048 00049 00050 class DistanceManager 00051 { 00052 private: 00053 std::string root_frame_id_; 00054 std::string chain_base_link_; 00055 std::string chain_tip_link_; 00056 00057 boost::scoped_ptr<ShapesManager> obstacle_mgr_; 00058 boost::scoped_ptr<ShapesManager> object_of_interest_mgr_; 00059 00060 std::vector<std::thread> self_collision_transform_threads_; 00061 std::mutex mtx_; 00062 std::mutex obstacle_mgr_mtx_; 00063 bool stop_sca_threads_; 00064 00065 boost::scoped_ptr<AdvancedChainFkSolverVel_recursive> adv_chn_fk_solver_vel_; 00066 KDL::Chain chain_; 00067 00068 ros::NodeHandle& nh_; 00069 ros::Publisher marker_pub_; 00070 ros::Publisher obstacle_distances_pub_; 00071 tf::TransformListener tf_listener_; 00072 Eigen::Affine3d tf_cb_frame_bl_; 00073 00074 std::vector<std::string> joints_; 00075 std::vector<std::string> segments_; 00076 KDL::JntArray last_q_; 00077 KDL::JntArray last_q_dot_; 00078 00079 LinkToCollision link_to_collision_; 00080 00081 static uint32_t seq_nr_; 00082 00088 void buildObstacleMesh(const moveit_msgs::CollisionObject::ConstPtr& msg, const tf::StampedTransform& transform); 00089 00095 void buildObstaclePrimitive(const moveit_msgs::CollisionObject::ConstPtr& msg, const tf::StampedTransform& transform); 00096 00097 public: 00101 DistanceManager(ros::NodeHandle& nh); 00102 00103 ~DistanceManager(); 00104 00105 inline const std::string getRootFrame() const 00106 { 00107 return this->root_frame_id_; 00108 } 00109 00113 void clear(); 00114 00119 void addObstacle(const std::string& id, PtrIMarkerShape_t s); 00120 00125 void addObjectOfInterest(const std::string& id, PtrIMarkerShape_t s); 00126 00130 void drawObstacles(); 00131 00135 void drawObjectsOfInterest(); 00136 00141 void jointstateCb(const sensor_msgs::JointState::ConstPtr& msg); 00142 00147 void registerObstacle(const moveit_msgs::CollisionObject::ConstPtr& msg); 00148 00153 int init(); 00154 00159 void transform(); 00160 00166 void transformSelfCollisionLinks(const std::string link_name); 00167 00172 void calculate(); 00173 00180 bool registerLinkOfInterest(cob_srvs::SetString::Request& request, 00181 cob_srvs::SetString::Response& response); 00182 00187 Eigen::Affine3d getSynchedCbToBlTransform(); 00188 }; 00189 00190 #endif /* DISTANCE_MANAGER_HPP_ */