Added a camera_reader node that reads the raw camera data coming from /wrist_camera/image_raw topic

Author: JatinRoopak

sasc/.gitignore

@@ -0,0 +1 @@
+log/

sasc/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(OpenCV REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -30,5 +31,19 @@ endif()
 install(DIRECTORY worlds
   DESTINATION share/${PROJECT_NAME}
 )
+add_executable(camera_reader src/camera_reader.cpp)
+
+ament_target_dependencies(camera_reader
+  rclcpp
+  sensor_msgs
+  cv_bridge
+  image_transport
+)
+
+target_link_libraries(camera_reader ${OpenCV_LIBRARIES})
+
+install(TARGETS
+  camera_reader
+  DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

sasc/src/camera_reader.cpp

@@ -0,0 +1,53 @@
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "cv_bridge/cv_bridge.h"
+#include "opencv2/highgui/highgui.hpp"
+
+using namespace std;
+
+class CameraReader : public rclcpp::Node {
+    public:
+        CameraReader() : Node("camera_reader_node"){
+            subscription_ = this -> create_subscription<sensor_msgs::msg::Image>(
+                "/wrist_camera/image_raw",
+                10,
+                std::bind(&CameraReader::topic_callback, this, std::placeholders::_1)
+            );
+
+            RCLCPP_INFO(this-> get_logger(), "waiting for camera data....");
+        }
+
+    private:
+        void topic_callback(const sensor_msgs::msg::Image::SharedPtr msg) const {
+            cv_bridge::CvImagePtr cv_ptr;
+            try{
+                cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+
+            }catch (cv_bridge::Exception& e) {
+                RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
+                return;
+            }
+
+            int height = cv_ptr->image.rows;
+            int width = cv_ptr->image.cols;
+
+            static int count = 0;
+            if (count%30 == 0){
+                RCLCPP_INFO(this->get_logger(), "Received Image: %dx%d pixels", width, height);
+
+            }
+            count++;
+        }
+        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
+        
+};
+
+
+int main(int argc, char*argv[]){
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<CameraReader>());
+
+    rclcpp::shutdown();
+    return 0;
+}
+