servers.cpp

Go to the documentation of this file.

#include <actionlib/server/action_server.h>
#include <boost/foreach.hpp>
#include <cv_bridge/cv_bridge.h>
#include <fstream>
#include <opencv-3.3.1-dev/opencv2/opencv.hpp>
#include <opencv-3.3.1-dev/opencv2/videoio.hpp>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rqt_bag_exporter/CloseBag.h>
#include <rqt_bag_exporter/EstimateVideoFps.h>
#include <rqt_bag_exporter/ExportToCSVAction.h>
#include <rqt_bag_exporter/ExportToVideoAction.h>
#include <rqt_bag_exporter/GetDuration.h>
#include <rqt_bag_exporter/ListTopics.h>
#include <rqt_bag_exporter/OpenBag.h>
#include <sensor_msgs/CompressedImage.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Duration.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int64.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
#include <std_msgs/Time.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/UInt64.h>
#include <std_msgs/UInt8.h>
#include <string>

std::shared_ptr<rosbag::Bag> bag;
std::string bag_file_name("");

void exportVideo(actionlib::ServerGoalHandle<rqt_bag_exporter::ExportToVideoAction> gh)
{
  gh.setAccepted();
  rqt_bag_exporter::ExportToVideoGoal goal(*gh.getGoal());
  rqt_bag_exporter::ExportToVideoResult result;
  rqt_bag_exporter::ExportToVideoFeedback feedback;

  if (!bag || bag->getSize() <= 0)
  {
    result.error = "No valid bag file is opened, call open_bag service to open one.";
    if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
      gh.setAborted(result);
    return;
  }

  std::string file_name(bag_file_name);
  int pos(file_name.find_last_of("."));
  file_name.erase(pos, std::string::npos);
  pos = file_name.find_last_of("/");
  file_name.erase(0, pos + 1);
  std::string topic_name(goal.topic_name);
  pos = topic_name.find_first_of("/");
  topic_name.erase(0, pos + 1);
  std::replace(topic_name.begin(), topic_name.end(), '/', '_');

  std::vector<std::string> topics;
  topics.push_back(goal.topic_name);
  rosbag::View view(*bag, rosbag::TopicQuery(topics));
  if (view.size() == 0)
  {
    result.error = "Topic '" + goal.topic_name + "' doesn't exist in this bag file";
    if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
      gh.setAborted(result);
    return;
  }

  feedback.progress_msg = "Starting...";
  feedback.progress_value = 0;
  gh.publishFeedback(feedback);

  cv::VideoWriter video;

  // Retrieve first image to get image info
  // Open video file
  const std::string video_name(goal.directory + "/" + file_name + "_" + topic_name + ".mkv");
  BOOST_FOREACH(rosbag::MessageInstance const first, view)
  {
    sensor_msgs::CompressedImage::ConstPtr s = first.instantiate<sensor_msgs::CompressedImage>();
    if (s == NULL)
      continue;
    try
    {
      cv::Mat image = cv::imdecode(s->data, 1); // Convert compressed image data to cv::Mat
      if (!video.open(video_name, cv::VideoWriter::fourcc('X', '2', '6', '4'), goal.fps, image.size()))
      {
        result.error = "Cannot open video file for writing";
        if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
          gh.setAborted(result);
        return;
      }
    }
    catch (cv_bridge::Exception& e)
    {
      result.error = "Could not convert image";
      if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
        gh.setAborted(result);
      return;
    }
    break;
  }

  ros::Time rosbag_begin(view.getBeginTime());
  BOOST_FOREACH(rosbag::MessageInstance const m, view)
  {
    if (gh.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
      return;

    if ((rosbag_begin.toSec() + goal.start_time) > m.getTime().toSec())
      continue;

    if ((rosbag_begin.toSec() + goal.end_time) < m.getTime().toSec())
      break;

    long unsigned real_time(m.getTime().sec - (rosbag_begin.sec + goal.start_time));
    long unsigned coeficient(100 / (goal.end_time - goal.start_time));
    long unsigned transmit_value(real_time * coeficient);
    feedback.progress_msg = "Exporting video topic " + goal.topic_name;
    feedback.progress_value = transmit_value;
    gh.publishFeedback(feedback);
    sensor_msgs::CompressedImage::ConstPtr s = m.instantiate<sensor_msgs::CompressedImage>();
    if (s == NULL)
      continue;
    try
    {
      cv::Mat image = cv::imdecode(s->data, 1);  // Convert compressed image data to cv::Mat
      video << image;
    }
    catch (cv_bridge::Exception& e)
    {
      result.error = "Could not convert image";
      gh.setAborted(result);
      return;
    }
  }

  feedback.progress_msg = "Writing video file";
  feedback.progress_value = 100;
  gh.publishFeedback(feedback);
  video.release();

  feedback.progress_msg = "Success";
  feedback.progress_value = 100;
  gh.publishFeedback(feedback);
  ros::Duration(0.5).sleep();
  gh.setSucceeded(result);
}

void exportVideoCancelCb(actionlib::ServerGoalHandle<rqt_bag_exporter::ExportToVideoAction> gh)
{
  if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE
      || gh.getGoalStatus().status == actionlib_msgs::GoalStatus::PREEMPTING)
    gh.setCanceled();
}

