fast_plane_detection: fast_plane_detection: fast_plane_detector

00001 /*
00002  * Copyright (c) 2011, Jeannette Bohg and Mårten Björkman 
00003  * ({bohg,celle}@csc.kth.se) 
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
00008  * met:
00009  *
00010  *  1.Redistributions of source code must retain the above copyright
00011  *    notice, this list of conditions and the following disclaimer.
00012  *  2.Redistributions in binary form must reproduce the above
00013  *    copyright notice, this list of conditions and the following
00014  *    disclaimer in the documentation and/or other materials provided
00015  *    with the distribution.  
00016  *  3.The name of Jeannette Bohg or Mårten Björkman may not be used to 
00017  *    endorse or promote products derived from this software without 
00018  *    specific prior written permission.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00023  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
00024  * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00025  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00026  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00027  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00028  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00029  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00030  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #include <ros/ros.h>
00034 
00035 #include <fast_plane_detection/Plane.h>
00036 #include <fast_plane_detection/PlaneInRegion.h>
00037 
00038 #include <fast_plane_detector/utils.h>
00039 
00040 #include <geometry_msgs/Vector3Stamped.h>
00041 #include <visualization_msgs/Marker.h>
00042 
00043 int main(int argc, char **argv)
00044 {
00048   ros::init(argc, argv, "detect_plane_in_region");
00049   if (argc != 4)
00050     {
00051     ROS_INFO("usage: detect_plane_in_region X Y Z");
00052     return 1;
00053   }
00054 
00055   // get a ros handle
00056   ros::NodeHandle n;
00057   // create a client for the service add_two_ints
00058   ros::ServiceClient client 
00059     = n.serviceClient<fast_plane_detection::PlaneInRegion>("fast_plane_detection");
00060   // service class AddTwoInts is instantiated
00061   
00063   ros::Publisher plane_marker_pub_;
00064   // publish plane markers
00065   plane_marker_pub_ 
00066     = n.advertise<visualization_msgs::Marker>("region_plane_markers", 1); 
00067 
00068   fast_plane_detection::PlaneInRegion srv;
00069   geometry_msgs::Vector3Stamped point;
00070   point.header.frame_id = "narrow_stereo_optical_frame";
00071   point.header.stamp = ros::Time::now();
00072   point.vector.x = atoll(argv[1]);
00073   point.vector.y = atoll(argv[2]);
00074   point.vector.z = atoll(argv[3]);
00075   // request members are filled
00076   srv.request.point  = point;
00077   srv.request.width  = 10;
00078   srv.request.height = 50;
00079 
00080   int marker_id = 345;
00081 
00082   // service call that is blocking until call is actually made
00083   // is returning true if response is received
00084   if (client.call(srv))
00085   {
00086     ROS_INFO("Service call returned successfully");
00087     ROS_INFO("srv.response.plane.top_left %f %f %f", 
00088              srv.response.plane.top_left.x, 
00089              srv.response.plane.top_left.y,
00090              srv.response.plane.top_left.z);
00091     // first send delete marker
00092     
00093     
00094     visualization_msgs::Marker delete_marker;
00095     delete_marker.header.stamp = ros::Time::now();
00096     delete_marker.header.frame_id = point.header.frame_id;
00097     delete_marker.id = marker_id;
00098     delete_marker.action = visualization_msgs::Marker::DELETE;
00099     delete_marker.ns = "tabletop_node";
00100     plane_marker_pub_.publish(delete_marker);
00101     
00102 
00103     // send current marker
00104     visualization_msgs::Marker planeMarker 
00105       = fast_plane_detection::getPlaneMarker( srv.response.plane.top_left, 
00106                                               srv.response.plane.top_right,
00107                                               srv.response.plane.bottom_left, 
00108                                               srv.response.plane.bottom_right);
00109     planeMarker.header.frame_id = point.header.frame_id;
00110     planeMarker.header.stamp = ros::Time::now();
00111     planeMarker.pose = srv.response.plane.pose.pose;
00112     planeMarker.ns = "tabletop_node";
00113     planeMarker.id = marker_id; //arbitrary
00114     plane_marker_pub_.publish(planeMarker);
00115     
00116 
00117   }  else  {
00118     ROS_ERROR("Failed to call service fast_plane_detection");
00119     return 1;
00120   }
00121   
00122   return 0;
00123 }
fast_plane_detector_client.cpp
