trajectory_buffer.cpp

Go to the documentation of this file.

#include <mutex>
#include <string>
#include <strings.h>

#include <ram_msgs/AdditiveManufacturingTrajectory.h>
#include <ram_trajectory_buffer/BufferParams.h>
#include <ram_trajectory_buffer/UnmodifiedTrajectory.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/buffer.h>
#include <rosbag/exceptions.h>
#include <rosbag/structures.h>
#include <rosbag/view.h>
#include <signal.h>

rosbag::Bag bag;
const unsigned msg_to_keep(10);
unsigned last_msg_published = 0;
ros::Publisher pub;

std::mutex msgs_mutex;
std::vector<ram_msgs::AdditiveManufacturingTrajectory> msgs; // Buffer 1. Contain all msgs

std::mutex unmodified_msgs_mutex;
std::vector<ram_msgs::AdditiveManufacturingTrajectory> unmodified_msgs; // Buffer 2. Contain just the unmodified msgs

// On shutdown, write bag file containing last trajectories
void mySigintHandler(int)
{
  std::lock_guard<std::mutex> lock(msgs_mutex);
  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);

  if ((last_msg_published + 1) < msgs.size()) // erase msgs
  {
    msgs.erase((msgs.begin() + last_msg_published + 1), msgs.end());
    // Erase msgs in the second buffer
    while (unmodified_msgs.back().generated != msgs.back().generated && !unmodified_msgs.empty())
      unmodified_msgs.pop_back();
  }

  bag.open("ram_buffer_trajectory.bag", rosbag::bagmode::Write);
  for (auto msg : msgs)
    bag.write("ram/trajectory", ros::Time::now(), msg);
  bag.close();

  bag.open("ram_buffer_unmodified_trajectory.bag", rosbag::bagmode::Write);
  for (auto msg : unmodified_msgs)
    bag.write("ram/trajectory", ros::Time::now(), msg);
  bag.close();

  ros::shutdown();
}

void saveTrajectoryCallback(const ros::MessageEvent<ram_msgs::AdditiveManufacturingTrajectory>& msg_event)
{
  if (ros::this_node::getName().compare(msg_event.getPublisherName()) == 0) // The publisher is himself
    return;

  std::lock_guard<std::mutex> lock(msgs_mutex);
  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);

  if ((last_msg_published + 1) < msgs.size()) // erase msgs
  {
    msgs.erase((msgs.begin() + last_msg_published + 1), msgs.end());
    // Erase msgs in the second buffer
    while (unmodified_msgs.back().generated != msgs.back().generated && !unmodified_msgs.empty())
      unmodified_msgs.pop_back();
  }

  last_msg_published = msgs.size();
  msgs.push_back(*msg_event.getMessage());

  if (msg_event.getMessage()->generated == msg_event.getMessage()->modified) // the trajectory is not modified
    unmodified_msgs.push_back(*msg_event.getMessage());
  // Always save the same numbers of elements
  if (msgs.size() > msg_to_keep)
  {
    msgs.erase(msgs.begin(), msgs.end() - msg_to_keep);
    last_msg_published = msgs.size() - 1;
  }
  if (unmodified_msgs.size() > msg_to_keep)
    unmodified_msgs.erase(unmodified_msgs.begin(), unmodified_msgs.end() - msg_to_keep);
}

bool trajectoryBufferCallback(ram_trajectory_buffer::BufferParams::Request &req,
                              ram_trajectory_buffer::BufferParams::Response &res)
{
  std::lock_guard<std::mutex> lock(msgs_mutex);

  ram_msgs::AdditiveManufacturingTrajectory trajectory;
  switch (req.button_id)
  {
    case 1:
      // Back button
      if (last_msg_published > 0)
        --last_msg_published;
      else
      {
        res.error = "Start of the buffer";
        return true;
      }
      break;
    case 2:
      // Forward button
      if ((last_msg_published + 1) < msgs.size())
        ++last_msg_published;
      else
      {
        res.error = "End of the buffer";
        return true;
      }
      break;
  }

  if (last_msg_published < msgs.size())
  {
    // Trajectory is published
    trajectory = msgs[last_msg_published];
    pub.publish(trajectory);
  }

  res.error.clear();
  return true;
}

