furniture_tracker.cpp
Go to the documentation of this file.
00001 
00010 #include <rail_ceiling/furniture_tracker.h>
00011 #include <fstream>
00012 
00013 using namespace std;
00014 
00015 FurnitureTracker::FurnitureTracker()
00016 {
00017   // private node handle
00018   ros::NodeHandle private_nh("~");
00019 
00020   // get number of marker topics (i.e. number of overhead cameras)
00021   int numMarkerTopics;
00022   private_nh.param("num_marker_topics", numMarkerTopics, 5);
00023   private_nh.param("read_initial_poses", readInitialPoses, false);
00024 
00025   // get config files
00026   stringstream ss;
00027   ss << ros::package::getPath("rail_ceiling") << "/config/markers.yaml";
00028   string markerConfigFile;
00029   private_nh.param("markers_config", markerConfigFile, ss.str());
00030   ss.str("");
00031   ss << ros::package::getPath("rail_ceiling") << "/config/furniture_footprints.yaml";
00032   string furnitureConfigFile;
00033   private_nh.param("furniture_footprints_config", furnitureConfigFile, ss.str());
00034 
00035   readConfigFiles(markerConfigFile, furnitureConfigFile);
00036   lastPublishedPoses.resize(furnitureList.size());
00037 
00038   furnitureLayerPub = n.advertise<rail_ceiling::Obstacles>("furniture_layer/update_obstacles", 1);
00039 
00040   // subscribe to marker topics
00041   markerSubscribers.resize(numMarkerTopics);
00042   for (unsigned int i = 0; i < numMarkerTopics; i ++)
00043   {
00044     stringstream topicStream;
00045     topicStream << "ceiling_cam_tracker_" << i << "/ar_pose_marker";
00046     markerSubscribers[i] = n.subscribe(topicStream.str(), 1, &FurnitureTracker::markerCallback, this);
00047   }
00048 
00049   allPosesServer = n.advertiseService("furniture_tracker/get_all_poses", &FurnitureTracker::getAllPoses, this);
00050 }
00051 
00052 void FurnitureTracker::readConfigFiles(std::string markerConfigFile, std::string furnitureConfigFile)
00053 {
00054   // parse the marker configuration file
00055 #ifdef YAMLCPP_GT_0_5_0
00056   YAML::Node markerConfig = YAML::LoadFile(markerConfigFile);
00057   unsigned int id = 0;
00058   for (size_t i = 0; i < markerConfig.size(); i ++)
00059   {
00060     Furniture f;
00061 
00062     // load the ID and type
00063     f.id = id;
00064     id ++;
00065     f.type = markerConfig[i]["type"].as<string>();
00066 
00067     //optionally load
00068     if (readInitialPoses && markerConfig[i]["initial_pose"])
00069     {
00070       geometry_msgs::Pose2D initialPose;
00071       initialPose.x = markerConfig[i]["initial_pose"][0].as<double>();
00072       initialPose.y = markerConfig[i]["initial_pose"][1].as<double>();
00073       initialPose.theta = markerConfig[i]["initial_pose"][2].as<double>();
00074       furniturePoses.push_back(initialPose);
00075       ROS_INFO("Read initial position for furniture piece %lu", i);
00076     }
00077     else
00078     {
00079       furniturePoses.resize(furniturePoses.size() + 1);
00080     }
00081 
00082     // load marker information
00083     YAML::Node markers = markerConfig[i]["markers"];
00084     f.markers.resize(markers.size());
00085     for (size_t j = 0; j < markers.size(); j ++)
00086     {
00087       f.markers[j].id = markers[j]["id"].as<int>();
00088       f.markers[j].fid = f.id;
00089       f.markers[j].pose.x = markers[j]["x"].as<double>();
00090       f.markers[j].pose.y = markers[j]["y"].as<double>();
00091       f.markers[j].pose.theta = markers[j]["theta"].as<double>();
00092       if (f.markers[j].id >= markerList.size())
00093         markerList.resize(f.markers[j].id + 1);
00094       markerList[f.markers[j].id] = f.markers[j];
00095     }
00096 
00097     // store the furniture piece
00098     furnitureList.push_back(f);
00099   }
00100 #else
00101   ifstream fin(markerConfigFile.c_str());
00102   YAML::Parser markerParser(fin);
00103   YAML::Node markerConfig;
00104   markerParser.GetNextDocument(markerConfig);
00105   unsigned int id = 0;
00106   for (size_t i = 0; i < markerConfig.size(); i ++)
00107   {
00108     Furniture f;
00109 
00110     // load the ID and type
00111     f.id = id;
00112     id ++;
00113     markerConfig[i]["type"] >> f.type;
00114 
00115     //optionally load
00116     if (readInitialPoses)
00117     {
00118       if (const YAML::Node *poseNode = markerConfig[i].FindValue("initial_pose"))
00119       {
00120         geometry_msgs::Pose2D initialPose;
00121         markerConfig[i]["initial_pose"][0] >> initialPose.x;
00122         markerConfig[i]["initial_pose"][1] >> initialPose.y;
00123         markerConfig[i]["initial_pose"][2] >> initialPose.theta;
00124         furniturePoses.push_back(initialPose);
00125         ROS_INFO("Read initial position for furniture piece %lu", i);
00126       }
00127       else
00128       {
00129         furniturePoses.resize(furniturePoses.size() + 1);
00130       }
00131     }
00132     else
00133     {
00134       furniturePoses.resize(furniturePoses.size() + 1);
00135     }
00136 
00137     // load marker information
00138     const YAML::Node &markers = markerConfig[i]["markers"];
00139     f.markers.resize(markers.size());
00140     for (size_t j = 0; j < markers.size(); j ++)
00141     {
00142       markers[j]["id"] >> f.markers[j].id;
00143       f.markers[j].fid = f.id;
00144       markers[j]["x"] >> f.markers[j].pose.x;
00145       markers[j]["y"] >> f.markers[j].pose.y;
00146       markers[j]["theta"] >> f.markers[j].pose.theta;
00147       if (f.markers[j].id >= markerList.size())
00148         markerList.resize(f.markers[j].id + 1);
00149       markerList[f.markers[j].id] = f.markers[j];
00150     }
00151 
00152     // store the furniture piece
00153     furnitureList.push_back(f);
00154   }
00155 #endif
00156 
00157   ROS_INFO("Read marker configurations for %lu pieces of furniture.", furnitureList.size());
00158 
00159 #ifdef YAMLCPP_GT_0_5_0
00160   // parse the furniture footprints configuration file
00161   YAML::Node furnitureConfig = YAML::LoadFile(furnitureConfigFile);
00162   for (size_t i = 0; i < furnitureConfig.size(); i ++)
00163   {
00164     FurnitureTransforms ft;
00165     ft.name = furnitureConfig[i]["name"].as<string>();
00166     if (furnitureConfig[i]["localization_footprint"])
00167     {
00168       YAML::Node polygons = furnitureConfig[i]["localization_footprint"];
00169       ft.localizationFootprint.resize(polygons.size());
00170       for (size_t j = 0; j < polygons.size(); j ++)
00171       {
00172         YAML::Node vertices = polygons[j]["polygon"];
00173         ft.localizationFootprint[j].points.resize(vertices.size());
00174         for (size_t k = 0; k < vertices.size(); k ++)
00175         {
00176           ft.localizationFootprint[j].points[k].x = vertices[k][0].as<float>();
00177           ft.localizationFootprint[j].points[k].y = vertices[k][1].as<float>();
00178           ft.localizationFootprint[j].points[k].z = 0.0;
00179         }
00180       }
00181     }
00182     if (furnitureConfig[i]["navigation_footprint"])
00183     {
00184       YAML::Node polygons = furnitureConfig[i]["navigation_footprint"];
00185       ft.navigationFootprint.resize(polygons.size());
00186       for (size_t j = 0; j < polygons.size(); j ++)
00187       {
00188         YAML::Node vertices = polygons[j]["polygon"];
00189         ft.navigationFootprint[j].points.resize(vertices.size());
00190         for (size_t k = 0; k < vertices.size(); k ++)
00191         {
00192           ft.navigationFootprint[j].points[k].x = vertices[k][0].as<float>();
00193           ft.navigationFootprint[j].points[k].y = vertices[k][1].as<float>();
00194           ft.navigationFootprint[j].points[k].z = 0.0;
00195         }
00196       }
00197     }
00198     footprintTransforms.push_back(ft);
00199   }
00200 #else
00201   ifstream fin2(furnitureConfigFile.c_str());
00202   YAML::Parser furnitureParser(fin2);
00203   YAML::Node furnitureConfig;
00204   furnitureParser.GetNextDocument(furnitureConfig);
00205   for (size_t i = 0; i < furnitureConfig.size(); i ++)
00206   {
00207     FurnitureTransforms ft;
00208     furnitureConfig[i]["name"] >> ft.name;
00209     if (const YAML::Node *lfNode = furnitureConfig[i].FindValue("localization_footprint"))
00210     {
00211       const YAML::Node &polygons = furnitureConfig[i]["localization_footprint"];
00212       ft.localizationFootprint.resize(polygons.size());
00213       for (size_t j = 0; j < polygons.size(); j ++)
00214       {
00215         const YAML::Node &vertices = polygons[j]["polygon"];
00216         ft.localizationFootprint[j].points.resize(vertices.size());
00217         for (size_t k = 0; k < vertices.size(); k ++)
00218         {
00219           vertices[k][0] >> ft.localizationFootprint[j].points[k].x;
00220           vertices[k][1] >> ft.localizationFootprint[j].points[k].y;
00221           ft.localizationFootprint[j].points[k].z = 0.0;
00222         }
00223       }
00224     }
00225     if (const YAML::Node *nfNode = furnitureConfig[i].FindValue("navigation_footprint"))
00226     {
00227       const YAML::Node &polygons = furnitureConfig[i]["navigation_footprint"];
00228       ft.navigationFootprint.resize(polygons.size());
00229       for (size_t j = 0; j < polygons.size(); j ++)
00230       {
00231         const YAML::Node &vertices = polygons[j]["polygon"];
00232         ft.navigationFootprint[j].points.resize(vertices.size());
00233         for (size_t k = 0; k < vertices.size(); k ++)
00234         {
00235           vertices[k][0] >> ft.navigationFootprint[j].points[k].x;
00236           vertices[k][1] >> ft.navigationFootprint[j].points[k].y;
00237           ft.navigationFootprint[j].points[k].z = 0.0;
00238         }
00239       }
00240     }
00241     footprintTransforms.push_back(ft);
00242   }
00243 #endif
00244 
00245   ROS_INFO("Read furniture footprints for:");
00246   for (unsigned int i = 0; i < footprintTransforms.size(); i ++)
00247   {
00248     ROS_INFO("%s", footprintTransforms[i].name.c_str());
00249   }
00250 }
00251 
00252 void FurnitureTracker::markerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
00253 {
00254   //update furniture poses based on new marker information
00255   for (unsigned int i = 0; i < msg->markers.size(); i ++)
00256   {
00257     //ignore if an unused marker is seen
00258     if (msg->markers[i].id >= markerList.size())
00259       continue;
00260     //uninitialized marker check
00261     if (markerList[msg->markers[i].id].id == 0)
00262       continue;
00263 
00264     Marker marker = markerList[msg->markers[i].id];
00265     unsigned int furnitureID = marker.fid;
00266     geometry_msgs::PoseStamped transformedPose;
00267     geometry_msgs::PoseStamped markerPose;
00268     markerPose = msg->markers[i].pose;
00269     markerPose.header.frame_id = msg->markers[i].header.frame_id;
00270     tfListener.transformPose("/map", markerPose, transformedPose);
00271     geometry_msgs::Pose2D furniturePose;
00272     float x = (float)transformedPose.pose.position.x;
00273     float y = (float)transformedPose.pose.position.y;
00274     float theta = (float)tf::getYaw(transformedPose.pose.orientation);
00275     float xt = -(float)marker.pose.x;
00276     float yt = -(float)marker.pose.y;
00277     float thetat = -(float)marker.pose.theta;
00278     furniturePose.x = x + xt*cos(theta + thetat) - yt*sin(theta + thetat);
00279     furniturePose.y = y + yt*cos(theta + thetat) + xt*sin(theta + thetat);
00280     furniturePose.theta = theta + thetat;
00281 
00282     if (furnitureID >= furniturePoses.size())
00283       furniturePoses.resize(furnitureID + 1);
00284     furniturePoses[furnitureID] = furniturePose;
00285   }
00286 }
00287 
00288 void FurnitureTracker::publishFurniturePoses()
00289 {
00290   rail_ceiling::Obstacles obstacles;
00291   bool updatedFurniturePose = false;
00292   for (unsigned int i = 0; i < furnitureList.size(); i ++)
00293   {
00294     int index = furnitureList[i].id;
00295     string type = furnitureList[i].type;
00296     //check if a marker has been received for the listed piece of furniture
00297     if (index < furniturePoses.size())
00298     {
00299       //check if a pose has been calculated for the listed piece of furniture
00300       if (updated(furniturePoses[index]))
00301       {
00302         //check if a pose has ever been published for this piece of furniture
00303         if (updated(lastPublishedPoses[index]))
00304         {
00305           //publish pose if it has changed significantly
00306           if (sqrt(pow(lastPublishedPoses[index].x - furniturePoses[index].x, 2) + pow(lastPublishedPoses[index].y - furniturePoses[index].y, 2)) > POSITION_THRESHOLD
00307                 || fabs(lastPublishedPoses[index].theta - furniturePoses[index].theta) > ANGULAR_THRESHOLD)
00308           {
00309             //DEBUG
00310             if (sqrt(pow(lastPublishedPoses[index].x - furniturePoses[index].x, 2) + pow(lastPublishedPoses[index].y - furniturePoses[index].y, 2)) > POSITION_THRESHOLD)
00311               ROS_INFO("Updated %s (id: %d) because of positional change.", type.c_str(), index);
00312             else if (fabs(lastPublishedPoses[index].theta - furniturePoses[index].theta) > ANGULAR_THRESHOLD)
00313               ROS_INFO("Updated %s (id: %d) because of angular change.", type.c_str(), index);
00314 
00315             updateMessages(index, type, &obstacles);
00316             updatedFurniturePose = true;
00317             lastPublishedPoses[index] = furniturePoses[index];
00318           }
00319         }
00320         else
00321         {
00322           //DEBUG
00323           ROS_INFO("Initial publish for %s (id: %d).", type.c_str(), index);
00324 
00325           //initial pose publish
00326           updateMessages(index, type, &obstacles);
00327           updatedFurniturePose = true;
00328           lastPublishedPoses[index] = furniturePoses[index];
00329         }
00330       }
00331     }
00332   }
00333 
00334   if (updatedFurniturePose)
00335   {
00336     furnitureLayerPub.publish(obstacles);
00337   }
00338 }
00339 
00340 bool FurnitureTracker::getAllPoses(rail_ceiling::GetAllObstacles::Request &req, rail_ceiling::GetAllObstacles::Response &res)
00341 {
00342   rail_ceiling::Obstacles obstacles;
00343   for (unsigned int i = 0; i < furniturePoses.size(); i ++)
00344   {
00345     if (updated(furniturePoses[i]))
00346     {
00347       int index = furnitureList[i].id;
00348       string type = furnitureList[i].type;
00349       FurnitureTransforms footprints;
00350       bool footprintsSet = false;
00351       for (unsigned int j = 0; j < footprintTransforms.size(); j ++)
00352       {
00353         if (footprintTransforms[j].name.compare(type) == 0)
00354         {
00355           footprints = footprintTransforms[j];
00356           footprintsSet = true;
00357           break;
00358         }
00359       }
00360       if (!footprintsSet)
00361       {
00362         ROS_INFO("Could not find footprint information for furniture of type %s.", type.c_str());
00363       }
00364       else
00365       {
00366         fillFootprintInformation(index, footprints.localizationFootprint, &obstacles, true);
00367         fillFootprintInformation(index, footprints.navigationFootprint, &obstacles, false);
00368       }
00369     }
00370   }
00371   res.navigationObstacles = obstacles.navigationObstacles;
00372   res.localizationObstacles = obstacles.localizationObstacles;
00373 
00374   return true;
00375 }
00376 
00377 void FurnitureTracker::updateMessages(int index, std::string type, rail_ceiling::Obstacles *obstacles)
00378 {
00379   //get footprints
00380   FurnitureTransforms footprints;
00381   bool footprintsSet = false;
00382   for (unsigned int j = 0; j < footprintTransforms.size(); j ++)
00383   {
00384     if (footprintTransforms[j].name.compare(type) == 0)
00385     {
00386       footprints = footprintTransforms[j];
00387       footprintsSet = true;
00388       break;
00389     }
00390   }
00391   if (!footprintsSet)
00392   {
00393     ROS_INFO("Could not find footprint information for furniture of type %s.", type.c_str());
00394     return;
00395   }
00396 
00397   //fill footprint polygons
00398   fillFootprintInformation(index, footprints.localizationFootprint, obstacles, true);
00399   fillFootprintInformation(index, footprints.navigationFootprint, obstacles, false);
00400 }
00401 
00402 void FurnitureTracker::fillFootprintInformation(int index, vector<geometry_msgs::Polygon> footprints, rail_ceiling::Obstacles *obstacles, bool isLocalization)
00403 {
00404   if (!footprints.empty())
00405   {
00406     rail_ceiling::Obstacle obstacle;
00407     obstacle.polygons.clear();
00408     for (unsigned int j = 0; j < footprints.size(); j ++)
00409     {
00410       geometry_msgs::Polygon polygon;
00411       for (unsigned int k = 0; k < footprints[j].points.size(); k ++)
00412       {
00413         geometry_msgs::Point32 point;
00414         point.x = (float)(furniturePoses[index].x + footprints[j].points[k].x * cos(furniturePoses[index].theta)
00415             - footprints[j].points[k].y * sin(furniturePoses[index].theta));
00416         point.y = (float)(furniturePoses[index].y + footprints[j].points[k].x * sin(furniturePoses[index].theta)
00417             + footprints[j].points[k].y * cos(furniturePoses[index].theta));
00418         polygon.points.push_back(point);
00419       }
00420       obstacle.polygons.push_back(polygon);
00421     }
00422     obstacle.id = index;
00423     if (isLocalization)
00424       obstacles->localizationObstacles.push_back(obstacle);
00425     else
00426       obstacles->navigationObstacles.push_back(obstacle);
00427   }
00428 }
00429 
00430 bool FurnitureTracker::updated(geometry_msgs::Pose2D pose)
00431 {
00432   return !(pose.x == 0 && pose.y == 0 && pose.theta == 0);
00433 }
00434 
00435 int main(int argc, char **argv)
00436 {
00437   ros::init(argc, argv, "furniture_tracker");
00438 
00439   FurnitureTracker ft;
00440 
00441   ros::Rate loop_rate(10);
00442   while (ros::ok())
00443   {
00444     ft.publishFurniturePoses();
00445     ros::spinOnce();
00446     loop_rate.sleep();
00447   }
00448 }


rail_ceiling
Author(s): Russell Toris , David Kent
autogenerated on Sun Mar 6 2016 12:12:39