29 #include <fcl/collision_object.h> 30 #include <fcl/collision.h> 31 #include <fcl/distance.h> 32 #include <fcl/collision_data.h> 34 #include <std_msgs/Float64.h> 35 #include <visualization_msgs/Marker.h> 37 #include "cob_control_msgs/ObstacleDistance.h" 38 #include "cob_control_msgs/ObstacleDistances.h" 40 #include <boost/filesystem.hpp> 41 #include <boost/filesystem/path.hpp> 42 #include <boost/pointer_cast.hpp> 51 #include <shape_msgs/Mesh.h> 52 #include <shape_msgs/MeshTriangle.h> 53 #include <shape_msgs/SolidPrimitive.h> 82 ROS_ERROR(
"Failed to construct kdl tree from parameter '/robot_description'.");
88 ROS_ERROR(
"Failed to get parameter \"joint_names\".");
92 if (!
nh_.
getParam(
"root_frame", this->root_frame_id_))
94 ROS_ERROR(
"Failed to get parameter \"chain_base_link\".");
98 if (!
nh_.
getParam(
"chain_base_link", this->chain_base_link_))
100 ROS_ERROR(
"Failed to get parameter \"chain_base_link\".");
104 if (!
nh_.
getParam(
"chain_tip_link", this->chain_tip_link_))
106 ROS_ERROR(
"Failed to get parameter \"chain_tip_link\".");
113 ROS_ERROR(
"Failed to initialize kinematic chain");
129 ROS_ERROR(
"Failed to initialize robot model from URDF by parameter \"/robot_description\".");
135 bool success =
false;
143 ROS_WARN(
"Parameter 'self_collision_map' not found or map empty.");
197 cob_control_msgs::ObstacleDistances obstacle_distances;
210 std::string object_of_interest_name = it->first;
211 std::vector<std::string>::const_iterator str_it = std::find(this->
segments_.begin(),
213 object_of_interest_name);
216 ROS_ERROR_STREAM(
"Could not find: " << object_of_interest_name <<
". Skipping it ...");
222 uint16_t idx = str_it - this->
segments_.begin();
223 geometry_msgs::Pose origin_p = ooi->getOriginRelToFrame();
230 KDL::Frame frame_with_offset = frame_pos * origin_f;
232 Eigen::Vector3d chainbase2frame_pos(frame_with_offset.
p.
x(),
233 frame_with_offset.
p.
y(),
234 frame_with_offset.
p.
z());
237 Eigen::Affine3d tmp_inv_tf_cb_frame_bl = tmp_tf_cb_frame_bl.inverse();
238 Eigen::Vector3d abs_jnt_pos = tmp_inv_tf_cb_frame_bl * chainbase2frame_pos;
240 Eigen::Quaterniond q;
242 Eigen::Matrix3d x = (tmp_inv_tf_cb_frame_bl * q).rotation();
243 Eigen::Quaterniond q_1(x);
246 geometry_msgs::Vector3 v3;
247 geometry_msgs::Quaternion quat;
250 ooi->updatePose(v3, quat);
252 fcl::CollisionObject ooi_co = ooi->getCollisionObject();
253 fcl::FCL_REAL last_dist = std::numeric_limits<fcl::FCL_REAL>::max();
258 const std::string obstacle_id = it->first;
267 fcl::CollisionObject collision_obj = obstacle->getCollisionObject();
268 fcl::DistanceResult dist_result;
269 fcl::DistanceRequest dist_request(
true, 5.0, 0.01);
270 fcl::FCL_REAL dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result);
273 Eigen::Vector3d abs_obst_vector(dist_result.nearest_points[1][
VEC_X],
274 dist_result.nearest_points[1][
VEC_Y],
275 dist_result.nearest_points[1][
VEC_Z]);
276 Eigen::Vector3d obst_vector = tmp_tf_cb_frame_bl * abs_obst_vector;
278 Eigen::Vector3d abs_jnt_pos_update(dist_result.nearest_points[0][
VEC_X],
279 dist_result.nearest_points[0][
VEC_Y],
280 dist_result.nearest_points[0][
VEC_Z]);
283 Eigen::Vector3d rel_base_link_frame_pos = tmp_tf_cb_frame_bl * abs_jnt_pos_update;
284 ROS_DEBUG_STREAM(
"Link \"" << object_of_interest_name <<
"\": Minimal distance: " << dist_result.min_distance);
287 cob_control_msgs::ObstacleDistance od_msg;
288 od_msg.distance = dist_result.min_distance;
289 od_msg.link_of_interest = object_of_interest_name;
290 od_msg.obstacle_id = obstacle_id;
297 obstacle_distances.distances.push_back(od_msg);
303 if (obstacle_distances.distances.size() > 0)
320 std::lock_guard<std::mutex> lock(
mtx_);
337 ROS_INFO_STREAM(
"Starting transform_listener thread for link name: " << link_name);
348 geometry_msgs::Transform msg_transform;
353 geometry_msgs::Pose origin_p = shape_ptr->getOriginRelToFrame();
358 shape_ptr->updatePose(msg_transform.translation, msg_transform.rotation);
380 for (uint16_t i = 0; i < msg->name.size(); i++)
382 if (strcmp(msg->name[i].c_str(),
joints_[j].c_str()) == 0)
384 q_temp(j) = msg->position[i];
385 q_dot_temp(j) = msg->velocity[i];
399 ROS_ERROR(
"jointstateCb: received unexpected 'joint_states'");
408 const std::string
frame_id = msg->header.frame_id;
410 Eigen::Affine3d tf_frame_root;
412 if (msg->meshes.size() > 0 && msg->primitives.size() > 0)
414 ROS_ERROR_STREAM(
"registerObstacle: Can either build mesh or primitive but not both in one message for one id.");
418 if (msg->operation == msg->ADD && this->obstacle_mgr_->count(msg->id) > 0)
420 ROS_ERROR_STREAM(
"registerObstacle: Element " << msg->id <<
" exists already. ADD not allowed!");
433 ROS_ERROR(
"TransformException: %s", ex.what());
437 if ((msg->type.db.length() > 0 && 0 < msg->mesh_poses.size()) ||
438 (msg->meshes.size() > 0 && msg->meshes.size() == msg->mesh_poses.size()))
443 if (msg->primitives.size() > 0 && msg->primitives.size() == msg->primitive_poses.size())
454 uint32_t m_size = msg->mesh_poses.size();
455 const std::string package_file_name = msg->type.db;
457 if (msg->mesh_poses.size() > 1)
459 ROS_WARN(
"Currenty only one mesh per message is supported! So only the first one is processed ...");
463 if (package_file_name.length() <= 0 && msg->mesh_poses.size() != msg->meshes.size())
465 ROS_ERROR(
"Mesh poses and meshes do not have the same size. If package resource string is empty then the sizes must be equal!");
469 if (msg->ADD == msg->operation)
471 for (uint32_t i = 0; i < m_size; ++i)
473 geometry_msgs::Pose p = msg->mesh_poses[i];
476 tf::Pose new_tf_p = transform * tf_p;
479 if (package_file_name.length() > 0)
488 shape_msgs::Mesh m = msg->meshes[i];
498 else if (msg->MOVE == msg->operation)
501 for (uint32_t i = 0; i < m_size; ++i)
505 geometry_msgs::Pose p = msg->mesh_poses[i];
508 tf::Pose new_tf_p = transform * tf_p;
514 else if (msg->REMOVE == msg->operation)
527 uint32_t p_size = msg->primitives.size();
528 if (msg->primitives.size() > 1)
530 ROS_WARN(
"Currenty only one primitive per message is supported! So only the first one is processed ...");
534 if (msg->ADD == msg->operation)
536 for (uint32_t i = 0; i < p_size; ++i)
538 shape_msgs::SolidPrimitive sp = msg->primitives[i];
539 geometry_msgs::Pose p = msg->primitive_poses[i];
542 tf::Pose new_tf_p = transform * tf_p;
547 if (shape_msgs::SolidPrimitive::BOX == sp.type)
549 dim(
FCL_BOX_X) = sp.dimensions[shape_msgs::SolidPrimitive::BOX_X];
550 dim(
FCL_BOX_Y) = sp.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
551 dim(
FCL_BOX_Z) = sp.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
553 else if (shape_msgs::SolidPrimitive::SPHERE == sp.type)
555 dim(
FCL_RADIUS) = sp.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS];
557 else if (shape_msgs::SolidPrimitive::CYLINDER == sp.type)
559 dim(
FCL_RADIUS) = sp.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS];
560 dim(
FCL_CYL_LENGTH) = sp.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
577 else if (msg->MOVE == msg->operation)
580 for (uint32_t i = 0; i < p_size; ++i)
584 geometry_msgs::Pose p = msg->primitive_poses[i];
587 tf::Pose new_tf_p = transform * tf_p;
593 else if (msg->REMOVE == msg->operation)
605 cob_srvs::SetString::Response& response)
609 response.success =
true;
610 response.message =
"Element " + request.data +
" already registered.";
617 Eigen::Quaterniond q;
622 response.success =
true;
623 response.message =
"Successfully registered element " + request.data +
".";
627 response.success =
false;
628 response.message =
"Failed to register element " + request.data +
"!";
633 response.success =
false;
634 response.message =
"Failed to register element " + request.data +
"!";
639 ROS_INFO_STREAM(
"DistanceManager::Registration: " << response.message);
646 std::lock_guard<std::mutex> lock(
mtx_);
void addObstacle(const std::string &id, PtrIMarkerShape_t s)
IMETHOD Frame GetFrame() const
std::vector< std::thread > self_collision_transform_threads_
std::mutex obstacle_mgr_mtx_
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
Class to manage fcl::Shapes and connect with RVIZ marker type.
const Segment & getSegment(unsigned int nr) const
std::shared_ptr< IMarkerShape > PtrIMarkerShape_t
KDL::JntArray last_q_dot_
void publish(const boost::shared_ptr< M > &message) const
tf::TransformListener tf_listener_
std::string chain_base_link_
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
ros::Publisher obstacle_distances_pub_
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
std::vector< std::string > joints_
void jointstateCb(const sensor_msgs::JointState::ConstPtr &msg)
unsigned int getNrOfSegments() const
MapSelfCollisions_t::iterator getSelfCollisionsIterBegin()
void buildObstacleMesh(const moveit_msgs::CollisionObject::ConstPtr &msg, const tf::StampedTransform &transform)
std_msgs::ColorRGBA obstacle_color_
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
boost::scoped_ptr< ShapesManager > object_of_interest_mgr_
std::string root_frame_id_
MapSelfCollisions_t::iterator getSelfCollisionsIterEnd()
DistanceManager(ros::NodeHandle &nh)
const std::string & getName() const
boost::scoped_ptr< ShapesManager > obstacle_mgr_
bool registerLinkOfInterest(cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
std::unordered_map< std::string, PtrIMarkerShape_t >::iterator MapIter_t
bool ignoreSelfCollisionPart(const std::string &link_of_interest, const std::string &self_collision_obstacle_link)
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
std::vector< std::string > segments_
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)
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getNrOfJoints() const
boost::scoped_ptr< AdvancedChainFkSolverVel_recursive > adv_chn_fk_solver_vel_
#define ROS_DEBUG_STREAM(args)
LinkToCollision link_to_collision_
static ShapeMsgTypeToVisMarkerType g_shapeMsgTypeToVisMarkerType
void transformSelfCollisionLinks(const std::string link_name)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
KDL_PARSER_PUBLIC bool treeFromParam(const std::string ¶m, KDL::Tree &tree)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
Eigen::Affine3d tf_cb_frame_bl_
bool getParam(const std::string &key, std::string &s) const
ros::Publisher marker_pub_
std::string chain_tip_link_
std::unordered_map< uint8_t, uint32_t > map_
void buildObstaclePrimitive(const moveit_msgs::CollisionObject::ConstPtr &msg, const tf::StampedTransform &transform)
void registerObstacle(const moveit_msgs::CollisionObject::ConstPtr &msg)
#define ROS_ERROR_STREAM(args)
Eigen::Affine3d getSynchedCbToBlTransform()
bool initSelfCollision(XmlRpc::XmlRpcValue &self_collision_params, boost::scoped_ptr< ShapesManager > &sm)
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)
void drawObjectsOfInterest()
static void transformTFToMsg(const Transform &bt, geometry_msgs::Transform &msg)
void addObjectOfInterest(const std::string &id, PtrIMarkerShape_t s)
bool initParameter(const std::string &root_frame_id, const std::string &urdf_param)