Analyst notebook query (#95)

* small fixes for running docker images

* starting work on pano coord estimation

* merging CameraModel and CameraProjection classes

* start query c++ tool

* adding topics in isaac names instead of hardcoded

* removing wifi plugin temporarily to allow multiple robots to be spawned

* changes to analyst docker build/run

* simplifying launch file

* bag read script changes

* updating jupyter notebooks files

* fix lint

analyst/CMakeLists.txt

@@ -0,0 +1,70 @@
+# Copyright (c) 2021, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
+# platform" software is 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.
+
+# Check to see if correct version of media has been downloaded into tree. We
+# dont do this if we are cross-compiling, as there is no need for it. We must
+# also make sure the media is copied in a native install to a simulator.
+
+cmake_minimum_required(VERSION 2.8.3)
+project(analyst_notebook)
+
+## Compile as C++14, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+## Find catkin macros and libraries
+SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake")
+find_package(catkin2 REQUIRED COMPONENTS
+  roscpp
+  rosbag
+  ff_msgs
+	camera
+	inspection
+	)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp
+    rosbag
+    ff_msgs
+	  camera
+	  inspection
+)
+
+
+###########
+## Build ##
+###########
+
+# Specify additional locations of header files
+# Your package locations should be listed before other locations
+include_directories(
+	include
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+## Declare a C++ executable: find_point_coordinate 
+add_executable(query_view_points tools/query_view_points.cc)
+add_dependencies(query_view_points ${catkin_EXPORTED_TARGETS})
+target_link_libraries(query_view_points
+gflags glog jsoncpp ${catkin_LIBRARIES})
+

analyst/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>analyst_notebook</name>
+  <version>0.0.0</version>
+  <description>The analyst_notebook package</description>
+  <license>
+    Apache License, Version 2.0
+  </license>
+  <author email="[email protected]">
+    ISAAC Flight Software
+  </author>
+  <maintainer email="[email protected]">
+    ISAAC Flight Software
+  </maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rosbag</build_depend>
+  <build_depend>ff_msgs</build_depend>
+  <build_depend>camera</build_depend>
+  <build_depend>inspection</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rosbag</build_export_depend>
+  <build_export_depend>ff_msgs</build_export_depend>
+  <build_export_depend>camera</build_export_depend>
+  <build_export_depend>inspection</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rosbag</exec_depend>
+  <exec_depend>ff_msgs</exec_depend>
+  <exec_depend>camera</exec_depend>
+  <exec_depend>inspection</exec_depend>
+</package>

analyst/tools/query_view_points.cc

@@ -0,0 +1,169 @@
+/* Copyright (c) 2021, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ *
+ * All rights reserved.
+ *
+ * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
+ * platform" software is 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.
+ */
+
+// Command line flags
+#include <gflags/gflags.h>
+#include <gflags/gflags_completions.h>
+
+#include <ros/ros.h>
+
+// Import messages
+#include <ff_msgs/EkfState.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+
+// Shared project includes
+#include <config_reader/config_reader.h>
+#include <camera/camera_params.h>
+#include <inspection/camera_projection.h>
+
+// json file reader lib
+#include <jsoncpp/json/allocator.h>
+#include <jsoncpp/json/json.h>
+#include <jsoncpp/json/reader.h>
+#include <jsoncpp/json/value.h>
+
+#include <cmath>
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <string>
+
+// Parameters
+DEFINE_string(camera, "sci_cam", "Camera name.");
+
+DEFINE_double(min_distance,         0.2, "Anomaly: minimum distance to target (m)");
+DEFINE_double(max_distance,         0.7, "Anomaly: maximum distance to target (m)");
+
+int main(int argc, char** argv) {
+  // Gather some data from the command
+  google::SetUsageMessage("Usage: rosrun inspection query_view_points <opts>");
+  google::SetVersionString("0.1.0");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  google::InitGoogleLogging(argv[0]);
+
+  // Check if an input list was provided
+  if (argc < 2) {
+    std::cerr << "Error: No input list provided." << std::endl;
+    return 1;
+  }
+
+  // Read transforms
+  config_reader::ConfigReader config;
+  config.AddFile("cameras.config");
+  config.AddFile("transforms.config");
+  if (!config.ReadFiles()) LOG(FATAL) << "Failed to read config files.";
+
+  // Read Transform
+  std::string transform_str;
+  Eigen::Affine3d transform_nav_to_cam;
+  transform_str = "nav_cam_to_" + FLAGS_camera + "_transform";
+  if (!msg_conversions::config_read_transform(&config, transform_str.c_str(), &transform_nav_to_cam)) {
+    LOG(FATAL) << "Unspecified transform: " << transform_str << " for robot: "
+               << getenv("ASTROBEE_ROBOT") << "\n";
+    return 1;
+  }
+  Eigen::Affine3d transform_body_to_nav;
+  transform_str = "nav_cam_transform";
+  if (!msg_conversions::config_read_transform(&config, transform_str.c_str(), &transform_body_to_nav)) {
+    LOG(FATAL) << "Unspecified transform: " << transform_str << " for robot: "
+               << getenv("ASTROBEE_ROBOT") << "\n";
+    return 1;
+  }
+
+  // Initialize camera view
+  Eigen::Affine3d transform_body_to_cam = transform_body_to_nav * transform_nav_to_cam;
+  geometry_msgs::Transform::ConstPtr msg_pointer(
+    new geometry_msgs::Transform(msg_conversions::eigen_transform_to_ros_transform(transform_body_to_cam)));
+
+  camera::CameraParameters cam_params(&config, FLAGS_camera.c_str());
+  inspection::CameraView camera(cam_params, 2.0, 0.19, msg_pointer);
+
+
+
+
+
+  // Extract the input list from the command-line argument
+  std::string input_json = argv[1];
+
+  Json::Reader reader;
+  Json::Value json;
+  if (!reader.parse(input_json, json)) {
+      std::cerr << "Failed to parse the input JSON string." << std::endl;
+      return 1;
+  }
+  // Check if the root element is an array
+  if (!json.isArray()) {
+      std::cerr << "Input JSON does not represent an array." << std::endl;
+      return 1;
+  }
+  std::stringstream output_json;
+  // Process each list element
+  for (const auto& element : json) {
+    // Assuming each list element has two dictionaries
+    if (element.size() != 2 || !element[0].isObject() || !element[1].isArray()) {
+      std::cerr << "Invalid list element encountered." << std::endl;
+      continue;
+    }
+
+    // // Extract values from the first dictionary
+    // const Json::Value& dict1 = element[0];
+    // // Access the values using the desired keys
+    // int seq = dict1["seq"].asInt();
+    // int secs = dict1["stamp"]["secs"].asInt();
+    // int nsecs = dict1["stamp"]["nsecs"].asInt();
+    // std::string frame_id = dict1["frame_id"].asString();
+
+    // Extract values from the second dictionary (assuming it contains 7 float values)
+    const Json::Value& dict = element[1];
+    std::vector<float> floatValues;
+    for (const auto& value : dict) {
+      if (value.isConvertibleTo(Json::realValue))
+        floatValues.push_back(value.asFloat());
+    }
+
+    Eigen::Vector3d target;
+    target[0] = element[1][0].asFloat();
+    target[1] = element[1][1].asFloat();
+    target[2] = element[1][2].asFloat();
+    geometry_msgs::Pose ground_truth;
+    ground_truth.position.x = element[1][3].asFloat();
+    ground_truth.position.y = element[1][4].asFloat();
+    ground_truth.position.z = element[1][5].asFloat();
+    ground_truth.orientation.x = element[1][6].asFloat();
+    ground_truth.orientation.y = element[1][7].asFloat();
+    ground_truth.orientation.z = element[1][8].asFloat();
+    ground_truth.orientation.w = element[1][9].asFloat();
+
+    int x, y;
+
+    if (camera.GetCamXYFromPoint((msg_conversions::ros_pose_to_eigen_transform(ground_truth) *
+    transform_body_to_cam).inverse(), target, x, y)) {
+    output_json << element;
+    }
+  }
+
+  // Print the output JSON
+  std::cout << output_json.str() << std::endl;
+
+  // Finish commandline flags
+  google::ShutDownCommandLineFlags();
+  // Make for great success
+  return 0;
+}

analyst/workspace/1_import_bagfiles.ipynb

@@ -17,11 +17,19 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {
     "tags": []
    },
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Connected to database!\n"
+     ]
+    }
+   ],
    "source": [
     "from pyArango.connection import *\n",
     "\n",