Package for reading from dinolite usb microscope

.github/workflows/ci.yaml

@@ -16,6 +16,7 @@ env:
     uwrt_mars_rover_drivetrain
     uwrt_mars_rover_drivetrain_description
     uwrt_mars_rover_drivetrain_hw
+    dinolite
   ROS_DISTRO: galactic
   IMAGE: "rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-desktop-latest"
 

README.md

@@ -12,17 +12,9 @@
 5. Update your system before continuing: `sudo apt update -y --no-install-recommends && sudo apt dist-upgrade -y`
 6. Install `rosdep`, the ROS dependency manager: `sudo apt install -y python3-rosdep`
 7. Download the repository's upstream dependencies: `vcs import --input uwrt_mars_rover/common_upstream_dependencies.repos`
-8. Update rosdep via `rosdep update --include-eol-distros`, including end-of-life ROS2 distros (we use Galactic).
-9. Navigate back to the root of your workspace, and install all dependencies for your ROS packages: `rosdep install --from-paths src -y --ignore-src`.
+8. Navigate back to the root of your workspace, and install all dependencies for your ROS packages: `RUN rosdep install --from-paths src -y --ignore-src`
 
-You can re-navigate to the root of your workspace at any time and rerun #8 and #9 to update your ROS packages' dependencies.
-For clarification, the repository should live in `~/ros2_ws/src/uwrt_mars_rover`.
-
-### Building and Installing the Repository
-1. Run `source /opt/ros/galactic/setup.bash`
-2. Navigate to the root of your ROS2 workspace (e.g: `cd ~/ros2_ws`)
-3. Build everything in the workspace: `colcon build`
-4. Install the built files: `. install/setup.bash`
+You can re-navigate to the root of your workspace at any time and rerun #8 to update your ROS packages' dependencies.
 
 
 ## Adding Dependencies

uwrt_mars_rover_science/dinolite/.gitignore

@@ -0,0 +1 @@
+settings.json

uwrt_mars_rover_science/dinolite/CMakeLists.txt

