trajectory_utils.cpp
Go to the documentation of this file.
1 #include <mutex>
2 #include <string>
3 #include <strings.h>
4 
5 #include <ram_msgs/AdditiveManufacturingTrajectory.h>
6 #include <ram_utils/BufferParams.h>
7 #include <ram_utils/ExportTrajectory.h>
8 #include <ram_utils/ImportTrajectory.h>
9 #include <ram_utils/UnmodifiedTrajectory.h>
10 #include <ros/ros.h>
11 #include <rosbag/bag.h>
12 #include <rosbag/buffer.h>
13 #include <rosbag/exceptions.h>
14 #include <rosbag/structures.h>
15 #include <rosbag/view.h>
16 #include <signal.h>
17 
19 
20 const unsigned msg_to_keep(10);
21 unsigned last_msg_published = 0;
23 
24 std::mutex msgs_mutex;
25 std::vector<ram_msgs::AdditiveManufacturingTrajectory> traj_msgs; // Buffer 1. Contain all msgs
26 
28 std::vector<ram_msgs::AdditiveManufacturingTrajectory> unmodified_traj_msgs; // Buffer 2. Contain just the unmodified msgs
29 
30 // On shutdown, write bag file containing last trajectories
31 void mySigintHandler(int)
32 {
33  std::lock_guard<std::mutex> lock(msgs_mutex);
34  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
35 
36  if ((last_msg_published + 1) < traj_msgs.size()) // erase msgs
37  {
38  traj_msgs.erase((traj_msgs.begin() + last_msg_published + 1), traj_msgs.end());
39  // Erase msgs in the second buffer
40  while (unmodified_traj_msgs.back().generated != traj_msgs.back().generated && !unmodified_traj_msgs.empty())
41  unmodified_traj_msgs.pop_back();
42  }
43 
44  buffer_bag.open("ram_buffer_trajectory.bag", rosbag::bagmode::Write);
45  for (auto msg : traj_msgs)
46  buffer_bag.write("ram/trajectory", ros::Time::now(), msg);
47  buffer_bag.close();
48 
49  buffer_bag.open("ram_buffer_unmodified_trajectory.bag", rosbag::bagmode::Write);
50  for (auto msg : unmodified_traj_msgs)
51  buffer_bag.write("ram/trajectory", ros::Time::now(), msg);
52  buffer_bag.close();
53 
54  ros::shutdown();
55 }
56 
58 {
59  if (ros::this_node::getName().compare(msg_event.getPublisherName()) == 0) // The publisher is himself
60  return;
61 
62  std::lock_guard<std::mutex> lock(msgs_mutex);
63  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
64 
65  if ((last_msg_published + 1) < traj_msgs.size()) // erase msgs
66  {
67  traj_msgs.erase((traj_msgs.begin() + last_msg_published + 1), traj_msgs.end());
68  // Erase msgs in the second buffer
69  while (unmodified_traj_msgs.back().generated != traj_msgs.back().generated && !unmodified_traj_msgs.empty())
70  unmodified_traj_msgs.pop_back();
71  }
72 
74  traj_msgs.push_back(*msg_event.getMessage());
75 
76  if (msg_event.getMessage()->generated == msg_event.getMessage()->modified) // the trajectory is not modified
77  unmodified_traj_msgs.push_back(*msg_event.getMessage());
78  // Always save the same numbers of elements
79  if (traj_msgs.size() > msg_to_keep)
80  {
81  traj_msgs.erase(traj_msgs.begin(), traj_msgs.end() - msg_to_keep);
82  last_msg_published = traj_msgs.size() - 1;
83  }
84  if (unmodified_traj_msgs.size() > msg_to_keep)
86 }
87 
88 bool trajectoryBufferCallback(ram_utils::BufferParams::Request &req,
89  ram_utils::BufferParams::Response &res)
90 {
91  std::lock_guard<std::mutex> lock(msgs_mutex);
92 
93  ram_msgs::AdditiveManufacturingTrajectory trajectory;
94  switch (req.button_id)
95  {
96  case 1:
97  // Back button
98  if (last_msg_published > 0)
100  else
101  {
102  res.error = "Start of the buffer";
103  return true;
104  }
105  break;
106  case 2:
107  // Forward button
108  if ((last_msg_published + 1) < traj_msgs.size())
110  else
111  {
112  res.error = "End of the buffer";
113  return true;
114  }
115  break;
116  }
117 
118  if (last_msg_published < traj_msgs.size())
119  {
120  // Trajectory is published
121  trajectory = traj_msgs[last_msg_published];
122  pub.publish(trajectory);
123  }
124 
125  res.error.clear();
126  return true;
127 }
128 
129 bool getUnmodifiedTrajectoryCallback(ram_utils::UnmodifiedTrajectory::Request &req,
130  ram_utils::UnmodifiedTrajectory::Response &res)
131 {
132  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
133  for (auto msg : unmodified_traj_msgs)
134  {
135  if (msg.generated == req.generated)
136  {
137  res.trajectory = msg;
138  return true;
139  }
140  }
141 
142  return false;
143 }
144 
145 std::string fileExtension(const std::string full_path)
146 {
147  size_t last_index = full_path.find_last_of("/");
148  std::string file_name = full_path.substr(last_index + 1, full_path.size());
149 
150  last_index = file_name.find_last_of("\\");
151  file_name = file_name.substr(last_index + 1, file_name.size());
152 
153  last_index = file_name.find_last_of(".");
154  if (last_index == std::string::npos)
155  return "";
156 
157  return file_name.substr(last_index + 1, file_name.size());
158 }
159 
160 bool exportTrajectoryCallback(ram_utils::ExportTrajectory::Request &req,
161  ram_utils::ExportTrajectory::Response &res)
162 {
163  std::lock_guard<std::mutex> lock(msgs_mutex);
164  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
165 
166  if (req.file_name.find("smb-share:") != std::string::npos)
167  {
168  res.error = "Cannot export file to network!\n" + req.file_name;
169  return true;
170  }
171 
172  std::string file_extension(fileExtension(req.file_name));
173  if (file_extension.compare("bag") != 0)
174  {
175  res.error = req.file_name + " is not a bag file";
176  return true;
177  }
178 
179  // Find current trajectory
180  if (last_msg_published >= traj_msgs.size())
181  {
182  res.error = "Current trajectory is empty";
183  return true;
184  }
185 
186  ram_msgs::AdditiveManufacturingTrajectory current_traj = traj_msgs[last_msg_published];
187  ram_msgs::AdditiveManufacturingTrajectory unmodified_current_traj = current_traj;
188 
189  if (current_traj.poses.size() == 0)
190  {
191  res.error = "Current trajectory is empty";
192  return true;
193  }
194 
195  // Find unmodified trajectory
196  bool trajectory_in_buffer = false;
197  for (auto msg : unmodified_traj_msgs)
198  {
199  if (current_traj.generated != msg.generated)
200  continue;
201  // Trajectories are equals
202  trajectory_in_buffer = true;
203  if (current_traj.modified != msg.modified)
204  {
205  unmodified_current_traj = msg;
206  break;
207  }
208  }
209 
210  if (!trajectory_in_buffer)
211  {
212  res.error = "Corresponding unmodified trajectory does not exist in the buffer";
213  return true;
214  }
215 
216  // Save trajectory
217  rosbag::Bag export_trajectoy_bag;
218  try
219  {
220  export_trajectoy_bag.open(req.file_name, rosbag::bagmode::Write);
221  export_trajectoy_bag.write("ram/trajectory", ros::Time::now(), current_traj);
222  export_trajectoy_bag.write("ram/trajectory", ros::Time::now(), unmodified_current_traj);
223  export_trajectoy_bag.close();
224  }
225  catch (rosbag::BagException &e)
226  {
227  export_trajectoy_bag.close();
228  ROS_ERROR_STREAM(e.what());
229  res.error = e.what();
230  return true;
231  }
232 
233  return true;
234 }
235 
236 bool importTrajectoryCallback(ram_utils::ImportTrajectory::Request &req,
237  ram_utils::ImportTrajectory::Response &res)
238 {
239  std::lock_guard<std::mutex> lock(msgs_mutex);
240  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
241 
242  std::string file_extension(fileExtension(req.file_name));
243  if (file_extension.compare("bag") != 0)
244  {
245  res.error = req.file_name + " is not a bag file";
246  return true;
247  }
248 
249  rosbag::Bag import_trajectoy_bag;
250 
251  std::vector<ram_msgs::AdditiveManufacturingTrajectory> msgs_from_file;
252  try
253  {
254  import_trajectoy_bag.open(req.file_name, rosbag::bagmode::Read);
255  std::vector<std::string> topics;
256  topics.push_back(std::string("ram/trajectory"));
257  rosbag::View view(import_trajectoy_bag, rosbag::TopicQuery(topics));
258 
259  for (auto m : view)
260  {
261  ram_msgs::AdditiveManufacturingTrajectory::ConstPtr s =
262  m.instantiate<ram_msgs::AdditiveManufacturingTrajectory>();
263  if (s != NULL)
264  msgs_from_file.push_back(*s);
265  }
266 
267  import_trajectoy_bag.close();
268  }
269  catch (rosbag::BagException &e)
270  {
271  import_trajectoy_bag.close();
272  ROS_ERROR_STREAM(e.what());
273  res.error = e.what();
274  return true;
275  }
276 
277  if (msgs_from_file.size() != 2)
278  {
279  res.error = "Unable to import trajectory";
280  return true;
281  }
282 
283  // Save trajectories
284  ram_msgs::AdditiveManufacturingTrajectory trajectory = msgs_from_file[0];
285  ram_msgs::AdditiveManufacturingTrajectory unmodified_trajectory = msgs_from_file[1];
286 
287  if (trajectory.poses.size() == 0)
288  {
289  res.error = "Trajectory is empty";
290  return true;
291  }
292 
293  // Save unmodified_trajectory
294  for (unsigned i(0); i < unmodified_traj_msgs.size(); ++i)
295  {
296  if (unmodified_trajectory.generated == unmodified_traj_msgs[i].generated)
297  {
298  unmodified_traj_msgs.erase(unmodified_traj_msgs.begin() + i); // avoid duplicate duplicate in unmodified buffer
299  break;
300  }
301  }
302  // Save trajectories
303  unmodified_traj_msgs.push_back(unmodified_trajectory);
304  traj_msgs.push_back(trajectory);
305  last_msg_published = traj_msgs.size() - 1;
306 
307  // Publish trajectory
308  pub.publish(trajectory);
309  return true;
310 }
311 
312 int main(int argc,
313  char **argv)
314 {
315  ros::init(argc, argv, "ram_utils", ros::init_options::NoSigintHandler);
316  signal(SIGINT, mySigintHandler);
318 
319  bool error_in_first_bag = false;
320  bool error_in_second_bag = false;
321 
322  // Read the bag file (first buffer)
323  try
324  {
325  buffer_bag.open("ram_buffer_trajectory.bag", rosbag::bagmode::Read);
326  std::vector<std::string> topics;
327  topics.push_back(std::string("ram/trajectory"));
328  rosbag::View view(buffer_bag, rosbag::TopicQuery(topics));
329  std::lock_guard<std::mutex> lock(msgs_mutex);
330 
331  for (auto m : view)
332  {
333  ram_msgs::AdditiveManufacturingTrajectory::ConstPtr s =
334  m.instantiate<ram_msgs::AdditiveManufacturingTrajectory>();
335  if (s != NULL)
336  traj_msgs.push_back(*s);
337  }
338 
339  last_msg_published = traj_msgs.size(); // There are not published trajectories
340  buffer_bag.close();
341  }
342  catch (rosbag::BagException &e)
343  {
344  buffer_bag.close();
345  ROS_WARN_STREAM(e.what());
346 
347  std::string file(getenv("HOME"));
348  file += "/.ros/ram_buffer_trajectory.bag";
349 
350  if (std::remove(file.c_str()) == 0)
351  ROS_WARN_STREAM(file << " was removed because corrupted");
352 
353  error_in_first_bag = true;
354  }
355 
356  // Read the bag file (second buffer)
357  try
358  {
359  buffer_bag.open("ram_buffer_unmodified_trajectory.bag", rosbag::bagmode::Read);
360  std::vector<std::string> topics;
361  topics.push_back(std::string("ram/trajectory"));
362  rosbag::View view(buffer_bag, rosbag::TopicQuery(topics));
363  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
364 
365  for (auto m : view)
366  {
367  ram_msgs::AdditiveManufacturingTrajectory::ConstPtr s =
368  m.instantiate<ram_msgs::AdditiveManufacturingTrajectory>();
369  if (s != NULL)
370  unmodified_traj_msgs.push_back(*s);
371  }
372  buffer_bag.close();
373  }
374  catch (rosbag::BagException &e)
375  {
376  buffer_bag.close();
377  ROS_WARN_STREAM(e.what());
378 
379  std::string file(getenv("HOME"));
380  file += "/.ros/ram_buffer_unmodified_trajectory.bag";
381 
382  if (std::remove(file.c_str()) == 0)
383  ROS_WARN_STREAM(file << " was removed because corrupted");
384 
385  error_in_second_bag = true;
386  }
387 
388  if (error_in_first_bag || error_in_second_bag)
389  {
390  traj_msgs.clear();
391  unmodified_traj_msgs.clear();
392  last_msg_published = 0;
393  }
394  // Publish on "ram/trajectory"
395  pub = nh.advertise<ram_msgs::AdditiveManufacturingTrajectory>("ram/trajectory", msg_to_keep, true);
396 
397  // Subscribe on "ram/trajectory"
398  ros::Subscriber sub = nh.subscribe("ram/trajectory", msg_to_keep, saveTrajectoryCallback);
399  ros::ServiceServer service_1 = nh.advertiseService("ram/buffer/get_trajectory", trajectoryBufferCallback);
400  ros::ServiceServer service_2 = nh.advertiseService("ram/buffer/get_unmodified_trajectory",
402  ros::ServiceServer service_3 = nh.advertiseService("ram/buffer/export_trajectory", exportTrajectoryCallback);
403  ros::ServiceServer service_4 = nh.advertiseService("ram/buffer/import_trajectory", importTrajectoryCallback);
404 
406  spinner.start();
408 
409  return 0;
410 }
const unsigned msg_to_keep(10)
std::mutex msgs_mutex
void publish(const boost::shared_ptr< M > &message) const
void open(std::string const &filename, uint32_t mode=bagmode::Read)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool exportTrajectoryCallback(ram_utils::ExportTrajectory::Request &req, ram_utils::ExportTrajectory::Response &res)
unsigned last_msg_published
XmlRpcServer s
std::mutex unmodified_msgs_mutex
std::unique_ptr< ros::NodeHandle > nh
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
ram_msgs::AdditiveManufacturingTrajectory trajectory
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void close()
std::vector< ram_msgs::AdditiveManufacturingTrajectory > traj_msgs
void spinner()
const std::string & getPublisherName() const
bool trajectoryBufferCallback(ram_utils::BufferParams::Request &req, ram_utils::BufferParams::Response &res)
std::vector< ram_msgs::AdditiveManufacturingTrajectory > unmodified_traj_msgs
ros::Publisher pub
rosbag::Bag buffer_bag
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
int main(int argc, char **argv)
bool getUnmodifiedTrajectoryCallback(ram_utils::UnmodifiedTrajectory::Request &req, ram_utils::UnmodifiedTrajectory::Response &res)
bool importTrajectoryCallback(ram_utils::ImportTrajectory::Request &req, ram_utils::ImportTrajectory::Response &res)
void saveTrajectoryCallback(const ros::MessageEvent< ram_msgs::AdditiveManufacturingTrajectory > &msg_event)
static Time now()
ROSCPP_DECL void shutdown()
#define ROS_ERROR_STREAM(args)
void mySigintHandler(int)
std::string fileExtension(const std::string full_path)
void write(std::string const &topic, ros::MessageEvent< T > const &event)
boost::shared_ptr< M > getMessage() const
ROSCPP_DECL void waitForShutdown()


ram_utils
Author(s): Victor Lamoine - Institut Maupertuis
autogenerated on Mon Jun 10 2019 14:49:54