publish_database_object.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034   
00035 #include <string>
00036 
00037 #include <ros/ros.h>
00038 
00039 #include <visualization_msgs/Marker.h>
00040 
00041 #include <household_objects_database_msgs/GetModelMesh.h>
00042 
00043 visualization_msgs::Marker getFitMarker(const shape_msgs::Mesh &mesh)
00044 {
00045   visualization_msgs::Marker marker;
00046   marker.action = visualization_msgs::Marker::ADD;
00047   marker.lifetime = ros::Duration();
00048   marker.type = visualization_msgs::Marker::POINTS;
00049   marker.color.a = 1.0;
00050   marker.color.r = 1.0;
00051   marker.color.g = 0.0;
00052   marker.color.b = 0.0;
00053   marker.scale.x = 0.003;
00054   marker.scale.y = 0.003;
00055   marker.points.insert(marker.points.begin(), mesh.vertices.begin(), mesh.vertices.end());
00056   //add a line starting from origin along the x direction of the object
00057   for (int i=0; i<40; i++)
00058   {
00059     geometry_msgs::Point p;
00060     p.x = 0.0005 * i;
00061     p.y = p.z = 0.0;
00062     marker.points.push_back(p);
00063   }
00064   return marker;
00065 }
00066 
00067 int main(int argc, char **argv)
00068 {
00069   if (argc < 2) 
00070   {
00071     ROS_ERROR("Usage: publish_database_object model_id");
00072     exit(0);
00073   }
00074 
00075   //create handlers and initialize ros
00076   ros::Publisher marker_pub_;
00077   ros::ServiceClient get_model_mesh_srv_;
00078   ros::init(argc, argv, "publish_database_mesh");
00079   ros::NodeHandle nh_("");
00080 
00081   //advertise markers
00082   marker_pub_ = nh_.advertise<visualization_msgs::Marker>(nh_.resolveName("database_object"), 10);
00083   //wait a little but to make sure rviz subscribes
00084   ros::Duration(1.0).sleep();
00085 
00086   //subscribe to service that gets a model mesh form the database
00087   std::string get_model_mesh_srv_name = "/objects_database_node/get_model_mesh";
00088   while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) && nh_.ok() ) 
00089   {
00090     ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
00091   }
00092   if (!nh_.ok()) exit(0);
00093   get_model_mesh_srv_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>
00094     (get_model_mesh_srv_name, true);
00095 
00096   int model_id = atoi(argv[1]);
00097   ROS_INFO("Publishing mesh for model %d", model_id);
00098   
00099   //get the mesh
00100   household_objects_database_msgs::GetModelMesh get_mesh;
00101   get_mesh.request.model_id = model_id;
00102   if ( !get_model_mesh_srv_.call(get_mesh) || 
00103        get_mesh.response.return_code.code != get_mesh.response.return_code.SUCCESS )
00104   {
00105     ROS_ERROR("Failed to call database get mesh service for marker display");
00106     exit(0);
00107   }
00108 
00109   visualization_msgs::Marker fitMarker =  getFitMarker(get_mesh.response.mesh);
00110   //change here with the desired frame
00111   fitMarker.header.frame_id = "foo_frame";
00112   fitMarker.header.stamp = ros::Time::now();
00113   //publish at origin
00114   fitMarker.pose.orientation.w = 1;
00115   fitMarker.ns = "publish_database_object";
00116   fitMarker.id = 0;
00117   marker_pub_.publish(fitMarker);
00118 
00119   ROS_INFO("Marker published");
00120   //wait a little but to make sure the marker gets to rviz
00121   ros::Duration(1.0).sleep();
00122 }


tabletop_object_detector
Author(s): Marius Muja and Matei Ciocarlie
autogenerated on Mon Oct 6 2014 11:45:30