Tools:add rosbag_to_record

modules/tools/rosbag_to_record/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_binary(
+    name = "rosbag_to_record",
+    srcs = ["rosbag_to_record.cc"],
+    linkopts = [
+       "-lprotobuf",
+    ],
+    deps = [
+        ":rosbag_to_record_lib",
+        "@ros//:ros_common",
+    ],
+)
+
+cc_library(
+    name = "rosbag_to_record_lib",
+    srcs = [
+        "channel_info.cc",
+    ],
+    hdrs = [
+        "rosbag_to_record.h",
+        "channel_info.h",
+    ],
+    deps = [
+        "//framework:cybertron", 
+        "//modules/planning/proto:planning_proto",
+        "//modules/control/proto:control_proto",
+        "//modules/guardian/proto:guardian_proto",
+        "//modules/canbus/proto:canbus_proto",
+        "//modules/perception/proto:perception_proto",
+        "//modules/prediction/proto:prediction_proto",
+        "//modules/localization/proto:localization_proto",
+        "//modules/localization/proto:imu_proto",
+        "//modules/localization/proto:gps_proto",
+        "//modules/common/proto:drive_event_proto",
+    ],
+)
+
+cpplint()

modules/tools/rosbag_to_record/channel_info.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/tools/rosbag_to_record/channel_info.h"
+
+namespace apollo {
+namespace tools {
+
+static const std::string DUMMY_STRING = "";
+
+ChannelInfo::ChannelInfo() {
+  InitChannels();
+}
+
+ChannelInfo::~ChannelInfo() {
+  channel_msg_type_.clear();
+  channel_proto_desc_.clear();
+}
+
+const std::string& ChannelInfo::GetMessageType(
+    const std::string& channel_name) {
+  auto search = channel_msg_type_.find(channel_name);
+  if (search != channel_msg_type_.end()) {
+    return search->second;
+  }
+  return DUMMY_STRING;
+}
+
+const std::string& ChannelInfo::GetProtoDesc(const std::string& channel_name) {
+  auto search = channel_proto_desc_.find(channel_name);
+  if (search != channel_proto_desc_.end()) {
+    return search->second;
+  }
+  return DUMMY_STRING;
+}
+
+const std::vector<std::string>& ChannelInfo::GetSupportChannels() {
+  return support_channels_;
+}
+
+void ChannelInfo::InitChannels() {
+  InitChannelInfo<apollo::perception::PerceptionObstacles>(
+      "/apollo/perception/obstacles", "apollo.perception.PerceptionObstacles");
+  InitChannelInfo<apollo::planning::ADCTrajectory>(
+      "/apollo/planning", "apollo.planning.ADCTrajectory");
+  InitChannelInfo<apollo::prediction::PredictionObstacles>(
+      "/apollo/prediction", "apollo.prediction.PredictionObstacles");
+  InitChannelInfo<apollo::canbus::Chassis>(
+      "/apollo/canbus/chassis", "apollo.canbus.Chassis");
+  InitChannelInfo<apollo::control::ControlCommand>(
+      "/apollo/control", "apollo.control.ControlCommand");
+  InitChannelInfo<apollo::guardian::GuardianCommand>(
+      "/apollo/guardian", "apollo.guardian.GuardianCommand");
+  InitChannelInfo<apollo::localization::LocalizationEstimate>(
+      "/apollo/localization/pose", "apollo.localization.LocalizationEstimate");
+  InitChannelInfo<apollo::perception::TrafficLightDetection>(
+      "/apollo/perception/traffic_light", "apollo.perception.TrafficLightDetection");
+  InitChannelInfo<apollo::common::DriveEvent>(
+      "/apollo/drive_event", "apollo.common.DriveEvent");
+  InitChannelInfo<apollo::localization::CorrectedImu>(
+      "/apollo/sensor/gnss/corrected_imu", "apollo.localization.CorrectedImu");
+  InitChannelInfo<apollo::localization::Gps>(
+      "/apollo/sensor/gnss/odometry", "apollo.localization.Gps");
+}
+
+}  // namespace tools
+}  // namespace apollo

