annotation_collection.hpp

Go to the documentation of this file.

/*
 * annotation_collection.hpp
 *
 *  Created on: May 7, 2014
 *      Author: jorge
 */

#ifndef ANNOTATION_COLLECTION_HPP_
#define ANNOTATION_COLLECTION_HPP_

#include <unique_id/unique_id.h>

#include <ros/serialization.h>
#include <ros/serialized_message.h>

#include <visualization_msgs/Marker.h>

#include <world_canvas_msgs/Annotation.h>
#include <world_canvas_msgs/AnnotationData.h>

#include "world_canvas_client_cpp/filter_criteria.hpp"
#include "world_canvas_client_cpp/world_canvas_client.hpp"

namespace wcf
{

class AnnotationCollection : public WorldCanvasClient
{
protected:
  ros::NodeHandle nh;
  ros::Publisher marker_pub;
  ros::Publisher markers_pub;

  FilterCriteria filter;

  std::vector<world_canvas_msgs::Annotation>     annotations;
  std::vector<world_canvas_msgs::AnnotationData> annots_data;

  std::vector<world_canvas_msgs::Annotation> annots_to_delete;
  bool saved;

  bool saveDeletes();

  std::vector<UniqueIDmsg> getAnnotsDataIDs();

  inline bool endsWith(const std::string& a, const std::string& b)
  {
    if (a.size() < b.size())
      return false;
    return (a.substr(a.size() - b.size()) == b);
  }

public:
  AnnotationCollection(const std::string& world, const std::string& srv_namespace = "");

  AnnotationCollection(const FilterCriteria& criteria, const std::string& srv_namespace = "");

  virtual ~AnnotationCollection();

  bool filterBy(const FilterCriteria& criteria);

  bool load();

  bool loadData();

  bool save();

  bool isSaved() { return saved; }

  bool add(const world_canvas_msgs::Annotation& annotation,
           const world_canvas_msgs::AnnotationData& annot_data);

  bool update(const world_canvas_msgs::Annotation& annotation,
              const world_canvas_msgs::AnnotationData& annot_data);

  bool remove(const uuid_msgs::UniqueID& id);

  bool clearMarkers(const std::string& topic);

  bool publishMarkers(const std::string& topic, bool clear_existing = true);

  bool publishMarker(const std::string& topic, int marker_id,
                     const world_canvas_msgs::Annotation& ann, bool clear_existing = true);

  visualization_msgs::Marker makeMarker(int id, const world_canvas_msgs::Annotation& ann);

  visualization_msgs::Marker makeLabel(const visualization_msgs::Marker& marker);

  bool publish(const std::string& topic_name, const std::string& topic_type = "",
               bool by_server = false, bool as_list = false);

  bool hasAnnotation(const UniqueIDmsg& id);

  const world_canvas_msgs::Annotation& getAnnotation(const UniqueIDmsg& id);

  std::vector<world_canvas_msgs::Annotation> getAnnotations(const std::string& name);

  std::vector<UniqueIDmsg> getAnnotationIDs();

  const std::vector<world_canvas_msgs::Annotation>& getAnnotations() { return annotations; }

  const world_canvas_msgs::AnnotationData& getData(const world_canvas_msgs::Annotation& ann);

  template <typename T>
  unsigned int getData(std::vector<T>& data)
  {
    unsigned int count = 0;
    std::string type = ros::message_traits::DataType<T>::value();

    for (unsigned int i = 0; i < annots_data.size(); ++i)
    {
      // Check id this data corresponds to an annotation of the requested type. TODO: but we should
      // a list of tuples containing anns + data, so we don't need to relay on data.type
      if (annots_data[i].type != type)
        continue;

      // Deserialize the ROS message contained on data field; we must clone the serialized data
      // because SerializedMessage requires a boost::shared_array as input that will try to destroy
      // the underlying array once out of scope. Also we must skip 4 bytes on message_start pointer
      // because the first 4 bytes on the buffer contains the (already known) serialized data size.
      T object;
      uint32_t serial_size = annots_data[i].data.size();
      boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
      memcpy(buffer.get(), &annots_data[i].data[0], serial_size);
      ros::SerializedMessage sm(buffer, serial_size);
      sm.type_info = &typeid(object);
      sm.message_start += 4;
      try
      {
        ROS_DEBUG("Deserializing object '%s' of type '%s'",
                  unique_id::toHexString(annots_data[i].id).c_str(), type.c_str());
        ros::serialization::deserializeMessage(sm, object);
        data.push_back(object);
        count++;
      }
      catch (ros::serialization::StreamOverrunException& e)
      {
        ROS_ERROR("Deserialization failed on object %d: %s", i, e.what());
      }
    }
    return count;
  }

};

} // namespace wcf

#endif /* ANNOTATION_COLLECTION_HPP_ */