Drivers: add gnss parser cli tool

modules/drivers/gnss/test/BUILD.test → modules/drivers/gnss/test/BUILD

@@ -2,26 +2,13 @@ load("//tools:cpplint.bzl", "cpplint")
 
 package(default_visibility = ["//visibility:public"])
 
-
-
 cc_binary(
     name = "parser_cli",
     srcs = ["parser_cli.cc"],
     deps = [
         "//modules/drivers/gnss/stream:gnss_stream",
         "//external:gflags",
-        "//modules/common",
-        "@ros//:ros_common",
-    ],
-)
-
-cc_binary(
-    name = "test_monitor",
-    srcs = ["test_monitor.cc"],
-    deps = [
-        "//modules/drivers/gnss/stream:gnss_stream",
-        "//external:gflags",
-        "//framework:cybertron",
+        "//cybertron",
         "@ros//:ros_common",
     ],
 )

modules/drivers/gnss/test/parser_cli.cc

 /******************************************************************************
- * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ * 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.
@@ -23,14 +23,16 @@
 #include <iostream>
 #include <memory>
 
-#include "modules/common/adapters/adapter_manager.h"
-#include "modules/common/util/file.h"
-#include "modules/drivers/gnss/gnss_gflags.h"
+#include "cybertron/cybertron.h"
+#include "cybertron/record/record_reader.h"
+
 #include "modules/drivers/gnss/parser/data_parser.h"
 #include "modules/drivers/gnss/proto/config.pb.h"
 #include "modules/drivers/gnss/stream/stream.h"
+
 #include "ros/include/rosbag/bag.h"
 #include "ros/include/rosbag/view.h"
+#include "ros/include/std_msgs/String.h"
 
 namespace apollo {
 namespace drivers {
@@ -44,8 +46,7 @@ void ParseBin(const char* filename, DataParser* parser) {
   char b[BUFFER_SIZE];
   while (f) {
     f.read(b, BUFFER_SIZE);
-    std_msgs::StringPtr msg(new std_msgs::String);
-    msg->data.assign(reinterpret_cast<const char*>(b), f.gcount());
+    std::string msg(reinterpret_cast<const char*>(b), f.gcount());
     parser->ParseRawData(msg);
   }
 }
@@ -59,26 +60,42 @@ void ParseBag(const char* filename, DataParser* parser) {
   for (auto const m : view) {
     std_msgs::String::ConstPtr msg = m.instantiate<std_msgs::String>();
     if (msg != nullptr) {
-      parser->ParseRawData(msg);
+      parser->ParseRawData(msg->data);
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+  }
+}
+
+void ParseRecord(const char* filename, DataParser* parser) {
+  cybertron::record::RecordReader reader(filename);
+  cybertron::record::RecordMessage message;
+  while (reader.ReadMessage(&message)) {
+    if (message.channel_name == "/apollo/sensor/gnss/raw_data") {
+      apollo::drivers::gnss::RawData msg;
+      msg.ParseFromString(message.content);
+      parser->ParseRawData(msg.data());
     }
     std::this_thread::sleep_for(std::chrono::milliseconds(2));
   }
 }
 
-void Parse(const char* filename, const char* file_type) {
+void Parse(const char* filename, const char* file_type,
+           const std::shared_ptr<::apollo::cybertron::Node>& node) {
   std::string type = std::string(file_type);
   config::Config config;
-  if (!common::util::GetProtoFromFile(
+  if (!apollo::cybertron::common::GetProtoFromFile(
           std::string("/apollo/modules/drivers/gnss/conf/gnss_conf.pb.txt"),
           &config)) {
     std::cout << "Unable to load gnss conf file";
   }
-  DataParser* parser = new DataParser(config);
+  DataParser* parser = new DataParser(config, node);
   parser->Init();
   if (type == "bag") {
     ParseBag(filename, parser);
   } else if (type == "bin") {
     ParseBin(filename, parser);
+  } else if (type == "record") {
+    ParseRecord(filename, parser);
   } else {
     std::cout << "unknown file type";
   }
@@ -91,14 +108,12 @@ void Parse(const char* filename, const char* file_type) {
 
 int main(int argc, char** argv) {
   if (argc < 3) {
-    std::cout << "Usage: " << argv[0] << " filename [bag|bin]" << std::endl;
+    std::cout << "Usage: " << argv[0] << " filename [record|bin]" << std::endl;
     return 0;
   }
-
-  ros::init(argc, argv, "parser_cli");
-  ros::Time::init();
-  ros::Time::now();
-  apollo::common::adapter::AdapterManager::Init(FLAGS_adapter_config_filename);
-  ::apollo::drivers::gnss::Parse(argv[1], argv[2]);
+  ::apollo::cybertron::Init("parser_cli");
+  std::shared_ptr<::apollo::cybertron::Node> parser_node(
+      ::apollo::cybertron::CreateNode("parser_cli"));
+  ::apollo::drivers::gnss::Parse(argv[1], argv[2], parser_node);
   return 0;
 }

modules/drivers/gnss/test/test_monitor.cc

-/******************************************************************************
- * Copyright 2017 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 <ros/ros.h>
-#include <std_msgs/String.h>
-#include <cmath>
-#include <memory>
-
-#include "modules/drivers/gnss/proto/gnss_status.pb.h"
-#include "modules/drivers/gnss/proto/ins.pb.h"
-
-void ins_status_callback(
-    const apollo::drivers::gnss_status::InsStatus &ins_status) {
-  switch (ins_status.type()) {
-    case apollo::drivers::gnss_status::InsStatus::GOOD:
-      fprintf(stdout, "INS status is GOOD.\r\n");
-      break;
-    case apollo::drivers::gnss_status::InsStatus::CONVERGING:
-      fprintf(stdout, "INS status is CONVERGING.\r\n");
-      break;
-    case apollo::drivers::gnss_status::InsStatus::INVALID:
-    default:
-      fprintf(stdout, "INS status is INVALID.\r\n");
-      break;
-  }
-}
-
-void ins_stat_callback(const ::apollo::drivers::gnss::InsStat &ins_stat) {
-  // std::cout << "INS stat: " << ins_stat.DebugString() << std::endl;
-}
-
-void stream_status_callback(
-    const apollo::drivers::gnss_status::StreamStatus &stream_status) {
-  switch (stream_status.ins_stream_type()) {
-    case apollo::drivers::gnss_status::StreamStatus::CONNECTED:
-      fprintf(stdout, "INS stream is CONNECTED.\r\n");
-      break;
-    case apollo::drivers::gnss_status::StreamStatus::DISCONNECTED:
-      fprintf(stdout, "INS stream is DISCONNECTED.\r\n");
-      break;
-  }
-
-  switch (stream_status.rtk_stream_in_type()) {
-    case apollo::drivers::gnss_status::StreamStatus::CONNECTED:
-      fprintf(stdout, "rtk stream in is CONNECTED.\r\n");
-      break;
-    case apollo::drivers::gnss_status::StreamStatus::DISCONNECTED:
-      fprintf(stdout, "rtk stream in is DISCONNECTED.\r\n");
-      break;
-  }
-
-  switch (stream_status.rtk_stream_out_type()) {
-    case apollo::drivers::gnss_status::StreamStatus::CONNECTED:
-      fprintf(stdout, "rtk stream out CONNECTED.\r\n");
-      break;
-    case apollo::drivers::gnss_status::StreamStatus::DISCONNECTED:
-      fprintf(stdout, "rtk stream out DISCONNECTED.\r\n");
-      break;
-  }
-}
-
-void gnss_status_callback(
-    const apollo::drivers::gnss_status::GnssStatus &gnss_status) {
-  // std::cout << "GNSS status: " << gnss_status.DebugString() << std::endl;
-}
-
-int main(int argc, char *argv[]) {
-  ros::init(argc, argv, std::string("gnss_monitor_test"));
-
-  ros::NodeHandle nh;
-  ros::Subscriber ins_status_sub;
-  ros::Subscriber gnss_status_sub;
-  ros::Subscriber stream_status_sub;
-  ros::Subscriber ins_stat_sub;
-
-  ins_status_sub =
-      nh.subscribe("/apollo/sensor/gnss/ins_status", 16, ins_status_callback);
-  gnss_status_sub =
-      nh.subscribe("/apollo/sensor/gnss/gnss_status", 16, gnss_status_callback);
-  stream_status_sub = nh.subscribe("/apollo/sensor/gnss/stream_status", 16,
-                                   stream_status_callback);
-  ins_stat_sub =
-      nh.subscribe("/apollo/sensor/gnss/ins_stat", 16, ins_stat_callback);
-
-  ros::spin();
-
-  ROS_ERROR("Exit");
-  return 0;
-}