bool getUnmodifiedTrajectoryCallback(ram_trajectory_buffer::UnmodifiedTrajectory::Request &req,
                                     ram_trajectory_buffer::UnmodifiedTrajectory::Response &res)
{
  std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);
  for (auto msg : unmodified_msgs)
  {
    if (msg.generated == req.generated)
    {
      res.trajectory = msg;
      return true;
    }
  }

  return false;
}

int main(int argc,
         char **argv)
{
  ros::init(argc, argv, "ram_trajectory_buffer", ros::init_options::NoSigintHandler);
  signal(SIGINT, mySigintHandler);
  ros::NodeHandle nh;

  bool error_in_first_bag = false;
  bool error_in_second_bag = false;

  // Read the bag file (first buffer)
  try
  {
    bag.open("ram_buffer_trajectory.bag", rosbag::bagmode::Read);
    std::vector<std::string> topics;
    topics.push_back(std::string("ram/trajectory"));
    rosbag::View view(bag, rosbag::TopicQuery(topics));
    std::lock_guard<std::mutex> lock(msgs_mutex);

    for (auto m : view)
    {
      ram_msgs::AdditiveManufacturingTrajectory::ConstPtr s =
          m.instantiate<ram_msgs::AdditiveManufacturingTrajectory>();
      if (s != NULL)
        msgs.push_back(*s);
    }

    last_msg_published = msgs.size(); // There are not published trajectories
    bag.close();
  }
  catch (rosbag::BagException &e)
  {
    bag.close();
    ROS_WARN_STREAM(e.what());

    std::string file(getenv("HOME"));
    file += "/.ros/ram_buffer_trajectory.bag";

    if (std::remove(file.c_str()) == 0)
      ROS_WARN_STREAM(file << " was removed because corrupted");

    error_in_first_bag = true;
  }

  // Read the bag file (second buffer)
  try
  {
    bag.open("ram_buffer_unmodified_trajectory.bag", rosbag::bagmode::Read);
    std::vector<std::string> topics;
    topics.push_back(std::string("ram/trajectory"));
    rosbag::View view(bag, rosbag::TopicQuery(topics));
    std::lock_guard<std::mutex> lock_1(unmodified_msgs_mutex);

    for (auto m : view)
    {
      ram_msgs::AdditiveManufacturingTrajectory::ConstPtr s =
          m.instantiate<ram_msgs::AdditiveManufacturingTrajectory>();
      if (s != NULL)
        unmodified_msgs.push_back(*s);
    }
    bag.close();
  }
  catch (rosbag::BagException &e)
  {
    bag.close();
    ROS_WARN_STREAM(e.what());

    std::string file(getenv("HOME"));
    file += "/.ros/ram_buffer_unmodified_trajectory.bag";

    if (std::remove(file.c_str()) == 0)
      ROS_WARN_STREAM(file << " was removed because corrupted");

    error_in_second_bag = true;
  }

  if (error_in_first_bag || error_in_second_bag)
  {
    msgs.clear();
    unmodified_msgs.clear();
    last_msg_published = 0;
  }
  // Publish on "ram/trajectory"
  pub = nh.advertise<ram_msgs::AdditiveManufacturingTrajectory>("ram/trajectory", msg_to_keep, true);

  // Subscribe on "ram/trajectory"
  ros::Subscriber sub = nh.subscribe("ram/trajectory", msg_to_keep, saveTrajectoryCallback);
  ros::ServiceServer service_1 = nh.advertiseService("ram/buffer/get_trajectory", trajectoryBufferCallback);
  ros::ServiceServer service_2 = nh.advertiseService("ram/buffer/get_unmodified_trajectory",
                                                     getUnmodifiedTrajectoryCallback);

  ros::AsyncSpinner spinner(0);
  spinner.start();
  ros::waitForShutdown();

  return 0;
}