From 37f04b96e269944d25f7db97b66b10be941ae3cb Mon Sep 17 00:00:00 2001
From: Robot <zhangwanjie@126.com>
Date: Mon, 29 Jan 2024 08:16:52 +0800
Subject: [PATCH] update demo

---
 README.md                     |  2 +-
 demo_cmakelists/3_my_node.txt | 35 ++++++++++++++++++++++++++++++
 demo_cpp/5_lidar_behavior.cpp |  1 -
 demo_cpp/9_cv_follow.cpp      |  5 ++++-
 demo_package/3_my_node.xml    | 20 ++++++++++++++++++
 exercises/9_cv_save_file.cpp  | 40 +++++++++++++++++++++++++++++++++++
 6 files changed, 100 insertions(+), 3 deletions(-)
 create mode 100644 demo_cmakelists/3_my_node.txt
 create mode 100644 demo_package/3_my_node.xml
 create mode 100644 exercises/9_cv_save_file.cpp

diff --git a/README.md b/README.md
index 7f903b1..d29ba95 100644
--- a/README.md
+++ b/README.md
@@ -10,7 +10,7 @@
 1. 获取源码:
 ```
 cd ~/ros2_ws/src/
-git clone https://gitee.com/zhangwanjie/wpr_simulation2.git
+git clone https://github.com/6-robot/wpr_simulation2.git
 ```
 2. 安装依赖项:  
 ROS2 Humble (Ubuntu 22.04)
diff --git a/demo_cmakelists/3_my_node.txt b/demo_cmakelists/3_my_node.txt
new file mode 100644
index 0000000..b6f5b39
--- /dev/null
+++ b/demo_cmakelists/3_my_node.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_pkg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_executable(my_node src/my_node.cpp)
+ament_target_dependencies(my_node "rclcpp")
+
+install(TARGETS
+ my_node
+DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/demo_cpp/5_lidar_behavior.cpp b/demo_cpp/5_lidar_behavior.cpp
index 30fe9b5..18e654f 100644
--- a/demo_cpp/5_lidar_behavior.cpp
+++ b/demo_cpp/5_lidar_behavior.cpp
@@ -1,5 +1,4 @@
 #include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
diff --git a/demo_cpp/9_cv_follow.cpp b/demo_cpp/9_cv_follow.cpp
index d9d767a..ff80f39 100644
--- a/demo_cpp/9_cv_follow.cpp
+++ b/demo_cpp/9_cv_follow.cpp
@@ -108,7 +108,7 @@ void Cam_RGB_Callback(const sensor_msgs::msg::Image::SharedPtr msg)
 
     // Show the image
     imshow("Result", imgThresholded);
-    imshow("Original", imgOriginal);
+    imshow("RGB", imgOriginal);
     cv::waitKey(5);
 }
 
@@ -123,6 +123,9 @@ int main(int argc, char** argv)
         10, 
         Cam_RGB_Callback);
 
+    namedWindow("RGB");
+    namedWindow("Result");
+    
     rclcpp::spin(node);
 
     cv::destroyAllWindows();
diff --git a/demo_package/3_my_node.xml b/demo_package/3_my_node.xml
new file mode 100644
index 0000000..906c104
--- /dev/null
+++ b/demo_package/3_my_node.xml
@@ -0,0 +1,20 @@
+<?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>my_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="robot@6-robot.com">robot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/exercises/9_cv_save_file.cpp b/exercises/9_cv_save_file.cpp
new file mode 100644
index 0000000..5d95539
--- /dev/null
+++ b/exercises/9_cv_save_file.cpp
@@ -0,0 +1,40 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+std::shared_ptr<rclcpp::Node> node;
+
+void CamRGBCallback(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+    cv_bridge::CvImagePtr cv_ptr;
+    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+
+    cv::Mat imgOriginal = cv_ptr->image;
+    cv::imshow("RGB", imgOriginal);
+    cv::waitKey(1);
+
+    // Save the image to file
+    cv::imwrite("/home/robot/image.jpg",imgOriginal);
+    printf("Saved the image to file!\n");
+}
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    node = std::make_shared<rclcpp::Node>("cv_save_file");
+
+    auto rgb_sub = node->create_subscription<sensor_msgs::msg::Image>(
+        "/kinect2/qhd/image_raw", 1, CamRGBCallback);
+
+    cv::namedWindow("RGB");
+
+    rclcpp::spin(node);
+    
+    cv::destroyAllWindows();
+    
+    rclcpp::shutdown();
+
+    return 0;
+}
\ No newline at end of file