ias_drawer_executive: ias_drawer_executive: GraspClusterClient.cpp Source File

00001 
00002 /*
00003  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of Willow Garage, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <ros/ros.h>
00032 
00033 #include <signal.h>
00034 #include <sensor_msgs/PointCloud2.h>
00035 #include <object_manipulation_msgs/GraspPlanning.h>
00036 #include <sensor_msgs/point_cloud_conversion.h>
00037 #include <tf/transform_listener.h>
00038 #include <visualization_msgs/Marker.h>
00039 #include <visualization_msgs/MarkerArray.h>
00040 
00041 int side_ = 0;
00042 
00043 tf::Stamped<tf::Pose> tool2wrist(tf::Stamped<tf::Pose> toolPose, double dist = -.18)
00044 {
00045     tf::Stamped<tf::Pose> wrist2tool;
00046     //wrist2tool.frame_id_ = (side_==0) ? "r_wrist_roll_link" : "l_wrist_roll_link";
00047     wrist2tool.stamp_ = ros::Time();
00048     wrist2tool.setOrigin(btVector3(dist,0,0));
00049     wrist2tool.setRotation(btQuaternion(0,0,0,1));
00050     tf::Stamped<tf::Pose> ret;
00051     ret = toolPose;
00052     ret *= wrist2tool;
00053     return ret;
00054 }
00055 
00056 
00057 tf::Stamped<tf::Pose> wrist2tool(tf::Stamped<tf::Pose> toolPose, double dist = .18)
00058 {
00059     tf::Stamped<tf::Pose> wrist2tool;
00060     //wrist2tool.frame_id_ = (side_==0) ? "r_wrist_roll_link" : "l_wrist_roll_link";
00061     wrist2tool.stamp_ = ros::Time();
00062     wrist2tool.setOrigin(btVector3(dist,0,0));
00063     wrist2tool.setRotation(btQuaternion(0,0,0,1));
00064     tf::Stamped<tf::Pose> ret;
00065     ret = toolPose;
00066     ret *= wrist2tool;
00067     return ret;
00068 }
00069 
00070 
00071 
00072 tf::TransformListener *listener_=0;
00073 
00074 tf::Stamped<tf::Pose> getPoseIn(const char target_frame[], tf::Stamped<tf::Pose>src)
00075 {
00076 
00077     if (src.frame_id_ == "NO_ID_STAMPED_DEFAULT_CONSTRUCTION") {
00078         ROS_ERROR("Frame not in TF: %s", "NO_ID_STAMPED_DEFAULT_CONSTRUCTION");
00079         tf::Stamped<tf::Pose> pose;
00080         return pose;
00081     }
00082 
00083     if (!listener_)
00084         listener_ = new tf::TransformListener();
00085 
00086     tf::Stamped<tf::Pose> transform;
00087     //this shouldnt be here TODO
00088     src.stamp_ = ros::Time(0);
00089 
00090     listener_->waitForTransform(src.frame_id_, target_frame,
00091                                 ros::Time(0), ros::Duration(30.0));
00092     bool transformOk = false;
00093     while (!transformOk)
00094     {
00095         try
00096         {
00097             transformOk = true;
00098             listener_->transformPose(target_frame, src, transform);
00099         }
00100         catch (tf::TransformException ex)
00101         {
00102             ROS_ERROR("getPoseIn %s",ex.what());
00103             // dirty:
00104             src.stamp_ = ros::Time(0);
00105             transformOk = false;
00106         }
00107         ros::spinOnce();
00108     }
00109     return transform;
00110 }
00111 
00112 
00113 int marker_id = 0;
00114 ros::Publisher vis_pub;
00115 visualization_msgs::MarkerArray mar;
00116 
00117 void publishMarker(tf::Stamped<tf::Pose> mark)
00118 {
00119         ros::NodeHandle node_handle;
00120         if (!marker_id) {
00121            vis_pub = node_handle.advertise<visualization_msgs::MarkerArray>( "/grasp_marker_array", 10, true );
00122         }
00123 
00124         visualization_msgs::Marker marker;
00125         marker.header.frame_id = mark.frame_id_;
00126         marker.header.stamp = ros::Time::now();
00127         marker.ns = "grasps";
00128         marker.id =  ++marker_id;
00129         marker.type = visualization_msgs::Marker::ARROW;
00130         marker.action = visualization_msgs::Marker::ADD;
00131 
00132         marker.pose.orientation.x = mark.getRotation().x();
00133         marker.pose.orientation.y = mark.getRotation().y();
00134         marker.pose.orientation.z = mark.getRotation().z();
00135         marker.pose.orientation.w = mark.getRotation().w();
00136         marker.pose.position.x = mark.getOrigin().x();
00137         marker.pose.position.y = mark.getOrigin().y();
00138         marker.pose.position.z = mark.getOrigin().z();
00139         marker.scale.x = 0.05;
00140         marker.scale.y = 0.05;
00141         marker.scale.z = 0.05;
00142         marker.color.r = 1.0;
00143         marker.color.g = 0.0;
00144         marker.color.b = 0.0;
00145         marker.color.a = 1.0;
00146 
00147         ROS_INFO("PUBLISH MARKER %i", marker.id);
00148 
00149         mar.markers.push_back(marker);
00150 }
00151 
00152 
00153 int main (int argc, char **argv)
00154 {
00155 
00156     //signal(SIGINT,quit);
00157 
00158     ros::init(argc, argv, "grasp_cluster_client");
00159 
00160     system("rosrun dynamic_reconfigure dynparam set  /camera_synchronizer_node '{projector_mode: 3, narrow_stereo_trig_mode: 3}'");
00161 
00162     ros::NodeHandle n_;
00163 
00164     sensor_msgs::PointCloud2 pc2 = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/stereo_table_cluster_detector/pointcloud_minmax_3d_node/cloud_cluster"));
00165     sensor_msgs::PointCloud pc;
00166     sensor_msgs::convertPointCloud2ToPointCloud(pc2,pc);
00167 
00168     ros::ServiceClient client = n_.serviceClient<object_manipulation_msgs::GraspPlanning>("/plan_point_cluster_grasp");
00169 
00170     object_manipulation_msgs::GraspPlanning gp;
00171 
00172     gp.request.arm_name = "right_arm";
00173     //gp.request.target.reference_frame_id = pc2.header.frame_id;
00174     gp.request.target.cluster = pc;
00175     ROS_INFO("CALLING");
00176     if (client.call(gp)) {
00177         ROS_INFO("RETURNED: %zu Grasps", gp.response.grasps.size());
00178         for (size_t i = 0; i < gp.response.grasps.size(); ++i) {
00179             ROS_INFO("GRASP %zu: frame %s", i, gp.response.grasps[i].grasp_posture.header.frame_id.c_str());
00180 
00181             tf::Stamped<tf::Pose> grasp;
00182             tf::Stamped<tf::Pose> pregrasp;
00183             grasp.frame_id_ = gp.response.grasps[i].grasp_posture.header.frame_id = "/base_link";
00184             grasp.setOrigin(btVector3(gp.response.grasps[i].grasp_pose.position.x,gp.response.grasps[i].grasp_pose.position.y,gp.response.grasps[i].grasp_pose.position.z));
00185             grasp.setRotation(btQuaternion(gp.response.grasps[i].grasp_pose.orientation.x,gp.response.grasps[i].grasp_pose.orientation.y,gp.response.grasps[i].grasp_pose.orientation.z,gp.response.grasps[i].grasp_pose.orientation.w));
00186             pregrasp = wrist2tool(grasp,.13);
00187             grasp = wrist2tool(grasp);
00188 
00189             grasp=getPoseIn("/map",grasp);
00190             pregrasp=getPoseIn("/map",pregrasp);
00191 
00192             ROS_INFO("PREGRASP[map] %f %f %f %f %f %f %f", pregrasp.getOrigin().x(),pregrasp.getOrigin().y(),pregrasp.getOrigin().z(),
00193                 pregrasp.getRotation().x(),pregrasp.getRotation().y(),pregrasp.getRotation().z(),pregrasp.getRotation().w());
00194             ROS_INFO("GRASP[map] %f %f %f %f %f %f %f", grasp.getOrigin().x(),grasp.getOrigin().y(),grasp.getOrigin().z(),
00195                 grasp.getRotation().x(),grasp.getRotation().y(),grasp.getRotation().z(),grasp.getRotation().w());
00196 
00197             publishMarker(grasp);
00198 
00199         }
00200     }
00201 
00202     vis_pub.publish( mar );
00203 
00204     ros::AsyncSpinner spinner(2); // Use 4 threads
00205     spinner.start();
00206     ros::waitForShutdown();
00207     return 0;
00208 }