diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 50437d6c..57f9b035 100644 --- a/.github/workflows/ci.yaml +++ b/.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" diff --git a/README.md b/README.md index 4874355b..6f9c109c 100644 --- a/README.md +++ b/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 diff --git a/uwrt_mars_rover_science/dinolite/.gitignore b/uwrt_mars_rover_science/dinolite/.gitignore new file mode 100644 index 00000000..c3d3a20e --- /dev/null +++ b/uwrt_mars_rover_science/dinolite/.gitignore @@ -0,0 +1 @@ +settings.json \ No newline at end of file diff --git a/uwrt_mars_rover_science/dinolite/CMakeLists.txt b/uwrt_mars_rover_science/dinolite/CMakeLists.txt new file mode 100644 index 00000000..0053e35c --- /dev/null +++ b/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;$\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;$\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() + + diff --git a/uwrt_mars_rover_science/dinolite/config/info.ini b/uwrt_mars_rover_science/dinolite/config/info.ini new file mode 100644 index 00000000..57975999 --- /dev/null +++ b/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 diff --git a/uwrt_mars_rover_science/dinolite/include/dinolite/cam_node.hpp b/uwrt_mars_rover_science/dinolite/include/dinolite/cam_node.hpp new file mode 100644 index 00000000..df4ffe9b --- /dev/null +++ b/uwrt_mars_rover_science/dinolite/include/dinolite/cam_node.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include "dinolite/camera_context.hpp" + +#include +#include + +#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 capture_; + + sensor_msgs::msg::CameraInfo camera_info_msg_; + + int publish_fps_; + rclcpp::Time next_stamp_; + + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr camera_info_pub_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace dinolite \ No newline at end of file diff --git a/uwrt_mars_rover_science/dinolite/include/dinolite/camera_context.hpp b/uwrt_mars_rover_science/dinolite/include/dinolite/camera_context.hpp new file mode 100644 index 00000000..331c7017 --- /dev/null +++ b/uwrt_mars_rover_science/dinolite/include/dinolite/camera_context.hpp @@ -0,0 +1,20 @@ +#pragma once + + +#include +#include + +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"; + }; + +} \ No newline at end of file diff --git a/uwrt_mars_rover_science/dinolite/launch/dinolite_launch.py b/uwrt_mars_rover_science/dinolite/launch/dinolite_launch.py new file mode 100644 index 00000000..7f23b6d0 --- /dev/null +++ b/uwrt_mars_rover_science/dinolite/launch/dinolite_launch.py @@ -0,0 +1 @@ +print("whats good") diff --git a/uwrt_mars_rover_science/dinolite/package.xml b/uwrt_mars_rover_science/dinolite/package.xml new file mode 100644 index 00000000..46bbe0f9 --- /dev/null +++ b/uwrt_mars_rover_science/dinolite/package.xml @@ -0,0 +1,25 @@ + + + + dinolite + 0.0.0 + TODO: Package description + edw + TODO: License declaration + + ament_cmake + + rclcpp + image_transport + cv_bridge + sensor_msgs + std_msgs + opencv2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/uwrt_mars_rover_science/dinolite/scripts/webcam_test.py b/uwrt_mars_rover_science/dinolite/scripts/webcam_test.py new file mode 100644 index 00000000..8369b09d --- /dev/null +++ b/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() diff --git a/uwrt_mars_rover_science/dinolite/src/.cam_node.cpp.swp b/uwrt_mars_rover_science/dinolite/src/.cam_node.cpp.swp new file mode 100644 index 00000000..65ccdca6 Binary files /dev/null and b/uwrt_mars_rover_science/dinolite/src/.cam_node.cpp.swp differ diff --git a/uwrt_mars_rover_science/dinolite/src/cam_node.cpp b/uwrt_mars_rover_science/dinolite/src/cam_node.cpp new file mode 100644 index 00000000..c73c3041 --- /dev/null +++ b/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(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("camera_info", 10); + } else { + RCLCPP_ERROR(get_logger(), "cannot get camera info, will not publish"); + camera_info_pub_ = nullptr; + } + + image_pub_ = create_publisher("image_raw", 10); + + // send message + timer_ = create_wall_timer( + std::chrono::duration + (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(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) \ No newline at end of file diff --git a/windows_amd64_upstream_dependencies.repos b/windows_amd64_upstream_dependencies.repos index 353514d1..4a5efa3d 100644 --- a/windows_amd64_upstream_dependencies.repos +++ b/windows_amd64_upstream_dependencies.repos @@ -40,3 +40,7 @@ repositories: type: git url: https://github.com/ros-controls/control_msgs.git version: galactic-devel + ros-perception/vision_opencv: + type: git + url: https://github.com/ros-perception/vision_opencv.git + version: galactic