modules/tools/rosbag_to_record/channel_info.h

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef MODULES_TOOLS_ROSBAG_TO_RECORD_CHANNEL_INFO_H_
+#define MODULES_TOOLS_ROSBAG_TO_RECORD_CHANNEL_INFO_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_map>
+
+#include "cybertron/message/message_traits.h"
+#include "cybertron/message/protobuf_factory.h"
+#include "cybertron/common/log.h"
+
+#include "modules/planning/proto/planning.pb.h"
+#include "modules/prediction/proto/prediction_obstacle.pb.h"
+#include "modules/perception/proto/perception_obstacle.pb.h"
+#include "modules/perception/proto/traffic_light_detection.pb.h"
+#include "modules/canbus/proto/chassis.pb.h"
+#include "modules/control/proto/control_cmd.pb.h"
+#include "modules/guardian/proto/guardian.pb.h"
+#include "modules/localization/proto/localization.pb.h"
+#include "modules/localization/proto/gps.pb.h"
+#include "modules/localization/proto/imu.pb.h"
+#include "modules/common/proto/drive_event.pb.h"
+
+namespace apollo {
+namespace tools {
+
+using namespace apollo::cybertron::message;
+using apollo::cybertron::message::ProtobufFactory;
+
+class ChannelInfo {
+ public:
+  using StrStrMap = std::unordered_map<std::string, std::string>;
+
+  virtual ~ChannelInfo();
+
+  static const std::shared_ptr<ChannelInfo>& Instance() {
+    static auto instance = std::shared_ptr<ChannelInfo>(new ChannelInfo());
+    return instance;
+  }
+
+  const std::string& GetMessageType(const std::string& channel_name);
+  const std::string& GetProtoDesc(const std::string& channel_name);
+  const std::vector<std::string>& GetSupportChannels();
+
+ private:
+  ChannelInfo();
+  ChannelInfo(const ChannelInfo&) = delete;
+  ChannelInfo& operator=(const ChannelInfo&) = delete;
+  
+  template<typename M>
+  void InitChannelInfo(const std::string& channel_name, const std::string& msg_type) {
+    channel_msg_type_[channel_name] = msg_type;
+    support_channels_.push_back(channel_name);
+    std::string proto_desc("");
+    M m;
+    ProtobufFactory::Instance()->GetDescriptorString(
+        msg_type, &proto_desc);
+    channel_proto_desc_[channel_name] = proto_desc;
+  }
+
+  void InitChannels();
+
+  StrStrMap channel_msg_type_;
+  StrStrMap channel_proto_desc_;
+  std::vector<std::string> support_channels_;
+};
+
+}  // namespace tools
+}  // namespace apollo
+
+#endif  // MODULES_TOOLS_ROSBAG_TO_RECORD_CHANNEL_INFO_H_

