[2024年度ゼミ] suetsugu

detect_circle/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "vector": "cpp"
+    }
+}

detect_circle/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(detect_circle)
+
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  roscpp
+  sensor_msgs
+  std_msgs
+  geometry_msgs
+  # message_generation
+)
+
+# add_message_files(
+#   FILES
+# PointStampedArray.msg
+# )
+
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+#   geometry_msgs
+# )
+
+# catkin_package(
+#   CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs message_runtime
+# )
+
+catkin_package(
+  CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME}_node src/detect_circle.cpp)
+
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)

detect_circle/launch/detect_circle.launch

@@ -0,0 +1,74 @@
+<!--
+<launch>
+    <node pkg="detect_circle" type="detect_circle" name="detect_circle" output="screen">
+        <param name="canny_threshold" type="int" value="100"/>
+        <param name="accumulator_threshold" type="int" value="50"/>
+    </node>
+</launch>
+-->
+
+<launch>
+
+  <node name="image_decompresser"
+       pkg="image_transport" type="republish"
+       args="compressed raw" respawn="true">
+    <remap from="in" to="/kinect_head/rgb/image_rect_color"/>
+    <remap from="out" to="/detect_circle/decompressed_image"/>
+  </node>
+
+  <!-- needs image_transport_plugins/#64 -->
+  <node name="depth_decompresser"
+       pkg="image_transport" type="republish"
+       args="compressedDepth raw" respawn="true">
+    <remap from="in" to="/kinect_head/depth_registered/image"/>
+    <remap from="out" to="/detect_circle/decompressed_depth"/>
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="decompress_points"
+          args="standalone depth_image_proc/point_cloud_xyzrgb">
+      <remap from="rgb/camera_info" to="/kinect_head/depth_registered/camera_info"/>
+      <remap from="rgb/image_rect_color" to="/detect_circle/decompressed_image"/>
+      <remap from="depth_registered/image_rect" to="/detect_circle/decompressed_depth"/>
+      <remap from="depth_registered/points" to="/detect_circle/depth_registered/points"/> 
+      <rosparam>
+        queue_size: 100
+      </rosparam>
+    </node>
+
+    <!-- <include file="$(find opencv_apps)/launch/hls_color_filter.launch">
+        <arg name="image" value="/detect_circle/decompressed_image" />
+        <arg name="h_limit_min" value="0" />
+        <arg name="h_limit_max" value="360" />
+        <arg name="l_limit_min" value="0" />
+        <arg name="l_limit_max" value="60" />
+        <arg name="s_limit_min" value="0" />
+        <arg name="s_limit_max" value="70" />
+    </include> -->
+
+    <!-- <node pkg="jsk_perception" type="mask_image_generator" name="mask_image_generator" output="screen">
+        <remap from="~input" to="/detect_circle/decompressed_image" />
+        <rosparam>
+            offset_x: 100
+            offset_y: 40
+            width: 450
+            height: 300
+        </rosparam>
+    </node> -->
+    <!-- <node pkg="jsk_perception" type="multiply_mask_image" name="multiply_mask_image" output="screen">
+        <remap from="~input/src1" to="hls_color_filter/image" />
+        <remap from="~input/src2" to="mask_image_generator/output" />
+    </node> -->
+
+    <include file="$(find opencv_apps)/launch/hough_circles.launch">
+        <arg name="image" value="/detect_circle/decompressed_image" />
+        <arg name="canny_threshold" value="160" />
+        <arg name="accumulator_threshold" value="100" />
+        <arg name="max_circle_radius" value="50" />
+    </include>
+    <node pkg="detect_circle" type="detect_circle_node" name="convert_hough_circle2point_stamped" output="screen">
+    </node>
+    <node pkg="jsk_pcl_ros" type="pointcloud_screenpoint" name="pointcloud_screenpoint" output="screen">
+        <remap from="~points" to="/detect_circle/depth_registered/points" />
+        <remap from="~point" to="/camera/circle/point" />
+    </node>
+</launch>

detect_circle/msg/PointStampedArray.msg

@@ -0,0 +1,2 @@
+Header header
+geometry_msgs/Point[] points

detect_circle/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>detect_circle</name>
+  <version>0.0.0</version>
+  <description>The detect_circle package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">mech-user</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/detect_circle</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+    <build_depend>message_generation</build_depend>
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+    <exec_depend>message_runtime</exec_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

detect_circle/src/detect_circle.cpp