void exportCsv(actionlib::ServerGoalHandle<rqt_bag_exporter::ExportToCSVAction> gh)
{
  gh.setAccepted();
  rqt_bag_exporter::ExportToCSVGoal goal;
  rqt_bag_exporter::ExportToCSVResult result;
  rqt_bag_exporter::ExportToCSVFeedback feedback;
  goal = *gh.getGoal();

  if (!bag || bag->getSize() <= 0)
  {
    result.error = "No valid bag file is opened, call open_bag service to open one.";
    if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
      gh.setAborted(result);
    return;
  }

  std::vector<std::string> topics;
  topics.push_back(goal.topic_name);
  rosbag::View view(*bag, rosbag::TopicQuery(topics));
  if (view.size() == 0)
  {
    result.error = "Topic '" + goal.topic_name + "' doesn't exist in this bag file";
    if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
      gh.setAborted(result);
    return;
  }

  feedback.progress_msg = "Starting CSV export";
  feedback.progress_value = 0;
  gh.publishFeedback(feedback);

  std::ofstream csv_file;
  std::string file_name(bag_file_name);
  int pos = file_name.find_last_of(".");
  file_name.erase(pos, std::string::npos);
  pos = file_name.find_last_of("/");
  file_name.erase(0, pos + 1);
  std::string topic_name(goal.topic_name);
  pos = topic_name.find_first_of("/");
  topic_name.erase(0, pos + 1);
  std::replace(topic_name.begin(), topic_name.end(), '/', '_');
  std::string path_file(goal.directory + "/" + file_name + "_" + topic_name + ".csv");
  csv_file.open(path_file);
  if (!csv_file.is_open())
  {
    result.error = "Failed to open CSV file " + path_file;
    if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
      gh.setAborted(result);
    return;
  }
  csv_file << "Time, " << goal.topic_name << std::endl;
  ros::Time rosbag_begin(view.getBeginTime());
  BOOST_FOREACH(rosbag::MessageInstance const m, view)
  {
    if (gh.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
      return;
    if ((rosbag_begin.toSec() + goal.start_time) > m.getTime().toSec())
    {
      continue;
    }
    if ((rosbag_begin.toSec() + goal.end_time) < m.getTime().toSec())
    {
      break;
    }
    std::uint64_t real_time(m.getTime().sec - (rosbag_begin.sec + goal.start_time));
    std::uint64_t coeficient(100 / (goal.end_time - goal.start_time));
    std::uint64_t transmit_value(real_time * coeficient);
    std::string transmit_msg("Exporting topic " + goal.topic_name);
    feedback.progress_msg = transmit_msg;
    feedback.progress_value = transmit_value;
    gh.publishFeedback(feedback);
    double time(m.getTime().toSec() - (rosbag_begin.toSec() + goal.start_time));

    std_msgs::Float32::ConstPtr ft_32 = m.instantiate<std_msgs::Float32>();
    if (ft_32 != NULL)
      csv_file << time << ", " << ft_32->data << std::endl;

    std_msgs::Float64::ConstPtr ft_64 = m.instantiate<std_msgs::Float64>();
    if (ft_64 != NULL)
      csv_file << time << ", " << ft_64->data << std::endl;

    std_msgs::Int8::ConstPtr int_8 = m.instantiate<std_msgs::Int8>();
    if (int_8 != NULL)
      csv_file << time << ", " << int_8->data << std::endl;

    std_msgs::Int16::ConstPtr int_16 = m.instantiate<std_msgs::Int16>();
    if (int_16 != NULL)
      csv_file << time << ", " << int_16->data << std::endl;

    std_msgs::Int32::ConstPtr int_32 = m.instantiate<std_msgs::Int32>();
    if (int_32 != NULL)
      csv_file << time << ", " << int_32->data << std::endl;

    std_msgs::Int64::ConstPtr int_64 = m.instantiate<std_msgs::Int64>();
    if (int_64 != NULL)
      csv_file << time << ", " << int_64->data << std::endl;

    std_msgs::UInt8::ConstPtr uint_8 = m.instantiate<std_msgs::UInt8>();
    if (uint_8 != NULL)
      csv_file << time << ", " << uint_8->data << std::endl;

    std_msgs::UInt16::ConstPtr uint_16 = m.instantiate<std_msgs::UInt16>();
    if (uint_16 != NULL)
      csv_file << time << ", " << uint_16->data << std::endl;

    std_msgs::UInt32::ConstPtr uint_32 = m.instantiate<std_msgs::UInt32>();
    if (uint_32 != NULL)
      csv_file << time << ", " << uint_32->data << std::endl;

    std_msgs::UInt64::ConstPtr uint_64 = m.instantiate<std_msgs::UInt64>();
    if (uint_64 != NULL)
      csv_file << time << ", " << uint_64->data << std::endl;

    std_msgs::Bool::ConstPtr Bool_ = m.instantiate<std_msgs::Bool>();
    if (Bool_ != NULL)
      csv_file << time << ", " << (Bool_->data ? "True" : "False") << std::endl;

    std_msgs::Duration::ConstPtr dur = m.instantiate<std_msgs::Duration>();
    if (dur != NULL)
      csv_file << time << ", " << dur->data << std::endl;

    std_msgs::String::ConstPtr str = m.instantiate<std_msgs::String>();
    if (str != NULL)
      csv_file << time << ", " << str->data << std::endl;

    std_msgs::Time::ConstPtr time_ = m.instantiate<std_msgs::Time>();
    if (time_ != NULL)
      csv_file << time << ", " << time_->data.toSec() << std::endl;
  }

  feedback.progress_msg = "Writing CSV file";
  feedback.progress_value = 100;
  gh.publishFeedback(feedback);

  csv_file.close();
  feedback.progress_msg = "Sucess";
  feedback.progress_value = 100;
  gh.publishFeedback(feedback);
  ros::Duration(0.25).sleep();

  gh.setSucceeded(result);
  return;
}

void exportCsvCancelCb(actionlib::ServerGoalHandle<rqt_bag_exporter::ExportToCSVAction> gh)
{
  if (gh.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE
      || gh.getGoalStatus().status == actionlib_msgs::GoalStatus::PREEMPTING)
    gh.setCanceled();
}

bool openBag(rqt_bag_exporter::OpenBag::Request &req,
             rqt_bag_exporter::OpenBag::Response &res)
{
  bag_file_name.clear();
  if (req.bag_file.find(".bag") == std::string::npos)
  {
    res.error = "Not a bag file";
    return true;
  }

  if (bag)
    bag->close();

  bag.reset(new rosbag::Bag);
  try
  {
    bag->open(req.bag_file, rosbag::bagmode::Read);
  }
  catch (rosbag::BagException &e)
  {
    res.error = e.what();
    return true;
  }

  bag_file_name = req.bag_file;
  return true;
}

bool closeBag(rqt_bag_exporter::CloseBag::Request &,
              rqt_bag_exporter::CloseBag::Response &)
{
  if (!bag || bag->getSize() != 0)
    return true;

  bag->close();
  return true;
}

bool listTopics(rqt_bag_exporter::ListTopics::Request &,
                rqt_bag_exporter::ListTopics::Response &res)
{
  if (!bag || bag->getSize() <= 0)
  {
    res.error = "No valid bag file is opened, call open_bag service to open one.";
    return true;
  }

  rosbag::View view(*bag);
  std::vector<const rosbag::ConnectionInfo *> connection_infos = view.getConnections();
  BOOST_FOREACH(const rosbag::ConnectionInfo *info, connection_infos)
  {
    res.topics_name.push_back(info->topic);
    res.topics_type.push_back(info->datatype);
  }
  return true;
}

bool estimateVideoFps(rqt_bag_exporter::EstimateVideoFps::Request &req,
                      rqt_bag_exporter::EstimateVideoFps::Response &res)
{
  if (!bag || bag->getSize() <= 0)
  {
    res.error = "No valid bag file is opened, call open_bag service to open one.";
    return true;
  }

  rosbag::View view(*bag, rosbag::TopicQuery(req.topic_name));
  res.fps = view.size() / (view.getEndTime().sec - view.getBeginTime().sec);
  return true;
}

bool getDuration(rqt_bag_exporter::GetDuration::Request &,
                 rqt_bag_exporter::GetDuration::Response &res)
{
  if (!bag || bag->getSize() <= 0)
  {
    res.error = "No valid bag file is opened, call open_bag service to open one.";
    return true;
  }

  rosbag::View view(*bag);
  res.duration = view.getEndTime().toSec() - view.getBeginTime().toSec();
  return true;
}

int main(int argc,
         char** argv)
{
  ros::init(argc, argv, "rqt_bag_exporter_servers");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(2);
  spinner.start();

  actionlib::ActionServer<rqt_bag_exporter::ExportToCSVAction> action_server_1(nh,
                                                                               "export_csv",
                                                                               &exportCsv,
                                                                               &exportCsvCancelCb,
                                                                               false);
  action_server_1.start();

  actionlib::ActionServer<rqt_bag_exporter::ExportToVideoAction> action_server_2(nh,
                                                                                 "export_video",
                                                                                 &exportVideo,
                                                                                 &exportVideoCancelCb,
                                                                                 false);
  action_server_2.start();

  ros::ServiceServer service_1 = nh.advertiseService("open_bag", openBag);
  ros::ServiceServer service_2 = nh.advertiseService("close_bag", closeBag);
  ros::ServiceServer service_3 = nh.advertiseService("list_topics", listTopics);
  ros::ServiceServer service_4 = nh.advertiseService("estimate_video_fps", estimateVideoFps);
  ros::ServiceServer service_5 = nh.advertiseService("get_duration", getDuration);

  ros::waitForShutdown();
  spinner.stop();
  return 0;
}