recognition_translator.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <actionlib/client/simple_action_client.h>
00032 
00033 #include <tabletop_object_detector/TabletopDetection.h>
00034 #include <object_recognition_msgs/ObjectRecognitionAction.h>
00035 #include <household_objects_database_msgs/DatabaseModelPoseList.h>
00036 #include <household_objects_database_msgs/TranslateRecognitionId.h>
00037 #include <object_manipulator/tools/service_action_wrappers.h>
00038 
00039 using namespace tabletop_object_detector;
00040 using namespace object_recognition_msgs;
00041 using namespace household_objects_database_msgs;
00042 using namespace object_manipulator;
00043 
00044 namespace pr2_interactive_object_detection {
00045 
00046 class RecognitionTranslator
00047 {
00048 private:
00049   ros::NodeHandle root_nh_;
00050   ros::ServiceServer server_;
00051   ServiceWrapper<TranslateRecognitionId> translation_client_;
00052   ActionWrapper<ObjectRecognitionAction> action_client_;
00053 
00054   bool getIdFromRecognitionId(std::string recognition_id, int &db_id)
00055   {
00056     try
00057     {
00058       TranslateRecognitionId srv;
00059       srv.request.recognition_id = recognition_id;
00060       if (!translation_client_.client().call(srv))
00061       {
00062         ROS_ERROR("Call to translation service failed");
00063         return false;
00064       }
00065       if (srv.response.result == srv.response.SUCCESS)
00066       {
00067         db_id = srv.response.household_objects_id;
00068         return true;
00069       }
00070       else if(srv.response.result == srv.response.ID_NOT_FOUND)
00071       {
00072         ROS_ERROR("Recognition id %s not found", recognition_id.c_str());
00073       }
00074       else
00075       {
00076         ROS_ERROR("Translate is service returned an error");
00077       }
00078       return false;
00079     }
00080     catch (ServiceNotFoundException &ex)
00081     {
00082       ROS_ERROR("Translation service not found");
00083       return false;
00084     }
00085   }
00086 
00087   bool serviceCB(TabletopDetection::Request &request, TabletopDetection::Response &response)
00088   {
00089     response.detection.result = response.detection.OTHER_ERROR;
00090     try
00091     {
00092       if (request.return_clusters || !request.return_models || request.num_models < 1)
00093       {
00094         ROS_INFO("TOD recognition can not return clusters; set return_clusters to false and return_models to true");
00095         return true;
00096       }
00097       //RecognizeObjectsGoal goal;
00098       ObjectRecognitionGoal goal;
00099       ROS_INFO("Calling recognition action");
00100       action_client_.client().sendGoal(goal);
00101       while (!action_client_.client().waitForResult(ros::Duration(2.0)) && root_nh_.ok())
00102       {
00103         ROS_INFO("Waiting for recognition action...");
00104       }
00105       if (!root_nh_.ok()) return true;
00106       if (action_client_.client().getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00107       {
00108         ROS_ERROR("Object recognition action failed");
00109         return true;
00110       }
00111       ObjectRecognitionResult result = *(action_client_.client().getResult());
00112 
00113       response.detection.result = response.detection.SUCCESS;
00114       ROS_INFO("Recognition action returned %zd results", result.recognized_objects.objects.size());
00115       for (size_t i=0; i<result.recognized_objects.objects.size(); i++)
00116       {
00117         DatabaseModelPose model_pose;
00118         if (!getIdFromRecognitionId(result.recognized_objects.objects[i].id.id, model_pose.model_id))
00119         {
00120           ROS_ERROR("Failed to get household ID for recognition ID %s", result.recognized_objects.objects[i].id.id.c_str());
00121           continue;
00122         }
00123         model_pose.pose.header = result.recognized_objects.objects[i].pose.header;
00124         model_pose.pose.pose = result.recognized_objects.objects[i].pose.pose.pose;
00125         //model_pose.confidence = result.confidence[i];
00126         //The world's most horrible hack
00127         //the rest of system still uses thresholds from the old tabletop object recognition
00128         model_pose.confidence = 0.002;
00129         DatabaseModelPoseList model_pose_list;
00130         model_pose_list.model_list.push_back(model_pose);
00131         response.detection.models.push_back(model_pose_list);
00132       }
00133       
00134     }
00135     catch (ServiceNotFoundException &ex)
00136     {
00137       ROS_ERROR("Recognition action server not found");
00138     }
00139     return true;
00140   }
00141 
00142 public:
00143   RecognitionTranslator() : 
00144     root_nh_(""),
00145     translation_client_("objects_database_node/translate_id"),
00146     action_client_("/object_recognition/recognize_objects", true)
00147   {
00148     server_ = root_nh_.advertiseService("object_recognition_translated", &RecognitionTranslator::serviceCB, this);    
00149   }
00150 };
00151 
00152 }
00153 
00154 int main(int argc, char **argv)
00155 {
00156   ros::init(argc, argv, "recognition_translator");
00157   pr2_interactive_object_detection::RecognitionTranslator rt;
00158   ros::spin();
00159   return 0;  
00160 }


pr2_interactive_object_detection
Author(s): David Gossow
autogenerated on Mon Oct 6 2014 12:02:20