@@ -0,0 +1,112 @@
+cmake_minimum_required(VERSION 3.8)
+project(dinolite)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Try for OpenCV 4.X, but settle for whatever is installed
+find_package(OpenCV 4 QUIET)
+if (NOT OpenCV_FOUND)
+  find_package(OpenCV REQUIRED)
+endif ()
+message(STATUS "Found OpenCV version ${OpenCV_VERSION}")
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(camera_calibration_parsers REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+# Create ament index resource which references the libraries in the binary dir
+set(node_plugins "")
+
+#=============
+# OpenCV camera node
+#=============
+
+add_library(
+  cam_node SHARED
+  src/cam_node.cpp
+)
+target_compile_definitions(
+  cam_node
+  PRIVATE "COMPOSITION_BUILDING_DLL"
+)
+ament_target_dependencies(cam_node
+  camera_calibration_parsers
+  class_loader
+  OpenCV
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+)
+rclcpp_components_register_nodes(cam_node "dinolite::CamNode")
+set(node_plugins "${node_plugins}dinolite::CamNode;$<TARGET_FILE:cam_node>\n")
+
+#=============
+# Test subscriber node
+#=============
+
+# add_library(
+#   subscriber_node SHARED
+#   src/subscriber_node.cpp
+# )
+# target_compile_definitions(
+#   subscriber_node
+#   PRIVATE "COMPOSITION_BUILDING_DLL"
+# )
+# ament_target_dependencies( subscriber_node
+#   class_loader
+#   rclcpp
+#   rclcpp_components
+#   sensor_msgs
+# )
+# rclcpp_components_register_nodes(subscriber_node "dinolite::ImageSubscriberNode")
+# set(node_plugins "${node_plugins}dinolite::ImageSubscriberNode;$<TARGET_FILE:subscriber_node>\n")
+
+# Install nodes
+install(
+  TARGETS cam_node
+  EXPORT export_cam_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# install(
+#   TARGETS subscriber_node
+#   EXPORT export_subscriber_node
+#   ARCHIVE DESTINATION lib
+#   LIBRARY DESTINATION lib
+#   RUNTIME DESTINATION bin
+# )
+
+# Install various directories
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+
+ament_export_dependencies(class_loader)
+
+ament_export_include_directories(include)
+
+ament_export_targets(export_cam_node)
+# ament_export_targets(export_cam_node export_subscriber_node)
+
+ament_export_libraries(gscam_node subscriber_node)
+ament_package()
+
+

uwrt_mars_rover_science/dinolite/config/info.ini

@@ -0,0 +1,29 @@
+# Dinolite microscope camera intrinsics
+
+[image]
+
+width
+640
+
+height
+480
+
+[dinolite]
+
+camera matrix
+0.00000 0.00000 0.00000
+0.00000 0.00000 0.00000
+0.00000 0.00000 0.00000
+
+distortion
+-0.41527 0.31874 -0.00197 0.00071 0.00000
+
+rectification
+1.00000 0.00000 0.00000
+0.00000 1.00000 0.00000
+0.00000 0.00000 1.00000
+
+projection
+0.00000 0.00000 0.00000 0.00000
+0.00000 0.00000 0.00000 0.00000
+0.00000 0.00000 0.00000 0.00000

uwrt_mars_rover_science/dinolite/include/dinolite/cam_node.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "dinolite/camera_context.hpp"
+
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+
+#include "opencv2/highgui/highgui.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+#include "camera_calibration_parsers/parse.hpp"
+
+namespace dinolite {
+
+class CamNode : public rclcpp::Node {
+
+public:
+    explicit CamNode(const rclcpp::NodeOptions &options);
+
+private:
+    void validate_parameters();
+    void frame();
+
+    void printer();
+
+    CameraContext cxt_;
+    std::shared_ptr<cv::VideoCapture> capture_;
+
+    sensor_msgs::msg::CameraInfo camera_info_msg_;
+
+    int publish_fps_;
+    rclcpp::Time next_stamp_;
+
+    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub_;
+
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+} // namespace dinolite

uwrt_mars_rover_science/dinolite/include/dinolite/camera_context.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+
+#include <cmath>
+#include <string>
+
+namespace dinolite {
+
+    struct CameraContext {
+        const bool file_ = false;
+        const int fps_ = 30;
+        const std::string filename_ = "";
+        const int index_ = 0; // 0 is default index of webcam
+        const int width_ = 640;
+        const int height_ = 480;
+        const std::string camera_info_path_ = "install/dinolite/share/dinolite/config/info.ini";
+        const std::string camera_frame_id_ = "camera_frame";
+    };
+
+}

uwrt_mars_rover_science/dinolite/launch/dinolite_launch.py

@@ -0,0 +1 @@
+print("whats good")

uwrt_mars_rover_science/dinolite/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dinolite</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">edw</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>image_transport</depend>
+  <depend>cv_bridge</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>opencv2</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

uwrt_mars_rover_science/dinolite/scripts/webcam_test.py

@@ -0,0 +1,19 @@
+"""Script for testing webcam connection to opencv."""
+
+import cv2
+
+# 0 is default index of webcam
+vid = cv2.VideoCapture(0)
+
+while(True):
+    # read frame
+    ret, frame = vid.read()
+    # display frame
+    cv2.imshow('frame', frame)
+
+    # press q to quit
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+vid.release()
+cv2.destroyAllWindows()

uwrt_mars_rover_science/dinolite/src/cam_node.cpp

@@ -0,0 +1,106 @@
+#include "dinolite/cam_node.hpp"
+
+namespace dinolite {
+
+    std::string mat_type2encoding(int mat_type) {
+        switch (mat_type) {
+        case CV_8UC1:
+            return "mono8";
+        case CV_8UC3:
+            return "bgr8";
+        case CV_16SC1:
+            return "mono16";
+        case CV_8UC4:
+            return "rgba8";
+        default:
+            throw std::runtime_error("unsupported encoding type");
+        }
+    }
+
+    CamNode:: CamNode(const rclcpp::NodeOptions &options) : Node("dinolite_cam", options) {
+        RCLCPP_INFO(get_logger(), "hello wordln");
+        RCLCPP_INFO(get_logger(), "OpenCV version: %d", CV_VERSION_MAJOR);
+
+        capture_ = std::make_shared<cv::VideoCapture>(cxt_.index_);
+
+        if (!capture_->isOpened()) {
+            RCLCPP_ERROR(get_logger(), "cannot open device %d", cxt_.index_);
+            return;
+        }
+        if (cxt_.height_ > 0) {
+            capture_->set(cv::CAP_PROP_FRAME_HEIGHT, cxt_.height_);
+        }
+        if (cxt_.width_ > 0) {
+            capture_->set(cv::CAP_PROP_FRAME_WIDTH, cxt_.width_);
+        }
+        if (cxt_.fps_ > 0) {
+            capture_->set(cv::CAP_PROP_FPS, cxt_.fps_);
+        }
+
+        double width = capture_->get(cv::CAP_PROP_FRAME_WIDTH);
+        double height = capture_->get(cv::CAP_PROP_FRAME_HEIGHT);
+        double fps = capture_->get(cv::CAP_PROP_FPS);
+
+        RCLCPP_INFO(get_logger(), 
+            "device %d open, width %g, height %g, device fps %g",
+            cxt_.index_, width, height, fps
+        );
+
+        assert(!cxt_.camera_info_path_.empty()); // readCalibration will crash if file_name is ""
+
+        std::string camera_name;
+        if (camera_calibration_parsers::readCalibration(cxt_.camera_info_path_, camera_name, camera_info_msg_)) {
+            RCLCPP_INFO(get_logger(), "got camera info for '%s'", camera_name.c_str());
+            camera_info_msg_.header.frame_id = cxt_.camera_frame_id_;
+            camera_info_pub_ = create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
+        } else {
+            RCLCPP_ERROR(get_logger(), "cannot get camera info, will not publish");
+            camera_info_pub_ = nullptr;
+        }
+
+        image_pub_ = create_publisher<sensor_msgs::msg::Image>("image_raw", 10);
+
+        // send message
+        timer_ = create_wall_timer(
+            std::chrono::duration
+                <int, std::chrono::milliseconds::period>(1),
+                std::bind(&CamNode::frame, this)
+        );
+
+    }
+
+    void CamNode:: frame() {
+        cv::Mat frame;
+        while(rclcpp::ok()) {
+            if (!capture_->read(frame)) {
+                RCLCPP_INFO(get_logger(), "EOF, stop publishing");
+                break;
+            }
+
+            auto stamp = now();
+
+            sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
+
+            image_msg->header.stamp = stamp;
+            image_msg->header.frame_id = cxt_.camera_frame_id_;
+            image_msg->height = frame.rows;
+            image_msg->width = frame.cols;
+            image_msg->encoding = mat_type2encoding(frame.type());
+            image_msg->is_bigendian = false;
+            image_msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);
+            image_msg->data.assign(frame.datastart, frame.dataend);
+
+            // publish message
+            image_pub_->publish(std::move(image_msg));
+            if (camera_info_pub_) {
+                camera_info_msg_.header.stamp = stamp;
+                camera_info_pub_->publish(camera_info_msg_);
+            }
+        }
+    }
+
+} // namespace dinolite
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(dinolite::CamNode)