ItemObserver.cpp
Go to the documentation of this file.
00001 
00012 // World Item Observer
00013 #include "world_item_observer/ItemObserver.h"
00014 
00015 // ROS
00016 #include <graspdb/Client.h>
00017 
00018 using namespace std;
00019 using namespace rail::spatial_temporal_learning;
00020 using namespace rail::pick_and_place;
00021 
00022 ItemObserver::ItemObserver() : worldlib::remote::Node()
00023 {
00024   // load the config
00025   okay_ &= this->loadWorldYamlFile();
00026 
00027   // create the client we need
00028   spatial_world_client_ = this->createSpatialWorldClient();
00029   okay_ &= spatial_world_client_->connect();
00030 
00031   // connect to the grasp model database to check for items
00032   int port = pick_and_place::graspdb::Client::DEFAULT_PORT;
00033   string host("127.0.0.1");
00034   string user("ros");
00035   string password("");
00036   string db("graspdb");
00037   node_.getParam("/graspdb/host", host);
00038   node_.getParam("/graspdb/port", port);
00039   node_.getParam("/graspdb/user", user);
00040   node_.getParam("/graspdb/password", password);
00041   node_.getParam("/graspdb/db", db);
00042   pick_and_place::graspdb::Client client(host, port, user, password, db);
00043 
00044   if (client.connect())
00045   {
00046     // add the items
00047     vector<string> objects;
00048     client.getUniqueGraspModelObjectNames(objects);
00049     for (size_t i = 0; i < objects.size(); i++)
00050     {
00051       world_.addItem(worldlib::world::Item(objects[i]));
00052     }
00053     ROS_INFO("Found %lu unique items in the world.", world_.getNumItems());
00054     okay_ &= true;
00055   }
00056 
00057   // connect to the segmented objects topic
00058   string recognized_objects_topic("/object_recognition_listener/recognized_objects");
00059   private_node_.getParam("recognized_objects_topic", recognized_objects_topic);
00060   recognized_objects_sub_ = node_.subscribe(recognized_objects_topic, 1, &ItemObserver::recognizedObjectsCallback,
00061                                             this);
00062 
00063   if (okay_)
00064   {
00065     ROS_INFO("Item Observer Initialized");
00066   }
00067 }
00068 
00069 ItemObserver::~ItemObserver()
00070 {
00071   // clean up the client
00072   delete spatial_world_client_;
00073 }
00074 
00075 void ItemObserver::recognizedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectListConstPtr &objects) const
00076 {
00077   // only work on a non-cleared list
00078   if (!objects->cleared)
00079   {
00080     size_t new_observation_count = 0;
00081     // used to keep track of what was seen where (to mark things as missing)
00082     map<string, vector<string> > seen;
00083     // only utilize recognized objects
00084     for (size_t i = 0; i < objects->objects.size(); i++)
00085     {
00086       const rail_manipulation_msgs::SegmentedObject &o = objects->objects[i];
00087       // transform the centroid to the fixed frame (shift down the Z)
00088       const worldlib::geometry::Position offset(o.centroid.x, o.centroid.y, o.centroid.z - (o.height / 2.0));
00089       const worldlib::geometry::Pose centroid(offset, worldlib::geometry::Orientation(o.orientation));
00090       const string &frame_id = o.point_cloud.header.frame_id;
00091       const worldlib::geometry::Pose p_centroid_world = this->transformToWorld(centroid, frame_id);
00092 
00093       // check if it is on a surface
00094       size_t room_i, surface_i, placement_surface_i;
00095       if (world_.findPlacementSurface(p_centroid_world.getPosition(), room_i, surface_i, placement_surface_i))
00096       {
00097         const worldlib::world::Room &room = world_.getRoom(room_i);
00098         const worldlib::world::Surface &surface = room.getSurface(surface_i);
00099         // check if it is recognized, otherwise just mark the surface as being seen
00100         if (o.recognized)
00101         {
00102           // determine the position of the item on the surface
00103           const worldlib::geometry::Pose p_centroid_room = room.fromParentFrame(p_centroid_world);
00104           const worldlib::geometry::Pose p_centroid_surface = surface.fromParentFrame(p_centroid_room);
00105 
00106           // create and store the Observation
00107           const worldlib::world::Item item(o.name, "", p_centroid_surface, o.width, o.depth, o.height);
00108           spatial_world_client_->addObservation(item, surface, p_centroid_surface);
00109           new_observation_count++;
00110           seen[surface.getName()].push_back(item.getName());
00111         } else
00112         {
00113           seen[surface.getName()].push_back("");
00114         }
00115       }
00116     }
00117 
00118     // check what items were no longer seen on the surface
00119     size_t removed_count = 0;
00120     for (map<string, vector<string> >::iterator iter = seen.begin(); iter != seen.end(); ++iter)
00121     {
00122       const string &surface_name = iter->first;
00123       const vector<string> &items_seen = iter->second;
00124       const vector<worldlib::world::Item> &world_items = world_.getItems();
00125       for (size_t i = 0; i < world_items.size(); i++)
00126       {
00127         // check if the item was seen
00128         const worldlib::world::Item &cur = world_items[i];
00129         if (std::find(items_seen.begin(), items_seen.end(), cur.getName()) == items_seen.end())
00130         {
00131           // check if the item had been there previously
00132           if (spatial_world_client_->itemExistsOnSurface(cur.getName(), surface_name))
00133           {
00134             // mark it as removed
00135             spatial_world_client_->markObservationsAsRemoved(cur.getName(), surface_name);
00136             removed_count++;
00137           }
00138         }
00139       }
00140     }
00141 
00142     ROS_INFO("Added %lu new observations and marked %lu as removed.", new_observation_count, removed_count);
00143   }
00144 }


world_item_observer
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:55:45