00001
00010 #include <rail_ceiling/furniture_tracker.h>
00011 #include <fstream>
00012
00013 using namespace std;
00014
00015 FurnitureTracker::FurnitureTracker()
00016 {
00017
00018 ros::NodeHandle private_nh("~");
00019
00020
00021 int numMarkerTopics;
00022 private_nh.param("num_marker_topics", numMarkerTopics, 5);
00023 private_nh.param("read_initial_poses", readInitialPoses, false);
00024
00025
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
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
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
00063 f.id = id;
00064 id ++;
00065 f.type = markerConfig[i]["type"].as<string>();
00066
00067
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
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
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
00111 f.id = id;
00112 id ++;
00113 markerConfig[i]["type"] >> f.type;
00114
00115
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
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
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
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
00255 for (unsigned int i = 0; i < msg->markers.size(); i ++)
00256 {
00257
00258 if (msg->markers[i].id >= markerList.size())
00259 continue;
00260
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
00297 if (index < furniturePoses.size())
00298 {
00299
00300 if (updated(furniturePoses[index]))
00301 {
00302
00303 if (updated(lastPublishedPoses[index]))
00304 {
00305
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
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
00323 ROS_INFO("Initial publish for %s (id: %d).", type.c_str(), index);
00324
00325
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
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
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 }