@@ -0,0 +1,162 @@
+// #include "opencv2/imgcodecs.hpp"
+// #include "opencv2/highgui.hpp"
+// #include "opencv2/imgproc.hpp"
+#include <ros/ros.h>
+#include <opencv_apps/CircleArrayStamped.h>
+// #include <image_transport/image_transport.h>
+// #include <cv_bridge/cv_bridge.h>
+// #include <sensor_msgs/image_encodings.h>
+#include <geometry_msgs/PointStamped.h>
+// #include <detect_circle/PointStampedArray.h>
+#include <iostream>
+
+
+
+// // initial and max values of the parameters of interests.
+// const int canny_threshold_initial_value = 100;
+// const int accumulator_threshold_initial_value = 50;
+// // const int max_accumulator_threshold = 200;
+// // const int max_canny_threshold = 255;
+
+// int canny_threshold = canny_threshold_initial_value;
+// int accumulator_threshold = accumulator_threshold_initial_value;
+
+// class DetectCircle
+// {
+//     private:
+//         ros::NodeHandle nh;
+//         // ros::NodeHandle pnh("~");
+//         ros::Subscriber sub;
+//         ros::Publisher pub_img;
+//         ros::Publisher pub_points;
+
+//     public:
+//         DetectCircle()
+//         {
+//             sub = nh.subscribe("/usb_cam/image_raw", 1, &DetectCircle::callback, this);
+//             pub_img = nh.advertise<sensor_msgs::Image>("/camera/image/circle", 1);
+//             pub_points = nh.advertise<detect_circle::PointStampedArray>("/camera/points/circle", 1);
+//             pnh.getParam("canny_threshold", canny_threshold);
+//             pnh.getParam("accumulator_threshold", accumulator_threshold);
+//         }
+
+//         ~DetectCircle(){};
+
+//         void callback(const sensor_msgs::ImageConstPtr& msg)
+//         {
+//             cv_bridge::CvImagePtr cv_ptr;
+//             try
+//             {
+//                 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+//             }
+//             catch(cv_bridge::Exception& e)
+//             {
+//                 ROS_ERROR("cv_bridge exception: %s", e.what());
+//                 return;
+//             }
+
+//             cv::Mat src, src_gray;
+
+//             src = cv_ptr->image;
+
+//             if(src.empty())
+//             {
+//                 std::cerr << "Invalid input image\n";
+//                 return;
+//             }
+
+//             // Convert it to gray
+//             cv::cvtColor(src, src_gray, cv::COLOR_BGR2GRAY);
+
+//             // Reduce the noise so we avoid false circle detection
+//             cv::GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2);
+
+//             detect_circle::PointStampedArray points;
+//             points.header = msg->header;
+
+//             // will hold the results of the detection
+//             std::vector<cv::Vec3f> circles;
+//             // runs the actual detection
+//             cv::HoughCircles(src_gray, circles, cv::HOUGH_GRADIENT, 1, src_gray.rows/8, canny_threshold, accumulator_threshold, 0, 0);
+
+//             // clone the colour, input image for displaying purposes
+//             cv::Mat img = src.clone();
+//             points.points.resize(circles.size());
+//             for(size_t i = 0; i < circles.size(); i++)
+//             {
+//                 cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
+//                 int radius = cvRound(circles[i][2]);
+//                 points.points[i].x = center.x;
+//                 points.points[i].y = center.y;
+//                 points.points[i].z = 0;
+//                 // circle center
+//                 cv::circle(img, center, 3, cv::Scalar(0,255,0), -1, 8, 0);
+//                 // circle outline
+//                 cv::circle(img, center, radius, cv::Scalar(0,0,255), 3, 8, 0);
+//             }
+
+//             sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
+
+//             ROS_INFO("Number of circles detected: %ld", circles.size());
+
+//             for (size_t i = 0; i < circles.size(); i++)
+//             {
+//                 ROS_INFO("Circle #%ld: center at (%d, %d)", i, (int)(points.points[i].x), (int)(points.points[i].y));
+//             }
+
+//             pub_img.publish(img_msg);
+//             pub_points.publish(points);     
+//         }
+
+// };
+
+class ConvertHoughCircle2PointStamped
+{
+    private:
+        ros::NodeHandle nh;
+        ros::Subscriber sub;
+        ros::Publisher pub;
+
+    public:
+        ConvertHoughCircle2PointStamped()
+        {
+            sub = nh.subscribe("/hough_circles/circles", 1, &ConvertHoughCircle2PointStamped::callback, this);
+            pub = nh.advertise<geometry_msgs::PointStamped>("/camera/circle/point", 1);
+        }
+
+        ~ConvertHoughCircle2PointStamped(){};
+
+        void callback(const opencv_apps::CircleArrayStamped& msg)
+        {
+            geometry_msgs::PointStamped point;
+            if (msg.circles.size() == 0)
+            {
+                ROS_INFO("No circle detected");
+                return;
+            }
+            point.header = msg.header;
+            point.point.x = msg.circles[0].center.x;
+            point.point.y = msg.circles[0].center.y;
+            point.point.z = 0;
+
+            ROS_INFO("Circle center at (%f, %f)", point.point.x, point.point.y);
+
+            pub.publish(point);
+        }
+
+};
+
+int main(int argc, char** argv)
+{
+    // ros::init(argc, argv, "detect_circle");
+    // DetectCircle dc;
+    // ROS_INFO("detect_circle node has started");
+    // ros::spin();
+
+    ros::init(argc, argv, "detect_circle");
+    ConvertHoughCircle2PointStamped chc2ps;
+    ROS_INFO("convert_hough_circle2point_stamped node has started");
+    ros::spin();
+
+    return 0;
+}

jsk_2024_10_semi/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_2024_10_semi)
+
+find_package(catkin REQUIRED COMPONENTS catkin_virtualenv)
+
+catkin_package()
+
+# Generate the virtualenv
+catkin_generate_virtualenv(
+  PYTHON_INTERPRETER python3.10
+  USE_SYSTEM_PACKAGES FALSE  # Default TRUE
+
+  # Provide extra arguments to the underlying pip invocation
+  #EXTRA_PIP_ARGS  -vvv
+  )
+
+install(DIRECTORY euslisp
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+
+install(FILES requirements.txt
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+catkin_install_python(
+  PROGRAMS scripts/conversation.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

jsk_2024_10_semi/README.md

@@ -0,0 +1,3 @@
+# jsk_2024_10_semi
+
+