recordViewer.cpp
Go to the documentation of this file.
1 
18 //Pkg includes
19 #include <ros/ros.h>
20 
21 #include <asr_ism_visualizations/record_visualizer_rviz.hpp>
22 #include <asr_ism_visualizations/ObjectModelVisualizerRVIZ.hpp>
23 
24 //ISM includes
25 #include <ISM/common_type/Tracks.hpp>
26 #include <ISM/common_type/VoteSpecifier.hpp>
27 #include <ISM/utility/TableHelper.hpp>
28 #include <ISM/utility/Util.hpp>
29 
31 {
32  public:
33  RecordViewer(): nh_("~")
34  {
35  std::string db_filename;
36  std::string base_frame;
37  std::string scene_name;
38  std::string visualization_topic;
39 
40  getNodeParameters(db_filename, base_frame, scene_name, visualization_topic);
41  ISM::TableHelperPtr table_helper = ISM::TableHelperPtr(new ISM::TableHelper(db_filename));
42  if(!table_helper->recordDataExists())
43  {
44  ISM::printRed("The database \"" + db_filename + "\" doesn't contain any recordings!\n");
45  exit(0);
46  }
47 
48  std::vector<ISM::ObjectSetPtr> recordedObjectSets;
49 
50  ISM::RecordedPatternPtr recordedPattern = table_helper->getRecordedPattern(scene_name);
51  if (recordedPattern == nullptr)
52  {
53  ISM::printRed("No pattern in the database, with the name: " + scene_name + "\n");
54  exit(0);
55  }
56 
57  recordedObjectSets = recordedPattern->objectSets;
58  this->recorded_object_tracks_ = ISM::TracksPtr(new ISM::Tracks(recordedObjectSets));
60 
61  visualization_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>(visualization_topic, 10000);
62  record_visualizer_ = new VIZ::RecordVisualizerRVIZ(visualization_publisher_, base_frame, "", 0);
63  object_model_visualizer_ = new VIZ::ObjectModelVisualizerRVIZ(visualization_publisher_, base_frame, "", 0);
64 
65  ISM::printGreen("Generate and publish record marker!\n");
68  record_visualizer_->addVisualization(recorded_object_tracks_);
69 
71  }
72 
73  private:
74 
75  void getNodeParameters(std::string& db_filename,std::string& base_frame, std::string& scene_name, std::string& visualization_topic)
76  {
77  if (!nh_.getParam("dbfilename", db_filename)) {
78  db_filename = "";
79  }
80  ROS_INFO_STREAM("dbfilename: " << db_filename);
81 
82  if (!nh_.getParam("baseFrame", base_frame)) {
83  base_frame = "/map";
84  }
85  ROS_INFO_STREAM("baseFrame: " << base_frame);
86 
87  if (!nh_.getParam("visualization_topic", visualization_topic)) {
88  visualization_topic = "";
89  }
90  ROS_INFO_STREAM("visualization_topic: " << visualization_topic);
91 
92  if (!nh_.getParam("sceneName", scene_name)) {
93  scene_name = "";
94  }
95  ROS_INFO_STREAM("sceneName: " << scene_name);
96  }
97 
98 
100  {
101  for(auto& track : recorded_object_tracks_->tracks)
102  {
103  for (auto obj : track->objects)
104  {
105  if (obj)
106  {
107  recorded_objects_.push_back(obj);
108  break;
109  }
110  }
111  }
112  }
113 
114 
116  {
117  unsigned int previous_number = number_of_subscriber_;
119  if(number_of_subscriber_ > 0 && (number_of_subscriber_ > previous_number))
120  {
121  ISM::printGreen("Publish record marker!\n");
122  record_visualizer_->publishCollectedMarkers();
123  object_model_visualizer_->publishCollectedMarkers();
124  std::cout<<"\n";
125  }
126  }
127 
128 
129  private:
132 
133  ISM::TracksPtr recorded_object_tracks_;
134  std::vector<ISM::ObjectPtr> recorded_objects_;
135 
136  //Visualization stuff
138  VIZ::RecordVisualizerRVIZ* record_visualizer_;
139  VIZ::ObjectModelVisualizerRVIZ* object_model_visualizer_;
140  unsigned int number_of_subscriber_;
141 };
142 
143 int main(int argc, char** argv)
144 {
145  ros::init(argc, argv, "recordViewer");
146  RecordViewer* record_viewer = new RecordViewer();
147 
148  ros::spin();
149 
150  delete record_viewer;
151  return 0;
152 }
unsigned int number_of_subscriber_
ros::NodeHandle nh_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Timer timer_
ISM::TracksPtr recorded_object_tracks_
std::vector< ISM::ObjectPtr > recorded_objects_
ROSCPP_DECL void spin(Spinner &spinner)
void initRecordedObjects()
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void refreshVisualization(const ros::TimerEvent &e)
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
ros::Publisher visualization_publisher_
VIZ::RecordVisualizerRVIZ * record_visualizer_
bool getParam(const std::string &key, std::string &s) const
VIZ::ObjectModelVisualizerRVIZ * object_model_visualizer_
void getNodeParameters(std::string &db_filename, std::string &base_frame, std::string &scene_name, std::string &visualization_topic)


asr_ism
Author(s): Borella Jocelyn, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
autogenerated on Thu Jan 9 2020 07:20:58