modules/tools/rosbag_to_record/rosbag_to_record.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "cybertron/proto/record.pb.h"
+
+#include "modules/tools/rosbag_to_record/rosbag_to_record.h"
+#include "modules/tools/rosbag_to_record/channel_info.h"
+
+#include "modules/planning/proto/planning.pb.h"
+#include "modules/prediction/proto/prediction_obstacle.pb.h"
+#include "modules/perception/proto/perception_obstacle.pb.h"
+#include "modules/perception/proto/traffic_light_detection.pb.h"
+#include "modules/canbus/proto/chassis.pb.h"
+#include "modules/control/proto/control_cmd.pb.h"
+#include "modules/guardian/proto/guardian.pb.h"
+#include "modules/localization/proto/localization.pb.h"
+
+using apollo::cybertron::proto::SingleMessage;
+using apollo::tools::ChannelInfo;
+
+void PrintUsage() {
+  std::cout << "Usage:\n"
+            << "  rosbag_to_record myfile.bag" << std::endl;
+}
+
+int main(int argc, char** argv) {
+  if (argc != 2) {
+    PrintUsage();
+    return -1;
+  }
+
+  const std::string rosbag_file_name = argv[1];
+  rosbag::Bag bag;
+  try {
+    bag.open(rosbag_file_name);
+  } catch (...) {
+    std::cerr << "Error: the input file is not a ros bag file." << std::endl;
+    return -1;
+  }
+
+  auto channel_info = ChannelInfo::Instance();
+  std::cout << "Info of ros bag file" << std::endl;
+  std::string command_line = "rosbag info " + rosbag_file_name;
+  system(command_line.c_str());
+
+  const std::string record_file_name =
+      rosbag_file_name.substr(0, rosbag_file_name.size() - 3) + "record";
+
+  auto record_writer =
+      std::make_shared<apollo::cybertron::record::RecordWriter>();
+  if (!record_writer->Open(record_file_name)) {
+    std::cerr << "Error: open file[" << record_file_name << "] failed.";
+  }
+
+  ros::Time::init();
+  ros::Time start_time = ros::Time::now();
+
+  rosbag::View view(bag, rosbag::TopicQuery(channel_info->GetSupportChannels()));
+
+  for (const rosbag::MessageInstance m : view) {
+    const std::string msg_type = m.getDataType();
+    const std::string channel_name = m.getTopic();
+    uint64_t nsec = m.getTime().toNSec();
+    
+//    auto msg_size = m.size();
+//
+//    std::vector<uint8_t> buffer;
+//    buffer.resize(msg_size);
+//    ros::serialization::IStream stream(buffer.data(), buffer.size());
+//    m.write(stream);
+//
+//    std::string str_msg;
+//    str_msg.reserve(msg_size);
+//    for (size_t i = 0; i < msg_size; ++i) {
+//      str_msg.push_back(buffer[i]);
+//    }
+    //TODO: find a way get all serialized str;
+    std::string serialized_str;
+    if (channel_name == "/apollo/perception/obstacles") {      
+      auto pb_msg = m.instantiate<apollo::perception::PerceptionObstacles>(); 
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/planning") {
+      auto pb_msg = m.instantiate<apollo::planning::ADCTrajectory>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/prediction") {
+      auto pb_msg = m.instantiate<apollo::prediction::PredictionObstacles>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/canbus/chassis") {
+      auto pb_msg = m.instantiate<apollo::canbus::Chassis>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/control") {
+      auto pb_msg = m.instantiate<apollo::control::ControlCommand>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/guardian") {
+      auto pb_msg = m.instantiate<apollo::guardian::GuardianCommand>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/localization/pose") {
+      auto pb_msg = m.instantiate<apollo::localization::LocalizationEstimate>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/perception/traffic_light") {
+      auto pb_msg = m.instantiate<apollo::perception::TrafficLightDetection>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/drive_event") {
+      auto pb_msg = m.instantiate<apollo::common::DriveEvent>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/sensor/gnss/corrected_imu") {
+      auto pb_msg = m.instantiate<apollo::localization::CorrectedImu>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else if (channel_name == "/apollo/sensor/gnss/odometry") {
+      auto pb_msg = m.instantiate<apollo::localization::Gps>();
+      pb_msg->SerializeToString(&serialized_str);
+    } else {
+      AWARN << "not support channel:" << channel_name;
+      continue;
+    }
+ 
+    auto desc = channel_info->GetProtoDesc(channel_name);
+    auto record_message_type = channel_info->GetMessageType(channel_name);
+    if (desc == "" || record_message_type == "") {
+      AWARN << "can not find desc or message tyep";
+    }
+    if (!record_writer->WriteChannel(channel_name, record_message_type, desc)) {
+      AERROR << "write channel info failed";
+    }
+    SingleMessage single_msg;
+    single_msg.set_channel_name(channel_name);
+    
+    single_msg.set_content(serialized_str);
+    single_msg.set_time(nsec);
+    if (!record_writer->WriteMessage(single_msg)) {
+      AERROR << "write single msg fail";
+    }
+  }
+
+  record_writer->Close();
+  record_writer = nullptr;
+  std::cout << "Info of record file" << std::endl;
+  command_line = "cyber_recorder info -f " + record_file_name;
+  system(command_line.c_str());
+
+  std::cout << "Convertion finished! Took " << ros::Time::now() - start_time
+            << " seconds in total." << std::endl;
+  return 0;
+}

modules/tools/rosbag_to_record/rosbag_to_record.h

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef MODULES_TOOLS_ROSBAG_TO_RECORD_ROSBAG_TO_RECORD_H_
+#define MODULES_TOOLS_ROSBAG_TO_RECORD_ROSBAG_TO_RECORD_H_
+
+#include <cstdint>
+#include <cstdlib>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+
+#include "cybertron/record/record_writer.h"
+
+#endif  // MODULES_TOOLS_ROSBAG_TO_RECORD_ROSBAG_TO_RECORD_H_