jsk_pcl_ros: interactive_cuboid_likelihood

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/interactive_cuboid_likelihood.h"
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039 #include <jsk_topic_tools/rosparam_utils.h>
00040 
00041 namespace jsk_pcl_ros
00042 {
00043   void InteractiveCuboidLikelihood::onInit()
00044   {
00045     DiagnosticNodelet::onInit();
00046     tf_ = TfListenerSingleton::getInstance();
00047     pub_ = pnh_->advertise<std_msgs::Float32>("output", 1);
00048     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00049     pnh_->param("frame_id", frame_id_, std::string("odom"));
00050     pnh_->param("sensor_frame", sensor_frame_, std::string("odom"));
00051 
00052     std::vector<double> initial_pos;
00053     std::vector<double> initial_rot;
00054     if (!jsk_topic_tools::readVectorParameter(*pnh_, "initial_pos", initial_pos)) {
00055       initial_pos.push_back(0);
00056       initial_pos.push_back(0);
00057       initial_pos.push_back(0);
00058     }
00059     if (!jsk_topic_tools::readVectorParameter(*pnh_, "initial_rot", initial_rot)) {
00060       initial_rot.push_back(0);
00061       initial_rot.push_back(0);
00062       initial_rot.push_back(0);
00063     }
00064     particle_.x = initial_pos[0];
00065     particle_.y = initial_pos[1];
00066     particle_.z = initial_pos[2];
00067     particle_.roll = initial_rot[0];
00068     particle_.pitch = initial_rot[1];
00069     particle_.yaw = initial_rot[2];
00070     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00071       boost::bind (&InteractiveCuboidLikelihood::configCallback, this, _1, _2);
00072     srv_->setCallback (f);
00073     sub_ = pnh_->subscribe("input", 1, &InteractiveCuboidLikelihood::likelihood, this);
00074     // Cuboid
00075     server_.reset(new interactive_markers::InteractiveMarkerServer(getName()));
00076     visualization_msgs::InteractiveMarker int_marker = particleToInteractiveMarker(particle_);
00077     server_->insert(int_marker, boost::bind(&InteractiveCuboidLikelihood::processFeedback, this, _1));
00078     server_->applyChanges();
00079 
00080     // SupportPlane
00081     plane_server_.reset(new interactive_markers::InteractiveMarkerServer(getName() + "_plane"));
00082     plane_pose_ = Eigen::Affine3f::Identity();
00083     visualization_msgs::InteractiveMarker plane_int_marker = planeInteractiveMarker();
00084     plane_server_->insert(plane_int_marker, boost::bind(&InteractiveCuboidLikelihood::processPlaneFeedback, this, _1));
00085     plane_server_->applyChanges();
00086     onInitPostProcess();
00087   }
00088 
00089   void InteractiveCuboidLikelihood::subscribe()
00090   {
00091     
00092   }
00093 
00094   void InteractiveCuboidLikelihood::unsubscribe()
00095   {
00096 
00097   }
00098 
00099   void InteractiveCuboidLikelihood::processFeedback(
00100     const visualization_msgs::InteractiveMarkerFeedback::ConstPtr& feedback)
00101   {
00102     boost::mutex::scoped_lock lock(mutex_);
00103     Eigen::Affine3f pose;
00104     tf::poseMsgToEigen(feedback->pose, pose);
00105     particle_.fromEigen(pose);
00106   }
00107   
00108   void InteractiveCuboidLikelihood::processPlaneFeedback(
00109     const visualization_msgs::InteractiveMarkerFeedback::ConstPtr& feedback)
00110   {
00111     boost::mutex::scoped_lock lock(mutex_);
00112     tf::poseMsgToEigen(feedback->pose, plane_pose_);
00113   }
00114 
00115   void InteractiveCuboidLikelihood::likelihood(
00116     const sensor_msgs::PointCloud2::ConstPtr& msg)
00117   {
00118     boost::mutex::scoped_lock lock(mutex_);
00119     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00120     pcl::fromROSMsg(*msg, *cloud);
00121     tf::StampedTransform transform
00122       = lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
00123                                     ros::Time(0.0),
00124                                     ros::Duration(0.0));
00125     Eigen::Vector3f vp;
00126     tf::vectorTFToEigen(transform.getOrigin(), vp);
00127     jsk_recognition_utils::Vertices vertices;
00128     vertices.push_back(Eigen::Vector3f(plane_pose_ * (Eigen::Vector3f::UnitX() + Eigen::Vector3f::UnitY())));
00129     vertices.push_back(Eigen::Vector3f(plane_pose_ * (- Eigen::Vector3f::UnitX() + Eigen::Vector3f::UnitY())));
00130     vertices.push_back(Eigen::Vector3f(plane_pose_ * (- Eigen::Vector3f::UnitX() - Eigen::Vector3f::UnitY())));
00131     vertices.push_back(Eigen::Vector3f(plane_pose_ * (Eigen::Vector3f::UnitX() - Eigen::Vector3f::UnitY())));
00132     Polygon::Ptr plane(new Polygon(vertices));
00133     //particle_.plane = plane;
00134     particle_.plane_index = 0;
00135     std::vector<Polygon::Ptr> polygons;
00136     polygons.push_back(plane);
00137     // for (size_t i = 0; i < vertices.size(); i++) {
00138     //   ROS_INFO("v: [%f, %f, %f]", vertices[i][0], vertices[i][1], vertices[i][2]);
00139     // }
00140     pcl::KdTreeFLANN<pcl::PointXYZ> tree;
00141     tree.setInputCloud(cloud);
00142     std::vector<float> polygon_likelihood(1, 1.0);
00143     double l = computeLikelihood(particle_, cloud, tree, vp, polygons, polygon_likelihood, config_);
00144     NODELET_INFO("likelihood: %f", l);
00145     std_msgs::Float32 float_msg;
00146     float_msg.data = l;
00147     pub_.publish(float_msg);
00148   }
00149 
00150   void InteractiveCuboidLikelihood::configCallback(
00151     Config& config, uint32_t level)
00152   {
00153     boost::mutex::scoped_lock lock(mutex_);
00154     config_ = config;
00155     particle_.dx = config_.dx;
00156     particle_.dy = config_.dy;
00157     particle_.dz = config_.dz;
00158     if (server_) {
00159       visualization_msgs::InteractiveMarker int_marker = particleToInteractiveMarker(particle_);
00160       server_->insert(int_marker, boost::bind(&InteractiveCuboidLikelihood::processFeedback, this, _1));
00161       server_->applyChanges();
00162     }
00163   }
00164 
00165   visualization_msgs::InteractiveMarker
00166   InteractiveCuboidLikelihood::planeInteractiveMarker()
00167   {
00168     visualization_msgs::InteractiveMarker int_marker;
00169     int_marker.header.frame_id = frame_id_;
00170     int_marker.header.stamp = ros::Time::now();
00171     int_marker.name = getName() + "_plane";
00172     visualization_msgs::InteractiveMarkerControl control;
00173     control.orientation.w = 1;
00174     control.orientation.x = 1;
00175     control.orientation.y = 0;
00176     control.orientation.z = 0;
00177     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00178     int_marker.controls.push_back(control);
00179     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00180     int_marker.controls.push_back(control);
00181 
00182     control.orientation.w = 1;
00183     control.orientation.x = 0;
00184     control.orientation.y = 1;
00185     control.orientation.z = 0;
00186     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00187     int_marker.controls.push_back(control);
00188     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00189     int_marker.controls.push_back(control);
00190 
00191     control.orientation.w = 1;
00192     control.orientation.x = 0;
00193     control.orientation.y = 0;
00194     control.orientation.z = 1;
00195     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00196     int_marker.controls.push_back(control);
00197     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00198     int_marker.controls.push_back(control);
00199     visualization_msgs::InteractiveMarkerControl box_control;
00200     box_control.always_visible = true;
00201     visualization_msgs::Marker plane_marker;
00202     plane_marker.type = visualization_msgs::Marker::CUBE;
00203     plane_marker.scale.x = 1.0;
00204     plane_marker.scale.y = 1.0;
00205     plane_marker.scale.z = 0.01;
00206     plane_marker.color.r = 1.0;
00207     plane_marker.color.a = 1.0;
00208     plane_marker.pose.orientation.w = 1.0;
00209     box_control.markers.push_back(plane_marker);
00210     int_marker.controls.push_back(box_control);
00211     return int_marker;
00212   }
00213   
00214   visualization_msgs::Marker
00215   InteractiveCuboidLikelihood::particleToMarker(
00216     const Particle& p)
00217   {
00218     visualization_msgs::Marker marker;
00219     marker.type = visualization_msgs::Marker::CUBE;
00220     marker.scale.x = p.dx;
00221     marker.scale.y = p.dy;
00222     marker.scale.z = p.dz;
00223     marker.pose.orientation.w = 1.0;
00224     marker.color.r = 0.5;
00225     marker.color.g = 0.5;
00226     marker.color.b = 0.5;
00227     marker.color.a = 1.0;
00228     return marker;
00229   }
00230   
00231   visualization_msgs::InteractiveMarker InteractiveCuboidLikelihood::particleToInteractiveMarker(const Particle& p)
00232   {
00233     visualization_msgs::InteractiveMarker int_marker;
00234     int_marker.header.frame_id = frame_id_;
00235     int_marker.header.stamp = ros::Time::now();
00236     int_marker.name = getName();
00237     Eigen::Affine3f pose = particle_.toEigenMatrix();
00238     tf::poseEigenToMsg(pose, int_marker.pose);
00239 
00240     visualization_msgs::InteractiveMarkerControl control;
00241     control.orientation.w = 1;
00242     control.orientation.x = 1;
00243     control.orientation.y = 0;
00244     control.orientation.z = 0;
00245     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00246     int_marker.controls.push_back(control);
00247     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00248     int_marker.controls.push_back(control);
00249 
00250     control.orientation.w = 1;
00251     control.orientation.x = 0;
00252     control.orientation.y = 1;
00253     control.orientation.z = 0;
00254     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00255     int_marker.controls.push_back(control);
00256     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00257     int_marker.controls.push_back(control);
00258 
00259     control.orientation.w = 1;
00260     control.orientation.x = 0;
00261     control.orientation.y = 0;
00262     control.orientation.z = 1;
00263     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00264     int_marker.controls.push_back(control);
00265     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00266     int_marker.controls.push_back(control);
00267     visualization_msgs::InteractiveMarkerControl box_control;
00268     box_control.always_visible = true;
00269     box_control.markers.push_back(particleToMarker(particle_));
00270     int_marker.controls.push_back(box_control);
00271     return int_marker;
00272   }
00273 
00274   
00275   
00276 }
00277 
00278 #include <pluginlib/class_list_macros.h>
00279 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::InteractiveCuboidLikelihood, nodelet::Nodelet);