hinted_handle_estimator.h
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 
00037 #ifndef JSK_PCL_ROS_HINTED_HANDLE_ESTIMATOR_H_
00038 #define JSK_PCL_ROS_HINTED_HANDLE_ESTIMATOR_H_
00039 
00040 #include <jsk_topic_tools/diagnostic_nodelet.h>
00041 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00042 #include <jsk_recognition_msgs/SimpleHandle.h>
00043 #include <geometry_msgs/PointStamped.h>
00044 #include <geometry_msgs/PoseStamped.h>
00045 #include <geometry_msgs/Pose.h>
00046 #include <std_msgs/Float64.h>
00047 #include <sensor_msgs/PointCloud2.h>
00048 #include <visualization_msgs/Marker.h>
00049 #include <visualization_msgs/MarkerArray.h>
00050 #include <message_filters/subscriber.h>
00051 #include <message_filters/time_synchronizer.h>
00052 #include <message_filters/synchronizer.h>
00053 #include <message_filters/sync_policies/approximate_time.h>
00054 #include <tf/transform_listener.h>
00055 #include <tf/tf.h>
00056 
00057 
00058 class handle_model
00059 {
00060 public:
00061   double finger_l;
00062   double finger_d;
00063   double finger_w;
00064   double arm_l;
00065   double arm_d;
00066   double arm_w;
00067   handle_model(){
00068   }
00069 };
00070 
00071 tf::Transform pose_to_tf(geometry_msgs::Pose pose){
00072   return tf::Transform(tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
00073 }
00074 geometry_msgs::Pose tf_to_pose(tf::Transform transform){
00075   geometry_msgs::Pose pose;
00076   tf::Quaternion q;
00077   transform.getBasis().getRotation(q);
00078   pose.orientation.x = q.getX(); pose.orientation.y=q.getY(); pose.orientation.z=q.getZ(), pose.orientation.w=q.getW();
00079   pose.position.x=transform.getOrigin().getX(), pose.position.y=transform.getOrigin().getY(), pose.position.z=transform.getOrigin().getZ();
00080   return pose;
00081 }
00082 geometry_msgs::Pose change_pose(geometry_msgs::Pose base_pose, geometry_msgs::Pose child_pose){
00083   return tf_to_pose(pose_to_tf(base_pose)*pose_to_tf(child_pose));
00084 }
00085 
00086 visualization_msgs::Marker make_box(float s_x, float s_y, float s_z, float r, float g, float b, float x, float y, float z)
00087 {
00088   visualization_msgs::Marker marker;
00089   marker.type = visualization_msgs::Marker::CUBE;
00090   marker.scale.x = s_x;
00091   marker.scale.y = s_y;
00092   marker.scale.z = s_z;
00093   marker.color.r = r;
00094   marker.color.g = g;
00095   marker.color.b = b;
00096   marker.color.a = 1.0;
00097   marker.pose.position.x = x;
00098   marker.pose.position.y = y;
00099   marker.pose.position.z = z;
00100   marker.pose.orientation.x = marker.pose.orientation.y = marker.pose.orientation.z = 0;
00101   marker.pose.orientation.w = 1.0;
00102   return marker;
00103 }
00104 
00105 visualization_msgs::MarkerArray make_handle_array(geometry_msgs::PoseStamped pose, handle_model handle){
00106   visualization_msgs::MarkerArray markerArray_msg;
00107   visualization_msgs::Marker marker_msg = make_box(handle.finger_l, handle.finger_w, handle.finger_d, 1.0, 0, 0, handle.finger_l*0.5, handle.arm_w*0.5 -handle.finger_w*0.5, 0);
00108   marker_msg.pose = change_pose(pose.pose, marker_msg.pose);
00109   marker_msg.header = pose.header;
00110   marker_msg.ns = std::string("debug_grasp_model");
00111   marker_msg.id = 1;
00112   markerArray_msg.markers.push_back(marker_msg);
00113   marker_msg = make_box(handle.finger_l, handle.finger_w, handle.finger_d, 0, 1.0, 0, handle.finger_l*0.5, -handle.arm_w*0.5 +handle.finger_w*0.5, 0);
00114   marker_msg.pose = change_pose(pose.pose, marker_msg.pose);
00115   marker_msg.header = pose.header;
00116   marker_msg.ns = std::string("debug_grasp_model");
00117   marker_msg.id = 2;
00118   markerArray_msg.markers.push_back(marker_msg);
00119   marker_msg = make_box(handle.arm_l-handle.finger_l, handle.arm_w, handle.arm_d, 0, 0, 1.0, -(handle.arm_l-handle.finger_l)*0.5, 0, 0);
00120   marker_msg.pose = change_pose(pose.pose, marker_msg.pose);
00121   marker_msg.header = pose.header;
00122   marker_msg.ns = std::string("debug_grasp_model");
00123   marker_msg.id = 3;
00124   markerArray_msg.markers.push_back(marker_msg);
00125   return markerArray_msg;
00126 }
00127 
00128 namespace jsk_pcl_ros
00129 {
00130   class HintedHandleEstimator: public jsk_topic_tools::DiagnosticNodelet
00131   {
00132   public:
00133     HintedHandleEstimator(): DiagnosticNodelet("HintedHandleEstimator") {}
00134     typedef message_filters::sync_policies::ApproximateTime<
00135       sensor_msgs::PointCloud2,
00136       geometry_msgs::PointStamped 
00137       > SyncPolicy;
00138     tf::TransformListener listener_;
00139     handle_model handle;
00140   protected:
00141     virtual void onInit();
00142     virtual void subscribe();
00143     virtual void unsubscribe();
00144     virtual void estimate(
00145       const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00146       const geometry_msgs::PointStampedConstPtr &point_msg);
00147 
00148     boost::mutex mutex_;
00149     ros::Publisher pub_pose_;
00150     ros::Publisher pub_length_;
00151     ros::Publisher pub_handle_;
00152     ros::Publisher pub_debug_marker_;
00153     ros::Publisher pub_debug_marker_array_;
00154     message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_;
00155     message_filters::Subscriber<geometry_msgs::PointStamped> sub_point_;
00156     boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
00157   private:
00158     
00159   };
00160 }
00161 
00162 #endif


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