collision_detector_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/collision_detector.h"
00038 #include <cmath>
00039 
00040 namespace jsk_pcl_ros
00041 {
00042   void CollisionDetector::onInit()
00043   {
00044     DiagnosticNodelet::onInit();
00045     initSelfMask();
00046     pnh_->param<std::string>("world_frame_id", world_frame_id_, "map");
00047     sub_ = pnh_->subscribe("input", 1, &CollisionDetector::pointcloudCallback, this);
00048     service_ = pnh_->advertiseService("check_collision", &CollisionDetector::serviceCallback, this);
00049     onInitPostProcess();
00050  }
00051 
00052   void CollisionDetector::subscribe()
00053   {
00054   }
00055 
00056   void CollisionDetector::unsubscribe()
00057   {
00058   }
00059 
00060   void CollisionDetector::initSelfMask()
00061   {
00062     // genearte urdf model
00063     std::string content;
00064     urdf::Model urdf_model;
00065     if (nh_->getParam("robot_description", content)) {
00066       if (!urdf_model.initString(content)) {
00067         NODELET_ERROR("Unable to parse URDF description!");
00068       }
00069     }
00070 
00071     std::string root_link_id;
00072     std::string world_frame_id;
00073     pnh_->param<std::string>("root_link_id", root_link_id, "BODY");
00074     pnh_->param<std::string>("world_frame_id", world_frame_id, "map");
00075 
00076     double default_padding, default_scale;
00077     pnh_->param("self_see_default_padding", default_padding, 0.01);
00078     pnh_->param("self_see_default_scale", default_scale, 1.0);
00079     std::vector<robot_self_filter::LinkInfo> links;
00080     std::string link_names;
00081 
00082     if (!pnh_->hasParam("self_see_links")) {
00083       NODELET_WARN("No links specified for self filtering.");
00084     } else {
00085       XmlRpc::XmlRpcValue ssl_vals;;
00086       pnh_->getParam("self_see_links", ssl_vals);
00087       if (ssl_vals.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00088         NODELET_WARN("Self see links need to be an array");
00089       } else {
00090         if (ssl_vals.size() == 0) {
00091           NODELET_WARN("No values in self see links array");
00092         } else {
00093           for (int i = 0; i < ssl_vals.size(); i++) {
00094             robot_self_filter::LinkInfo li;
00095             if (ssl_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00096               NODELET_WARN("Self see links entry %d is not a structure.  Stopping processing of self see links",i);
00097               break;
00098             }
00099             if (!ssl_vals[i].hasMember("name")) {
00100               NODELET_WARN("Self see links entry %d has no name.  Stopping processing of self see links",i);
00101               break;
00102             }
00103             li.name = std::string(ssl_vals[i]["name"]);
00104             if (!ssl_vals[i].hasMember("padding")) {
00105               NODELET_DEBUG("Self see links entry %d has no padding.  Assuming default padding of %g",i,default_padding);
00106               li.padding = default_padding;
00107             } else {
00108               li.padding = ssl_vals[i]["padding"];
00109             }
00110             if (!ssl_vals[i].hasMember("scale")) {
00111               NODELET_DEBUG("Self see links entry %d has no scale.  Assuming default scale of %g",i,default_scale);
00112               li.scale = default_scale;
00113             } else {
00114               li.scale = ssl_vals[i]["scale"];
00115             }
00116             links.push_back(li);
00117           }
00118         }
00119       }
00120     }
00121     self_mask_ = boost::shared_ptr<robot_self_filter::SelfMaskUrdfRobot>(new robot_self_filter::SelfMaskUrdfRobot(tf_listener_, tf_broadcaster_, links, urdf_model, root_link_id, world_frame_id));
00122   }
00123 
00124   void CollisionDetector::pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00125   {
00126     boost::mutex::scoped_lock lock(mutex_);
00127     NODELET_DEBUG("update pointcloud.");
00128 
00129     pcl::fromROSMsg(*msg, cloud_);
00130     cloud_frame_id_ = msg->header.frame_id;
00131     cloud_stamp_ = msg->header.stamp;
00132   }
00133 
00134   bool CollisionDetector::serviceCallback(jsk_recognition_msgs::CheckCollision::Request &req,
00135                                           jsk_recognition_msgs::CheckCollision::Response &res)
00136   {
00137     sensor_msgs::JointState joint = req.joint;
00138     geometry_msgs::PoseStamped pose = req.pose;
00139     res.result = checkCollision(joint, pose);
00140     return true;
00141   }
00142 
00143   bool CollisionDetector::checkCollision(const sensor_msgs::JointState& joint,
00144                                          const geometry_msgs::PoseStamped& pose)
00145   {
00146     boost::mutex::scoped_lock lock(mutex_);
00147     NODELET_DEBUG("checkCollision is called.");
00148 
00149     // calculate the sensor transformation
00150     tf::StampedTransform sensor_to_world_tf;
00151     try {
00152       tf_listener_.lookupTransform(world_frame_id_, cloud_frame_id_, cloud_stamp_, sensor_to_world_tf);
00153     } catch (tf::TransformException& ex) {
00154       NODELET_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
00155       return false;
00156     }
00157 
00158     // transform point cloud
00159     Eigen::Matrix4f sensor_to_world;
00160     pcl_ros::transformAsMatrix(sensor_to_world_tf, sensor_to_world);
00161     pcl::transformPointCloud(cloud_, cloud_, sensor_to_world);
00162 
00163     self_mask_->assumeFrameFromJointAngle(joint, pose);
00164 
00165     // check containment for all point cloud
00166     bool contain_flag = false;
00167     pcl::PointXYZ p;
00168     for (size_t i = 0; i < cloud_.size(); i++) {
00169       p = cloud_.at(i);
00170       if (finite(p.x) && finite(p.y) && finite(p.z) &&
00171          self_mask_->getMaskContainment(p.x, p.y, p.z) == robot_self_filter::INSIDE) {
00172         contain_flag = true;
00173         break;
00174       }
00175     }
00176 
00177     if (contain_flag) {
00178       NODELET_INFO("collision!");
00179     } else {
00180       NODELET_INFO("no collision!");
00181     }
00182     return contain_flag;
00183   }
00184 }
00185 
00186 #include <pluginlib/class_list_macros.h>
00187 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::CollisionDetector, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:42