furniture_tracker.h

Go to the documentation of this file.

#ifndef FURNITURE_TRACKER_H_
#define FURNITURE_TRACKER_H_

#include <ros/ros.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/Pose2D.h>
#include <rail_ceiling/GetAllObstacles.h>
#include <rail_ceiling/Obstacles.h>
#include <ros/package.h>
#include <tf/transform_listener.h>
#include <yaml-cpp/yaml.h>

#define POSITION_THRESHOLD .03 //threshold at which to republish changes in furniture position
#define ANGULAR_THRESHOLD .087 //threshold at which to republish changes in furniture angle, ~5 degrees

typedef struct _Marker
{
  unsigned int id;
  unsigned int fid; //associated furniture id
  geometry_msgs::Pose2D pose;
} Marker;

typedef struct _Furniture
{
  unsigned int id;
  std::string type;
  std::vector<Marker> markers;
} Furniture;

typedef struct _FurnitureTransforms
{
  std::string name;
  std::vector<geometry_msgs::Polygon> localizationFootprint;
  std::vector<geometry_msgs::Polygon> navigationFootprint;
} FurnitureTransforms;

class FurnitureTracker
{
public:
  FurnitureTracker();

  void publishFurniturePoses();

private:
  ros::NodeHandle n;

  ros::Publisher furnitureLayerPub;
  ros::ServiceServer allPosesServer;
  std::vector<ros::Subscriber> markerSubscribers;

  tf::TransformListener tfListener;

  bool readInitialPoses;
  std::vector<Marker> markerList;
  std::vector<Furniture> furnitureList;
  std::vector<FurnitureTransforms> footprintTransforms;
  std::vector<geometry_msgs::Pose2D> furniturePoses;
  std::vector<geometry_msgs::Pose2D> lastPublishedPoses;

  void readConfigFiles(std::string markerConfigFile, std::string furnitureConfigFile);

  void markerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg);

  bool getAllPoses(rail_ceiling::GetAllObstacles::Request &req, rail_ceiling::GetAllObstacles::Response &res);

  void updateMessages(int index, std::string type, rail_ceiling::Obstacles *obstacles);

  void fillFootprintInformation(int index, std::vector<geometry_msgs::Polygon> footprints, rail_ceiling::Obstacles *obstacles, bool isLocalization);

  bool updated(geometry_msgs::Pose2D pose);
};

int main(int argc, char **argv);

#endif