diff --git a/.github/workflows/clone.yml b/.github/workflows/clone.yml new file mode 100644 index 0000000..074054b --- /dev/null +++ b/.github/workflows/clone.yml @@ -0,0 +1,90 @@ +name: GitHub Clone Count Update Everyday + +on: + schedule: + - cron: "0 */24 * * *" + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + + - name: gh login + run: echo "${{ secrets.SECRET_TOKEN }}" | gh auth login --with-token + + - name: parse latest clone count + run: | + curl --user "${{ github.actor }}:${{ secrets.SECRET_TOKEN }}" \ + -H "Accept: application/vnd.github.v3+json" \ + https://api.github.com/repos/${{ github.repository }}/traffic/clones \ + > clone.json + + - name: create gist and download previous count + id: set_id + run: | + if gh secret list | grep -q "GIST_ID" + then + echo "GIST_ID found" + echo "GIST=${{ secrets.GIST_ID }}" >> $GITHUB_OUTPUT + curl https://gist.githubusercontent.com/${{ github.actor }}/${{ secrets.GIST_ID }}/raw/clone.json > clone_before.json + if cat clone_before.json | grep '404: Not Found'; then + echo "GIST_ID not valid anymore. Creating another gist..." + gist_id=$(gh gist create clone.json | awk -F / '{print $NF}') + echo $gist_id | gh secret set GIST_ID + echo "GIST=$gist_id" >> $GITHUB_OUTPUT + cp clone.json clone_before.json + git rm --ignore-unmatch CLONE.md + fi + else + echo "GIST_ID not found. Creating a gist..." + gist_id=$(gh gist create clone.json | awk -F / '{print $NF}') + echo $gist_id | gh secret set GIST_ID + echo "GIST=$gist_id" >> $GITHUB_OUTPUT + cp clone.json clone_before.json + fi + + - name: update clone.json + run: | + curl https://raw.githubusercontent.com/MShawon/github-clone-count-badge/master/main.py > main.py + python3 main.py + + - name: Update gist with latest count + run: | + content=$(sed -e 's/\\/\\\\/g' -e 's/\t/\\t/g' -e 's/\"/\\"/g' -e 's/\r//g' "clone.json" | sed -E ':a;N;$!ba;s/\r{0,1}\n/\\n/g') + echo '{"description": "${{ github.repository }} clone statistics", "files": {"clone.json": {"content": "'"$content"'"}}}' > post_clone.json + curl -s -X PATCH \ + --user "${{ github.actor }}:${{ secrets.SECRET_TOKEN }}" \ + -H "Content-Type: application/json" \ + -d @post_clone.json https://api.github.com/gists/${{ steps.set_id.outputs.GIST }} > /dev/null 2>&1 + + if [ ! -f CLONE.md ]; then + shields="https://img.shields.io/badge/dynamic/json?color=success&label=Clone&query=count&url=" + url="https://gist.githubusercontent.com/${{ github.actor }}/${{ steps.set_id.outputs.GIST }}/raw/clone.json" + repo="https://github.com/MShawon/github-clone-count-badge" + echo ''> CLONE.md + echo ' + **Markdown** + + ```markdown' >> CLONE.md + echo "[![GitHub Clones]($shields$url&logo=github)]($repo)" >> CLONE.md + echo ' + ``` + + **HTML** + ```html' >> CLONE.md + echo "GitHub Clones" >> CLONE.md + echo '```' >> CLONE.md + + git add CLONE.md + git config --global user.name "GitHub Action" + git config --global user.email "action@github.com" + git commit -m "create clone count badge" + fi + + - name: Push + uses: ad-m/github-push-action@master + with: + github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 01412d7..0000000 --- a/.travis.yml +++ /dev/null @@ -1,30 +0,0 @@ -# This config file for Travis CI utilizes ros-industrial/industrial_ci package. -# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst - -sudo: required -dist: trusty -services: - - docker -language: generic -python: - - "3.8" -compiler: - - gcc -notifications: - email: - on_success: change - on_failure: always - recipients: - - ronaldsonbellande@gmail.com -env: - matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_intelligence_control_system_demo.rosinstall" -branches: - only: - - master - - noetic -install: - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config -script: - - source .ci_config/travis.sh - diff --git a/git_scripts/.gitignore b/git_scripts/.gitignore new file mode 100644 index 0000000..e5a7a9c --- /dev/null +++ b/git_scripts/.gitignore @@ -0,0 +1,3 @@ +fix_errors.sh +push.sh +repository_recal.sh diff --git a/humanoid_robot_intelligence_control_system_ball_detector/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_ball_detector/CHANGELOG.rst deleted file mode 100644 index 538e59c..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/CHANGELOG.rst +++ /dev/null @@ -1,34 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_intelligence_control_system_ball_detector -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2023-10-03) ------------------- -* Make it compatible for ROS1/ROS2 -* Fix bugs -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.1 (2023-09-27) ------------------- -* Starting from this point it under a new license -* Fix errors and Issues -* Rename Repository for a completely different purpose -* Make it compatible with ROS/ROS2 -* Upgrade version of all builds and make it more compatible -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.0 (2021-05-05) ------------------- -* Update package.xml and CMakeList.txt for noetic branch -* Contributors: Ronaldson Bellande - -0.1.0 (2018-04-19) ------------------- -* first release for ROS Kinetic -* added launch files in order to move the camera setting to humanoid_robot_intelligence_control_system_camera_setting package -* added missing package in find_package() -* refacoring to release -* split repositoryfrom ROBOTIS-HUMANOID_ROBOT -* Contributors: Kayman, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_ball_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_ball_detector/CMakeLists.txt deleted file mode 100644 index dff5405..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/CMakeLists.txt +++ /dev/null @@ -1,133 +0,0 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_intelligence_control_system_ball_detector) - - -if($ENV{ROS_VERSION} EQUAL 1) - find_package( - catkin REQUIRED COMPONENTS - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - message_generation - ) - - find_package(Boost REQUIRED COMPONENTS thread) - find_package(OpenCV 4.2 REQUIRED) - find_package(PkgConfig REQUIRED) -else() - find_package(ament_cmake REQUIRED) -endif() - - -pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) -find_path( - YAML_CPP_INCLUDE_DIR - NAMES yaml_cpp.h - PATHS ${YAML_CPP_INCLUDE_DIRS} -) -find_library( - YAML_CPP_LIBRARY - NAMES YAML_CPP - PATHS ${YAML_CPP_LIBRARY_DIRS} -) -link_directories(${YAML_CPP_LIBRARY_DIRS}) - -if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - add_definitions(-DHAVE_NEW_YAMLCPP) -endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - -add_message_files( - FILES - CircleSetStamped.msg - BallDetectorParams.msg -) - -add_service_files( - FILES - GetParameters.srv - SetParameters.srv -) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -generate_dynamic_reconfigure_options(cfg/DetectorParams.cfg) - -if($ENV{ROS_VERSION} EQUAL 1) - catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - message_runtime - DEPENDS Boost OpenCV - ) -endif() - - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} -) - -add_executable( - ball_detector_node - src/ball_detector.cpp - src/ball_detector_node.cpp -) - -add_dependencies(ball_detector_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries( - ball_detector_node - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${OpenCV_LIBRARIES} - ${YAML_CPP_LIBRARIES} -) - -install( - TARGETS ball_detector_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install( - DIRECTORY config launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParams.cfg b/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParams.cfg deleted file mode 100755 index abc5bff..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParams.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -PACKAGE='humanoid_robot_intelligence_control_system_ball_detector' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# Name Type Reconfiguration levexl Description Default Min Max -gen.add("gaussian_blur_size",int_t , -1, "Size of Gaussian Blur Kernel (odd value!)", 7, 1, 11) -gen.add("gaussian_blur_sigma",double_t , -1, "Std deviation of Gaussian Blur Kernel", 2, 1, 5) -gen.add("canny_edge_th",double_t , -1, "Threshold of the edge detector", 50, 50, 200) -gen.add("hough_accum_resolution",double_t , -1, "Resolution of the Hough accumulator, in terms of inverse ratio of image resolution", 2, 1, 8) -gen.add("min_circle_dist",double_t , -1, "Minimum distance between circles", 40, 10, 200) -gen.add("hough_accum_th",double_t , -1, "Accumulator threshold to decide circle detection", 15, 10, 200) -gen.add("min_radius",int_t , -1, "Minimum circle radius allowed", 20, 10, 200) -gen.add("max_radius",int_t , -1, "Maximum circle radius allowed", 150, 100, 600) -gen.add("filter_h_min",int_t , -1, "Threshold of H filter", 180, 0, 359) -gen.add("filter_h_max",int_t , -1, "Threshold of H filter", 245, 0, 359) -gen.add("filter_s_min",int_t , -1, "Threshold of S filter", 200, 0, 255) -gen.add("filter_s_max",int_t , -1, "Threshold of S filter", 255, 0, 255) -gen.add("filter_v_min",int_t , -1, "Threshold of V filter", 50, 0, 255) -gen.add("filter_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255) -gen.add("use_second_filter", bool_t, 0, "Use second filter", False) -gen.add("filter2_h_min",int_t , -1, "Threshold of H filter", 160, 0, 359) -gen.add("filter2_h_max",int_t , -1, "Threshold of H filter", 255, 0, 359) -gen.add("filter2_s_min",int_t , -1, "Threshold of S filter", 0, 0, 255) -gen.add("filter2_s_max",int_t , -1, "Threshold of S filter", 55, 0, 255) -gen.add("filter2_v_min",int_t , -1, "Threshold of V filter", 180, 0, 255) -gen.add("filter2_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255) -gen.add("ellipse_size",int_t , -1, "Ellipse size", 2, 1, 9) -gen.add("debug_image", bool_t, 0, "Show filtered image to debug", False) - -exit(gen.generate(PACKAGE, "ball_detector_node", "DetectorParams")) diff --git a/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParamsBlue.cfg b/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParamsBlue.cfg deleted file mode 100755 index 9ef2700..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParamsBlue.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -PACKAGE='humanoid_robot_intelligence_control_system_ball_detector' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# Name Type Reconfiguration levexl Description Default Min Max -gen.add("gaussian_blur_size",int_t , -1, "Size of Gaussian Blur Kernel (odd value!)", 7, 1, 11) -gen.add("gaussian_blur_sigma",double_t , -1, "Std deviation of Gaussian Blur Kernel", 2, 1, 5) -gen.add("canny_edge_th",double_t , -1, "Threshold of the edge detector", 50, 50, 200) -gen.add("hough_accum_resolution",double_t , -1, "Resolution of the Hough accumulator, in terms of inverse ratio of image resolution", 2, 1, 8) -gen.add("min_circle_dist",double_t , -1, "Minimum distance between circles", 40, 10, 200) -gen.add("hough_accum_th",double_t , -1, "Accumulator threshold to decide circle detection", 15, 10, 200) -gen.add("min_radius",int_t , -1, "Minimum circle radius allowed", 20, 10, 200) -gen.add("max_radius",int_t , -1, "Maximum circle radius allowed", 150, 100, 600) -gen.add("filter_h_min",int_t , -1, "Threshold of H filter", 180, 0, 359) -gen.add("filter_h_max",int_t , -1, "Threshold of H filter", 245, 0, 359) -gen.add("filter_s_min",int_t , -1, "Threshold of S filter", 200, 0, 255) -gen.add("filter_s_max",int_t , -1, "Threshold of S filter", 255, 0, 255) -gen.add("filter_v_min",int_t , -1, "Threshold of V filter", 50, 0, 255) -gen.add("filter_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255) -gen.add("use_second_filter", bool_t, 0, "Use second filter", False) -gen.add("filter2_h_min",int_t , -1, "Threshold of H filter", 160, 0, 359) -gen.add("filter2_h_max",int_t , -1, "Threshold of H filter", 255, 0, 359) -gen.add("filter2_s_min",int_t , -1, "Threshold of S filter", 0, 0, 255) -gen.add("filter2_s_max",int_t , -1, "Threshold of S filter", 55, 0, 255) -gen.add("filter2_v_min",int_t , -1, "Threshold of V filter", 180, 0, 255) -gen.add("filter2_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255) -gen.add("ellipse_size",int_t , -1, "Ellipse size", 2, 1, 9) -gen.add("debug_image", bool_t, 0, "Show filtered image to debug", False) - -exit(gen.generate(PACKAGE, "ball_detector_node", "DetectorParamsBlue")) diff --git a/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParamsRed.cfg b/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParamsRed.cfg deleted file mode 100755 index c0eed94..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/cfg/DetectorParamsRed.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -PACKAGE='humanoid_robot_intelligence_control_system_ball_detector' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# Name Type Reconfiguration levexl Description Default Min Max -gen.add("gaussian_blur_size",int_t , -1, "Size of Gaussian Blur Kernel (odd value!)", 7, 1, 11) -gen.add("gaussian_blur_sigma",double_t , -1, "Std deviation of Gaussian Blur Kernel", 2, 1, 5) -gen.add("canny_edge_th",double_t , -1, "Threshold of the edge detector", 50, 50, 200) -gen.add("hough_accum_resolution",double_t , -1, "Resolution of the Hough accumulator, in terms of inverse ratio of image resolution", 2, 1, 8) -gen.add("min_circle_dist",double_t , -1, "Minimum distance between circles", 40, 10, 200) -gen.add("hough_accum_th",double_t , -1, "Accumulator threshold to decide circle detection", 15, 10, 200) -gen.add("min_radius",int_t , -1, "Minimum circle radius allowed", 20, 10, 200) -gen.add("max_radius",int_t , -1, "Maximum circle radius allowed", 150, 100, 600) -gen.add("filter_h_min",int_t , -1, "Threshold of H filter", 330, 0, 359) -gen.add("filter_h_max",int_t , -1, "Threshold of H filter", 30, 0, 359) -gen.add("filter_s_min",int_t , -1, "Threshold of S filter", 128, 0, 255) -gen.add("filter_s_max",int_t , -1, "Threshold of S filter", 255, 0, 255) -gen.add("filter_v_min",int_t , -1, "Threshold of V filter", 128, 0, 255) -gen.add("filter_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255) -gen.add("use_second_filter", bool_t, 0, "Use second filter", False) -gen.add("filter2_h_min",int_t , -1, "Threshold of H filter", 160, 0, 359) -gen.add("filter2_h_max",int_t , -1, "Threshold of H filter", 255, 0, 359) -gen.add("filter2_s_min",int_t , -1, "Threshold of S filter", 0, 0, 255) -gen.add("filter2_s_max",int_t , -1, "Threshold of S filter", 55, 0, 255) -gen.add("filter2_v_min",int_t , -1, "Threshold of V filter", 180, 0, 255) -gen.add("filter2_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255) -gen.add("ellipse_size",int_t , -1, "Ellipse size", 5, 1, 9) -gen.add("debug_image", bool_t, 0, "Show filtered image to debug", False) - -exit(gen.generate(PACKAGE, "ball_detector_node", "DetectorParamsRed")) diff --git a/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params.yaml b/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params.yaml deleted file mode 100644 index f7013fb..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params.yaml +++ /dev/null @@ -1,23 +0,0 @@ -gaussian_blur_size: 7 -gaussian_blur_sigma: 2 -canny_edge_th: 100 -hough_accum_resolution: 1 -min_circle_dist: 100 -hough_accum_th: 28 -min_radius: 20 -max_radius: 300 -filter_h_min: 355 -filter_h_max: 25 -filter_s_min: 220 -filter_s_max: 255 -filter_v_min: 80 -filter_v_max: 255 -use_second_filter: false -filter2_h_min: 30 -filter2_h_max: 355 -filter2_s_min: 0 -filter2_s_max: 40 -filter2_v_min: 200 -filter2_v_max: 255 -ellipse_size: 2 -filter_debug: false \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params_default.yaml b/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params_default.yaml deleted file mode 100644 index 49cc203..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params_default.yaml +++ /dev/null @@ -1,23 +0,0 @@ -gaussian_blur_size: 7 -gaussian_blur_sigma: 2 -canny_edge_th: 100 -hough_accum_resolution: 1 -min_circle_dist: 100 -hough_accum_th: 28 -min_radius: 20 -max_radius: 300 -filter_h_min: 350 -filter_h_max: 15 -filter_s_min: 200 -filter_s_max: 255 -filter_v_min: 60 -filter_v_max: 255 -use_second_filter: false -filter2_h_min: 30 -filter2_h_max: 355 -filter2_s_min: 0 -filter2_s_max: 40 -filter2_v_min: 200 -filter2_v_max: 255 -ellipse_size: 2 -filter_debug: false \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params_op.yaml b/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params_op.yaml deleted file mode 100644 index 7310922..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/config/ball_detector_params_op.yaml +++ /dev/null @@ -1,23 +0,0 @@ -gaussian_blur_size: 7 -gaussian_blur_sigma: 2.52 -canny_edge_th: 100.5 -hough_accum_resolution: 1 -min_circle_dist: 28.5 -hough_accum_th: 26.6 -min_radius: 25 -max_radius: 150 -filter_h_min: 350 -filter_h_max: 20 -filter_s_min: 90 -filter_s_max: 255 -filter_v_min: 86 -filter_v_max: 255 -use_second_filter: true -filter2_h_min: 30 -filter2_h_max: 355 -filter2_s_min: 0 -filter2_s_max: 40 -filter2_v_min: 200 -filter2_v_max: 255 -ellipse_size: 1 -filter_debug: false \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_ball_detector/include/humanoid_robot_intelligence_control_system_ball_detector/ball_detector.h b/humanoid_robot_intelligence_control_system_ball_detector/include/humanoid_robot_intelligence_control_system_ball_detector/ball_detector.h deleted file mode 100644 index 91a1bf6..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/include/humanoid_robot_intelligence_control_system_ball_detector/ball_detector.h +++ /dev/null @@ -1,169 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef _BALL_DETECTOR_H_ -#define _BALL_DETECTOR_H_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_ball_detector/ball_detector_config.h" - -#include "humanoid_robot_intelligence_control_system_ball_detector/CircleSetStamped.h" -#include "humanoid_robot_intelligence_control_system_ball_detector/DetectorParamsConfig.h" -#include "humanoid_robot_intelligence_control_system_ball_detector/GetParameters.h" -#include "humanoid_robot_intelligence_control_system_ball_detector/SetParameters.h" - -namespace humanoid_robot_intelligence_control_system_op { - -class BallDetector { -public: - BallDetector(); - ~BallDetector(); - - // checks if a new image has been received - bool newImage(); - - // execute circle detection with the cureent image - void process(); - - // publish the output image (input image + marked circles) - void publishImage(); - - // publish the circle set data - void publishCircles(); - -protected: - const static int NOT_FOUND_TH = 30; - - // callbacks to image subscription - void imageCallback(const sensor_msgs::ImageConstPtr &msg); - - // callbacks to camera info subscription - void cameraInfoCallback(const sensor_msgs::CameraInfo &msg); - - void dynParamCallback(humanoid_robot_intelligence_control_system_ball_detector::DetectorParamsConfig &config, - uint32_t level); - void enableCallback(const std_msgs::Bool::ConstPtr &msg); - - void paramCommandCallback(const std_msgs::String::ConstPtr &msg); - bool setParamCallback(humanoid_robot_intelligence_control_system_ball_detector::SetParameters::Request &req, - humanoid_robot_intelligence_control_system_ball_detector::SetParameters::Response &res); - bool getParamCallback(humanoid_robot_intelligence_control_system_ball_detector::GetParameters::Request &req, - humanoid_robot_intelligence_control_system_ball_detector::GetParameters::Response &res); - void resetParameter(); - void publishParam(); - - void printConfig(); - void saveConfig(); - void setInputImage(const cv::Mat &inIm); - void setInputImage(const cv::Mat &inIm, cv::Mat &in_filter_img); - void getOutputImage(cv::Mat &outIm); - void filterImage(); - void filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img); - void makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range); - void makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img); - void inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, - cv::Mat &output_img); - void mophology(const cv::Mat &intput_img, cv::Mat &output_img, - int ellipse_size); - void houghDetection(const unsigned int imgEncoding); - void houghDetection2(const cv::Mat &input_hough); - void drawOutputImage(); - - // ros node handle - ros::NodeHandle nh_; - - ros::Subscriber enable_sub_; - - // image publisher/subscriber - image_transport::ImageTransport it_; - image_transport::Publisher image_pub_; - cv_bridge::CvImage cv_img_pub_; - image_transport::Subscriber image_sub_; - cv_bridge::CvImagePtr cv_img_ptr_sub_; - - bool enable_; - bool init_param_; - int not_found_count_; - - // circle set publisher - humanoid_robot_intelligence_control_system_ball_detector::CircleSetStamped circles_msg_; - ros::Publisher circles_pub_; - - // camera info subscriber - sensor_msgs::CameraInfo camera_info_msg_; - ros::Subscriber camera_info_sub_; - ros::Publisher camera_info_pub_; - - // dynamic reconfigure - DetectorConfig params_config_; - std::string param_path_; - bool has_path_; - - // web setting - std::string default_setting_path_; - ros::Publisher param_pub_; - ros::Subscriber param_command_sub_; - ros::ServiceServer get_param_client_; - ros::ServiceServer set_param_client_; - - // flag indicating a new image has been received - bool new_image_flag_; - - // image time stamp and frame id - ros::Time sub_time_; - std::string image_frame_id_; - - // img encoding id - unsigned int img_encoding_; - - /** \brief Set of detected circles - * - * Detected circles. For a circle i: - * x_i: circles[i][0] - * y_i: circles[i][1] - * radius_i: circles[i][2] - * - **/ - std::vector circles_; - cv::Mat in_image_; - cv::Mat out_image_; - - dynamic_reconfigure::Server - param_server_; - dynamic_reconfigure::Server< - humanoid_robot_intelligence_control_system_ball_detector::DetectorParamsConfig>::CallbackType callback_fnc_; -}; - -} // namespace humanoid_robot_intelligence_control_system_op -#endif // _BALL_DETECTOR_H_ diff --git a/humanoid_robot_intelligence_control_system_ball_detector/include/humanoid_robot_intelligence_control_system_ball_detector/ball_detector_config.h b/humanoid_robot_intelligence_control_system_ball_detector/include/humanoid_robot_intelligence_control_system_ball_detector/ball_detector_config.h deleted file mode 100644 index a7e7a3a..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/include/humanoid_robot_intelligence_control_system_ball_detector/ball_detector_config.h +++ /dev/null @@ -1,90 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef _DETECTOR_CONFIG_H_ -#define _DETECTOR_CONFIG_H_ - -namespace humanoid_robot_intelligence_control_system_op { - -// constants -const int GAUSSIAN_BLUR_SIZE_DEFAULT = 7; -const double GAUSSIAN_BLUR_SIGMA_DEFAULT = 2; -const double CANNY_EDGE_TH_DEFAULT = 130; -const double HOUGH_ACCUM_RESOLUTION_DEFAULT = 2; -const double MIN_CIRCLE_DIST_DEFAULT = 30; -const double HOUGH_ACCUM_TH_DEFAULT = 120; -const int MIN_RADIUS_DEFAULT = 30; -const int MAX_RADIUS_DEFAULT = 400; -const unsigned int IMG_MONO = 0; -const unsigned int IMG_RGB8 = 1; -const int FILTER_RANGE_DEFAULT_MIN = 160; -const int FILTER_RANGE_DEFAULT_MAX = 255; -const int FILTER_H_MIN_DEFAULT = 0; -const int FILTER_H_MAX_DEFAULT = 30; -const int FILTER_S_MIN_DEFAULT = 0; -const int FILTER_S_MAX_DEFAULT = 255; -const int FILTER_V_MIN_DEFAULT = 0; -const int FILTER_V_MAX_DEFAULT = 255; -const int ELLIPSE_SIZE = 5; - -class HsvFilter { -public: - HsvFilter() - : h_min(FILTER_H_MIN_DEFAULT), h_max(FILTER_H_MAX_DEFAULT), - s_min(FILTER_S_MIN_DEFAULT), s_max(FILTER_S_MAX_DEFAULT), - v_min(FILTER_V_MIN_DEFAULT), v_max(FILTER_V_MAX_DEFAULT) {} - - int h_min; - int h_max; - int s_min; - int s_max; - int v_min; - int v_max; -}; - -class DetectorConfig { -public: - int gaussian_blur_size; // size of gaussian blur kernel mask [pixel] - double gaussian_blur_sigma; // sigma of gaussian blur kernel mask [pixel] - double canny_edge_th; // threshold of the edge detector. - double hough_accum_resolution; // resolution of the Hough accumulator, in - // terms of inverse ratio of image resolution - double min_circle_dist; // Minimum distance between circles - double hough_accum_th; // accumulator threshold to decide circle detection - int min_radius; // minimum circle radius allowed - int max_radius; // maximum circle radius allowed - HsvFilter filter_threshold; // filter threshold - bool use_second_filter; - HsvFilter filter2_threshold; // filter threshold - int ellipse_size; - bool debug; // to debug log - - DetectorConfig() - : canny_edge_th(CANNY_EDGE_TH_DEFAULT), - hough_accum_resolution(HOUGH_ACCUM_RESOLUTION_DEFAULT), - min_circle_dist(MIN_CIRCLE_DIST_DEFAULT), - hough_accum_th(HOUGH_ACCUM_TH_DEFAULT), min_radius(MIN_RADIUS_DEFAULT), - max_radius(MAX_RADIUS_DEFAULT), filter_threshold(), - use_second_filter(false), filter2_threshold(), - ellipse_size(ELLIPSE_SIZE), debug(false) {} - - ~DetectorConfig() {} -}; - -} // namespace humanoid_robot_intelligence_control_system_op -#endif // _DETECTOR_CONFIG_H_ diff --git a/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector_from_usb_cam.launch b/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector_from_usb_cam.launch deleted file mode 100644 index 2d690b8..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector_from_usb_cam.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector_from_uvc.launch b/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector_from_uvc.launch deleted file mode 100644 index a28ee87..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector_from_uvc.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_ball_detector/msg/BallDetectorParams.msg b/humanoid_robot_intelligence_control_system_ball_detector/msg/BallDetectorParams.msg deleted file mode 100644 index cd1c210..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/msg/BallDetectorParams.msg +++ /dev/null @@ -1,17 +0,0 @@ -# This represents the parameters of ball_detector - -uint32 gaussian_blur_size # only odd number, 1 - 11 -float32 gaussian_blur_sigma # 1 - 5 -float32 canny_edge_th # 50 - 200 -float32 hough_accum_resolution # 1 - 8 -float32 hough_accum_th # 10 - 200 -float32 min_circle_dist # 10 - 200 -uint32 min_radius # 10 - 200 -uint32 max_radius # 100 - 600 -uint32 filter_h_min # 0 - 359 -uint32 filter_h_max -uint32 filter_s_min # 0 - 255 -uint32 filter_s_max -uint32 filter_v_min # 0 - 255 -uint32 filter_v_max -uint32 ellipse_size # 1 - 9 diff --git a/humanoid_robot_intelligence_control_system_ball_detector/msg/CircleSetStamped.msg b/humanoid_robot_intelligence_control_system_ball_detector/msg/CircleSetStamped.msg deleted file mode 100644 index 4ed5885..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/msg/CircleSetStamped.msg +++ /dev/null @@ -1,9 +0,0 @@ -# This represents the set of detected circles - -#timestamp and frame id of the image frame -std_msgs/Header header - -#set of detected circles: -# (circles[i].x, circles[i].y) is the center point in image coordinates -# circles[i].z is the circle radius -geometry_msgs/Point[] circles diff --git a/humanoid_robot_intelligence_control_system_ball_detector/rviz/ball_detector.rviz b/humanoid_robot_intelligence_control_system_ball_detector/rviz/ball_detector.rviz deleted file mode 100644 index 7fd856d..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/rviz/ball_detector.rviz +++ /dev/null @@ -1,138 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 355 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Image -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: "" - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Image - Enabled: true - Image Topic: /ball_detector_node/image_out - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.785398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.785398 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027500000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001d2000001ec0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003af0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 1985 - Y: 24 diff --git a/humanoid_robot_intelligence_control_system_ball_detector/src/ball_detector.cpp b/humanoid_robot_intelligence_control_system_ball_detector/src/ball_detector.cpp deleted file mode 100644 index 2916b71..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/src/ball_detector.cpp +++ /dev/null @@ -1,904 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include - -#include "humanoid_robot_intelligence_control_system_ball_detector/ball_detector.h" - -namespace humanoid_robot_intelligence_control_system_op { - -BallDetector::BallDetector() - : nh_(ros::this_node::getName()), it_(this->nh_), enable_(true), - params_config_(), init_param_(false), not_found_count_(0) { - has_path_ = nh_.getParam("yaml_path", param_path_); - - if (has_path_) - std::cout << "Path : " << param_path_ << std::endl; - - // detector config struct - DetectorConfig detect_config; - - // get user parameters from dynamic reconfigure (yaml entries) - nh_.param("gaussian_blur_size", detect_config.gaussian_blur_size, - params_config_.gaussian_blur_size); - if (detect_config.gaussian_blur_size % 2 == 0) - detect_config.gaussian_blur_size -= 1; - if (detect_config.gaussian_blur_size <= 0) - detect_config.gaussian_blur_size = 1; - nh_.param("gaussian_blur_sigma", detect_config.gaussian_blur_sigma, - params_config_.gaussian_blur_sigma); - nh_.param("canny_edge_th", detect_config.canny_edge_th, - params_config_.canny_edge_th); - nh_.param("hough_accum_resolution", - detect_config.hough_accum_resolution, - params_config_.hough_accum_resolution); - nh_.param("min_circle_dist", detect_config.min_circle_dist, - params_config_.min_circle_dist); - nh_.param("hough_accum_th", detect_config.hough_accum_th, - params_config_.hough_accum_th); - nh_.param("min_radius", detect_config.min_radius, - params_config_.min_radius); - nh_.param("max_radius", detect_config.max_radius, - params_config_.max_radius); - nh_.param("filter_h_min", detect_config.filter_threshold.h_min, - params_config_.filter_threshold.h_min); - nh_.param("filter_h_max", detect_config.filter_threshold.h_max, - params_config_.filter_threshold.h_max); - nh_.param("filter_s_min", detect_config.filter_threshold.s_min, - params_config_.filter_threshold.s_min); - nh_.param("filter_s_max", detect_config.filter_threshold.s_max, - params_config_.filter_threshold.s_max); - nh_.param("filter_v_min", detect_config.filter_threshold.v_min, - params_config_.filter_threshold.v_min); - nh_.param("filter_v_max", detect_config.filter_threshold.v_max, - params_config_.filter_threshold.v_max); - nh_.param("use_second_filter", detect_config.use_second_filter, - params_config_.use_second_filter); - nh_.param("filter2_h_min", detect_config.filter2_threshold.h_min, - params_config_.filter2_threshold.h_min); - nh_.param("filter2_h_max", detect_config.filter2_threshold.h_max, - params_config_.filter2_threshold.h_max); - nh_.param("filter2_s_min", detect_config.filter2_threshold.s_min, - params_config_.filter2_threshold.s_min); - nh_.param("filter2_s_max", detect_config.filter2_threshold.s_max, - params_config_.filter2_threshold.s_max); - nh_.param("filter2_v_min", detect_config.filter2_threshold.v_min, - params_config_.filter2_threshold.v_min); - nh_.param("filter2_v_max", detect_config.filter2_threshold.v_max, - params_config_.filter2_threshold.v_max); - nh_.param("ellipse_size", detect_config.ellipse_size, - params_config_.ellipse_size); - nh_.param("filter_debug", detect_config.debug, params_config_.debug); - - // sets publishers - image_pub_ = it_.advertise("image_out", 100); - circles_pub_ = - nh_.advertise("circle_set", 100); - camera_info_pub_ = nh_.advertise("camera_info", 100); - - // sets subscribers - enable_sub_ = nh_.subscribe("enable", 1, &BallDetector::enableCallback, this); - image_sub_ = it_.subscribe("image_in", 1, &BallDetector::imageCallback, this); - camera_info_sub_ = nh_.subscribe("cameraInfo_in", 100, - &BallDetector::cameraInfoCallback, this); - - // initializes newImageFlag - new_image_flag_ = false; - - // dynamic_reconfigure - callback_fnc_ = boost::bind(&BallDetector::dynParamCallback, this, _1, _2); - param_server_.setCallback(callback_fnc_); - - // web setting - param_pub_ = - nh_.advertise("current_params", 1); - param_command_sub_ = nh_.subscribe("param_command", 1, - &BallDetector::paramCommandCallback, this); - set_param_client_ = - nh_.advertiseService("set_param", &BallDetector::setParamCallback, this); - get_param_client_ = - nh_.advertiseService("get_param", &BallDetector::getParamCallback, this); - default_setting_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + - "/config/ball_detector_params_default.yaml"; - - // sets config and prints it - params_config_ = detect_config; - init_param_ = true; - printConfig(); -} - -BallDetector::~BallDetector() {} - -bool BallDetector::newImage() { - if (new_image_flag_) { - new_image_flag_ = false; - return true; - } else { - return false; - } -} - -void BallDetector::process() { - if (enable_ == false) - return; - - if (cv_img_ptr_sub_ != NULL) { - cv::Mat img_hsv, img_filtered; - - // set input image - setInputImage(cv_img_ptr_sub_->image, img_hsv); - - // image filtering - filterImage(img_hsv, img_filtered); - - // detect circles - houghDetection2(img_filtered); - - // // set input image - // setInputImage(cv_img_ptr_sub_->image); - - // // image filtering - // filterImage(); - - // //detect circles - // houghDetection(this->img_encoding_); - } -} - -void BallDetector::publishImage() { - if (enable_ == false) - return; - - // image_raw topic - cv_img_pub_.header.seq++; - cv_img_pub_.header.stamp = sub_time_; - cv_img_pub_.header.frame_id = image_frame_id_; - switch (img_encoding_) { - case IMG_RGB8: - cv_img_pub_.encoding = sensor_msgs::image_encodings::RGB8; - break; - case IMG_MONO: - cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8; - break; - default: - cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8; - break; - } - getOutputImage(cv_img_pub_.image); - image_pub_.publish(cv_img_pub_.toImageMsg()); - camera_info_pub_.publish(camera_info_msg_); -} - -void BallDetector::publishCircles() { - if (enable_ == false) - return; - - if (circles_.size() == 0) - return; - - // clears and resize the message - circles_msg_.circles.clear(); - circles_msg_.circles.resize(circles_.size()); - - // fill header - circles_msg_.header.seq++; - circles_msg_.header.stamp = sub_time_; - circles_msg_.header.frame_id = - "detector"; // To do: get frame_id from input image - - // fill circle data - // top(-1), bottom(+1) - // left(-1), right(+1) - for (int idx = 0; idx < circles_.size(); idx++) { - circles_msg_.circles[idx].x = - circles_[idx][0] / out_image_.cols * 2 - 1; // x (-1 ~ 1) - circles_msg_.circles[idx].y = - circles_[idx][1] / out_image_.rows * 2 - 1; // y (-1 ~ 1) - circles_msg_.circles[idx].z = circles_[idx][2]; // radius - } - - // publish message - circles_pub_.publish(circles_msg_); -} - -void BallDetector::enableCallback(const std_msgs::Bool::ConstPtr &msg) { - enable_ = msg->data; -} - -void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr &msg) { - if (enable_ == false) - return; - - try { - if (msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - this->img_encoding_ = IMG_MONO; - if (msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) - this->img_encoding_ = IMG_RGB8; - this->cv_img_ptr_sub_ = cv_bridge::toCvCopy(msg, msg->encoding); - } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // indicates a new image is available - this->sub_time_ = msg->header.stamp; - this->image_frame_id_ = msg->header.frame_id; - this->new_image_flag_ = true; - return; -} - -void BallDetector::dynParamCallback( - humanoid_robot_intelligence_control_system_ball_detector:: - DetectorParamsConfig &config, - uint32_t level) { - params_config_.gaussian_blur_size = config.gaussian_blur_size; - params_config_.gaussian_blur_sigma = config.gaussian_blur_sigma; - params_config_.canny_edge_th = config.canny_edge_th; - params_config_.hough_accum_resolution = config.hough_accum_resolution; - params_config_.min_circle_dist = config.min_circle_dist; - params_config_.hough_accum_th = config.hough_accum_th; - params_config_.min_radius = config.min_radius; - params_config_.max_radius = config.max_radius; - params_config_.filter_threshold.h_min = config.filter_h_min; - params_config_.filter_threshold.h_max = config.filter_h_max; - params_config_.filter_threshold.s_min = config.filter_s_min; - params_config_.filter_threshold.s_max = config.filter_s_max; - params_config_.filter_threshold.v_min = config.filter_v_min; - params_config_.filter_threshold.v_max = config.filter_v_max; - params_config_.use_second_filter = config.use_second_filter; - params_config_.filter2_threshold.h_min = config.filter2_h_min; - params_config_.filter2_threshold.h_max = config.filter2_h_max; - params_config_.filter2_threshold.s_min = config.filter2_s_min; - params_config_.filter2_threshold.s_max = config.filter2_s_max; - params_config_.filter2_threshold.v_min = config.filter2_v_min; - params_config_.filter2_threshold.v_max = config.filter2_v_max; - params_config_.ellipse_size = config.ellipse_size; - params_config_.debug = config.debug_image; - - // gaussian_blur has to be odd number. - if (params_config_.gaussian_blur_size % 2 == 0) - params_config_.gaussian_blur_size -= 1; - if (params_config_.gaussian_blur_size <= 0) - params_config_.gaussian_blur_size = 1; - - printConfig(); - saveConfig(); -} - -void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo &msg) { - if (enable_ == false) - return; - - camera_info_msg_ = msg; -} - -void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg) { - if (msg->data == "debug") { - params_config_.debug = true; - saveConfig(); - } else if (msg->data == "normal") { - params_config_.debug = false; - saveConfig(); - } else if (msg->data == "reset") { - // load default parameters and apply - resetParameter(); - } -} - -bool BallDetector::setParamCallback( - humanoid_robot_intelligence_control_system_ball_detector::SetParameters:: - Request &req, - humanoid_robot_intelligence_control_system_ball_detector::SetParameters:: - Response &res) { - params_config_.gaussian_blur_size = req.params.gaussian_blur_size; - params_config_.gaussian_blur_sigma = req.params.gaussian_blur_sigma; - params_config_.canny_edge_th = req.params.canny_edge_th; - params_config_.hough_accum_resolution = req.params.hough_accum_resolution; - params_config_.min_circle_dist = req.params.min_circle_dist; - params_config_.hough_accum_th = req.params.hough_accum_th; - params_config_.min_radius = req.params.min_radius; - params_config_.max_radius = req.params.max_radius; - params_config_.filter_threshold.h_min = req.params.filter_h_min; - params_config_.filter_threshold.h_max = req.params.filter_h_max; - params_config_.filter_threshold.s_min = req.params.filter_s_min; - params_config_.filter_threshold.s_max = req.params.filter_s_max; - params_config_.filter_threshold.v_min = req.params.filter_v_min; - params_config_.filter_threshold.v_max = req.params.filter_v_max; - params_config_.ellipse_size = req.params.ellipse_size; - - saveConfig(); - - res.returns = req.params; - - return true; -} - -bool BallDetector::getParamCallback( - humanoid_robot_intelligence_control_system_ball_detector::GetParameters:: - Request &req, - humanoid_robot_intelligence_control_system_ball_detector::GetParameters:: - Response &res) { - res.returns.gaussian_blur_size = params_config_.gaussian_blur_size; - res.returns.gaussian_blur_sigma = params_config_.gaussian_blur_sigma; - res.returns.canny_edge_th = params_config_.canny_edge_th; - res.returns.hough_accum_resolution = params_config_.hough_accum_resolution; - res.returns.min_circle_dist = params_config_.min_circle_dist; - res.returns.hough_accum_th = params_config_.hough_accum_th; - res.returns.min_radius = params_config_.min_radius; - res.returns.max_radius = params_config_.max_radius; - res.returns.filter_h_min = params_config_.filter_threshold.h_min; - res.returns.filter_h_max = params_config_.filter_threshold.h_max; - res.returns.filter_s_min = params_config_.filter_threshold.s_min; - res.returns.filter_s_max = params_config_.filter_threshold.s_max; - res.returns.filter_v_min = params_config_.filter_threshold.v_min; - res.returns.filter_v_max = params_config_.filter_threshold.v_max; - res.returns.ellipse_size = params_config_.ellipse_size; - - return true; -} - -void BallDetector::resetParameter() { - - YAML::Node doc; - - try { - // load yaml - doc = YAML::LoadFile(default_setting_path_.c_str()); - - // parse - params_config_.gaussian_blur_size = doc["gaussian_blur_size"].as(); - params_config_.gaussian_blur_sigma = - doc["gaussian_blur_sigma"].as(); - params_config_.canny_edge_th = doc["canny_edge_th"].as(); - params_config_.hough_accum_resolution = - doc["hough_accum_resolution"].as(); - params_config_.min_circle_dist = doc["min_circle_dist"].as(); - params_config_.hough_accum_th = doc["hough_accum_th"].as(); - params_config_.min_radius = doc["min_radius"].as(); - params_config_.max_radius = doc["max_radius"].as(); - params_config_.filter_threshold.h_min = doc["filter_h_min"].as(); - params_config_.filter_threshold.h_max = doc["filter_h_max"].as(); - params_config_.filter_threshold.s_min = doc["filter_s_min"].as(); - params_config_.filter_threshold.s_max = doc["filter_s_max"].as(); - params_config_.filter_threshold.v_min = doc["filter_v_min"].as(); - params_config_.filter_threshold.v_max = doc["filter_v_max"].as(); - params_config_.use_second_filter = doc["use_second_filter"].as(); - params_config_.filter2_threshold.h_min = doc["filter2_h_min"].as(); - params_config_.filter2_threshold.h_max = doc["filter2_h_max"].as(); - params_config_.filter2_threshold.s_min = doc["filter2_s_min"].as(); - params_config_.filter2_threshold.s_max = doc["filter2_s_max"].as(); - params_config_.filter2_threshold.v_min = doc["filter2_v_min"].as(); - params_config_.filter2_threshold.v_max = doc["filter2_v_max"].as(); - params_config_.ellipse_size = doc["ellipse_size"].as(); - params_config_.debug = doc["filter_debug"].as(); - - // gaussian_blur has to be odd number. - if (params_config_.gaussian_blur_size % 2 == 0) - params_config_.gaussian_blur_size -= 1; - if (params_config_.gaussian_blur_size <= 0) - params_config_.gaussian_blur_size = 1; - - printConfig(); - saveConfig(); - - publishParam(); - } catch (const std::exception &e) { - ROS_ERROR_STREAM( - "Failed to Get default parameters : " << default_setting_path_); - return; - } -} - -void BallDetector::publishParam() { - humanoid_robot_intelligence_control_system_ball_detector::BallDetectorParams - params; - - params.gaussian_blur_size = params_config_.gaussian_blur_size; - params.gaussian_blur_sigma = params_config_.gaussian_blur_sigma; - params.canny_edge_th = params_config_.canny_edge_th; - params.hough_accum_resolution = params_config_.hough_accum_resolution; - params.min_circle_dist = params_config_.min_circle_dist; - params.hough_accum_th = params_config_.hough_accum_th; - params.min_radius = params_config_.min_radius; - params.max_radius = params_config_.max_radius; - params.filter_h_min = params_config_.filter_threshold.h_min; - params.filter_h_max = params_config_.filter_threshold.h_max; - params.filter_s_min = params_config_.filter_threshold.s_min; - params.filter_s_max = params_config_.filter_threshold.s_max; - params.filter_v_min = params_config_.filter_threshold.v_min; - params.filter_v_max = params_config_.filter_threshold.v_max; - params.ellipse_size = params_config_.ellipse_size; - - param_pub_.publish(params); -} - -void BallDetector::printConfig() { - if (init_param_ == false) - return; - - std::cout << "Detetctor Configuration:" << std::endl - << " gaussian_blur_size: " << params_config_.gaussian_blur_size - << std::endl - << " gaussian_blur_sigma: " << params_config_.gaussian_blur_sigma - << std::endl - << " canny_edge_th: " << params_config_.canny_edge_th - << std::endl - << " hough_accum_resolution: " - << params_config_.hough_accum_resolution << std::endl - << " min_circle_dist: " << params_config_.min_circle_dist - << std::endl - << " hough_accum_th: " << params_config_.hough_accum_th - << std::endl - << " min_radius: " << params_config_.min_radius << std::endl - << " max_radius: " << params_config_.max_radius << std::endl - << " filter_h_min: " << params_config_.filter_threshold.h_min - << std::endl - << " filter_h_max: " << params_config_.filter_threshold.h_max - << std::endl - << " filter_s_min: " << params_config_.filter_threshold.s_min - << std::endl - << " filter_s_max: " << params_config_.filter_threshold.s_max - << std::endl - << " filter_v_min: " << params_config_.filter_threshold.v_min - << std::endl - << " filter_v_max: " << params_config_.filter_threshold.v_max - << std::endl - << " use_second_filter: " << params_config_.use_second_filter - << std::endl - << " filter2_h_min: " << params_config_.filter2_threshold.h_min - << std::endl - << " filter2_h_max: " << params_config_.filter2_threshold.h_max - << std::endl - << " filter2_s_min: " << params_config_.filter2_threshold.s_min - << std::endl - << " filter2_s_max: " << params_config_.filter2_threshold.s_max - << std::endl - << " filter2_v_min: " << params_config_.filter2_threshold.v_min - << std::endl - << " filter2_v_max: " << params_config_.filter2_threshold.v_max - << std::endl - << " ellipse_size: " << params_config_.ellipse_size << std::endl - << " filter_image_to_debug: " << params_config_.debug - << std::endl - << std::endl; -} - -void BallDetector::saveConfig() { - if (has_path_ == false) - return; - - YAML::Emitter yaml_out; - - yaml_out << YAML::BeginMap; - yaml_out << YAML::Key << "gaussian_blur_size" << YAML::Value - << params_config_.gaussian_blur_size; - yaml_out << YAML::Key << "gaussian_blur_sigma" << YAML::Value - << params_config_.gaussian_blur_sigma; - yaml_out << YAML::Key << "canny_edge_th" << YAML::Value - << params_config_.canny_edge_th; - yaml_out << YAML::Key << "hough_accum_resolution" << YAML::Value - << params_config_.hough_accum_resolution; - yaml_out << YAML::Key << "min_circle_dist" << YAML::Value - << params_config_.min_circle_dist; - yaml_out << YAML::Key << "hough_accum_th" << YAML::Value - << params_config_.hough_accum_th; - yaml_out << YAML::Key << "min_radius" << YAML::Value - << params_config_.min_radius; - yaml_out << YAML::Key << "max_radius" << YAML::Value - << params_config_.max_radius; - yaml_out << YAML::Key << "filter_h_min" << YAML::Value - << params_config_.filter_threshold.h_min; - yaml_out << YAML::Key << "filter_h_max" << YAML::Value - << params_config_.filter_threshold.h_max; - yaml_out << YAML::Key << "filter_s_min" << YAML::Value - << params_config_.filter_threshold.s_min; - yaml_out << YAML::Key << "filter_s_max" << YAML::Value - << params_config_.filter_threshold.s_max; - yaml_out << YAML::Key << "filter_v_min" << YAML::Value - << params_config_.filter_threshold.v_min; - yaml_out << YAML::Key << "filter_v_max" << YAML::Value - << params_config_.filter_threshold.v_max; - yaml_out << YAML::Key << "use_second_filter" << YAML::Value - << params_config_.use_second_filter; - yaml_out << YAML::Key << "filter2_h_min" << YAML::Value - << params_config_.filter2_threshold.h_min; - yaml_out << YAML::Key << "filter2_h_max" << YAML::Value - << params_config_.filter2_threshold.h_max; - yaml_out << YAML::Key << "filter2_s_min" << YAML::Value - << params_config_.filter2_threshold.s_min; - yaml_out << YAML::Key << "filter2_s_max" << YAML::Value - << params_config_.filter2_threshold.s_max; - yaml_out << YAML::Key << "filter2_v_min" << YAML::Value - << params_config_.filter2_threshold.v_min; - yaml_out << YAML::Key << "filter2_v_max" << YAML::Value - << params_config_.filter2_threshold.v_max; - yaml_out << YAML::Key << "ellipse_size" << YAML::Value - << params_config_.ellipse_size; - yaml_out << YAML::Key << "filter_debug" << YAML::Value - << params_config_.debug; - yaml_out << YAML::EndMap; - - // output to file - std::ofstream fout(param_path_.c_str()); - fout << yaml_out.c_str(); -} - -void BallDetector::setInputImage(const cv::Mat &inIm) { - in_image_ = inIm.clone(); - - if (params_config_.debug == false) - out_image_ = in_image_.clone(); -} - -void BallDetector::setInputImage(const cv::Mat &inIm, cv::Mat &in_filter_img) { - cv::cvtColor(inIm, in_filter_img, cv::COLOR_RGB2HSV); - - if (params_config_.debug == false) - out_image_ = inIm.clone(); -} - -void BallDetector::getOutputImage(cv::Mat &outIm) { - this->drawOutputImage(); - outIm = out_image_.clone(); -} - -void BallDetector::filterImage() { - if (!in_image_.data) - return; - - cv::Mat img_hsv, img_filtered; - cv::cvtColor(in_image_, img_hsv, cv::COLOR_RGB2HSV); - - inRangeHsv(img_hsv, params_config_.filter_threshold, img_filtered); - - // mophology : open and close - mophology(img_filtered, img_filtered, params_config_.ellipse_size); - - if (params_config_.use_second_filter == true) { - // mask - cv::Mat img_mask; - - // check hsv range - cv::Mat img_filtered2; - inRangeHsv(img_hsv, params_config_.filter2_threshold, img_filtered2); - - makeFilterMaskFromBall(img_filtered, img_mask); - cv::bitwise_and(img_filtered2, img_mask, img_filtered2); - - // or - cv::bitwise_or(img_filtered, img_filtered2, img_filtered); - } - - mophology(img_filtered, img_filtered, params_config_.ellipse_size); - - cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB); -} - -void BallDetector::filterImage(const cv::Mat &in_filter_img, - cv::Mat &out_filter_img) { - if (!in_filter_img.data) - return; - - inRangeHsv(in_filter_img, params_config_.filter_threshold, out_filter_img); - - // mophology : open and close - mophology(out_filter_img, out_filter_img, params_config_.ellipse_size); - - if (params_config_.use_second_filter == true) { - // mask - cv::Mat img_mask; - - // check hsv range - cv::Mat img_filtered2; - inRangeHsv(in_filter_img, params_config_.filter2_threshold, img_filtered2); - - makeFilterMaskFromBall(out_filter_img, img_mask); - cv::bitwise_and(img_filtered2, img_mask, img_filtered2); - - // or - cv::bitwise_or(out_filter_img, img_filtered2, out_filter_img); - } - - mophology(out_filter_img, out_filter_img, params_config_.ellipse_size); - - // cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB); - - // draws results to output Image - if (params_config_.debug == true) - cv::cvtColor(out_filter_img, out_image_, cv::COLOR_GRAY2RGB); - // out_image_ = in_image_.clone(); -} - -void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, - int range) { - // source_img. - mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1); - - int source_height = source_img.rows; - int source_width = source_img.cols; - - // channel : 1 - if (source_img.channels() != 1) - return; - - for (int i = 0; i < source_height; i++) { - for (int j = 0; j < source_width; j++) { - uint8_t pixel = source_img.at(i, j); - - if (pixel == 0) - continue; - - for (int mask_i = i - range; mask_i <= i + range; mask_i++) { - if (mask_i < 0 || mask_i >= source_height) - continue; - - for (int mask_j = j - range; mask_j <= j + range; mask_j++) { - if (mask_j < 0 || mask_j >= source_width) - continue; - - mask_img.at(mask_i, mask_j, 0) = 255; - } - } - } - } -} - -void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, - cv::Mat &mask_img) { - // source_img. - mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1); - - if (circles_.size() == 0) - return; - - // channel : 1 - if (source_img.channels() != 1) - return; - - cv::Mat img_labels, stats, centroids; - int numOfLables = cv::connectedComponentsWithStats( - source_img, img_labels, stats, centroids, 8, CV_32S); - for (int j = 1; j < numOfLables; j++) { - int area = stats.at(j, cv::CC_STAT_AREA); - int left = stats.at(j, cv::CC_STAT_LEFT); - int top = stats.at(j, cv::CC_STAT_TOP); - int width = stats.at(j, cv::CC_STAT_WIDTH); - int height = stats.at(j, cv::CC_STAT_HEIGHT); - - int center_x = left + width * 0.5; - int center_y = top + height * 0.5; - int radius = (width + height) * 0.5; - - for (int mask_i = center_y - radius; mask_i <= center_y + radius; - mask_i++) { - if (mask_i < 0 || mask_i >= source_img.rows) - continue; - - int mask_offset = abs(mask_i - center_y) * 0.5; - - for (int mask_j = center_x - radius + mask_offset; - mask_j <= center_x + radius - mask_offset; mask_j++) { - if (mask_j < 0 || mask_j >= source_img.cols) - continue; - - mask_img.at(mask_i, mask_j, 0) = 255; - } - } - } -} - -void BallDetector::inRangeHsv(const cv::Mat &input_img, - const HsvFilter &filter_value, - cv::Mat &output_img) { - // 0-360 -> 0-180 - int scaled_hue_min = static_cast(filter_value.h_min * 0.5); - int scaled_hue_max = static_cast(filter_value.h_max * 0.5); - - if (scaled_hue_min <= scaled_hue_max) { - cv::Scalar min_value = - cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0); - cv::Scalar max_value = - cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0); - - cv::inRange(input_img, min_value, max_value, output_img); - } else { - cv::Mat lower_hue_range, upper_hue_range; - cv::Scalar min_value, max_value; - - min_value = cv::Scalar(0, filter_value.s_min, filter_value.v_min, 0); - max_value = - cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0); - cv::inRange(input_img, min_value, max_value, lower_hue_range); - - min_value = - cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0); - max_value = cv::Scalar(179, filter_value.s_max, filter_value.v_max, 0); - cv::inRange(input_img, min_value, max_value, upper_hue_range); - - cv::bitwise_or(lower_hue_range, upper_hue_range, output_img); - } -} - -void BallDetector::mophology(const cv::Mat &intput_img, cv::Mat &output_img, - int ellipse_size) { - cv::erode(intput_img, output_img, - cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(ellipse_size, ellipse_size))); - cv::dilate( - output_img, output_img, - cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(ellipse_size * 2, ellipse_size * 2))); - - cv::dilate(output_img, output_img, - cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(ellipse_size, ellipse_size))); - cv::erode(output_img, output_img, - cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(ellipse_size, ellipse_size))); -} - -void BallDetector::houghDetection(const unsigned int imgEncoding) { - cv::Mat gray_image; - std::vector circles_current; - std::vector prev_circles = circles_; - - // clear previous circles - circles_.clear(); - - // If input image is RGB, convert it to gray - if (imgEncoding == IMG_RGB8) - cv::cvtColor(in_image_, gray_image, CV_RGB2GRAY); - - // Reduce the noise so we avoid false circle detection - cv::GaussianBlur(gray_image, gray_image, - cv::Size(params_config_.gaussian_blur_size, - params_config_.gaussian_blur_size), - params_config_.gaussian_blur_sigma); - - double hough_accum_th = params_config_.hough_accum_th; - - // Apply the Hough Transform to find the circles - cv::HoughCircles(gray_image, circles_current, CV_HOUGH_GRADIENT, - params_config_.hough_accum_resolution, - params_config_.min_circle_dist, params_config_.canny_edge_th, - hough_accum_th, params_config_.min_radius, - params_config_.max_radius); - - if (circles_current.size() == 0) - not_found_count_ += 1; - else - not_found_count_ = 0; - - double alpha = 0.2; - - for (int ix = 0; ix < circles_current.size(); ix++) { - cv::Point2d center = - cv::Point(circles_current[ix][0], circles_current[ix][1]); - double radius = circles_current[ix][2]; - - for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++) { - cv::Point2d prev_center = - cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]); - double prev_radius = prev_circles[prev_ix][2]; - - cv::Point2d diff = center - prev_center; - double radius_th = std::max(radius, prev_radius) * 0.75; - - if (sqrt(diff.dot(diff)) < radius_th) { - if (abs(radius - prev_radius) < radius_th) { - circles_current[ix] = - circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha); - } - - break; - } - } - - circles_.push_back(circles_current[ix]); - } -} - -void BallDetector::houghDetection2(const cv::Mat &input_hough) { - // cv::Mat gray_image; - std::vector circles_current; - std::vector prev_circles = circles_; - - // clear previous circles - circles_.clear(); - - // If input image is RGB, convert it to gray - // if (imgEncoding == IMG_RGB8) - // cv::cvtColor(input_hough, gray_image, CV_RGB2GRAY); - - // Reduce the noise so we avoid false circle detection - cv::GaussianBlur(input_hough, input_hough, - cv::Size(params_config_.gaussian_blur_size, - params_config_.gaussian_blur_size), - params_config_.gaussian_blur_sigma); - - double hough_accum_th = params_config_.hough_accum_th; - - // Apply the Hough Transform to find the circles - cv::HoughCircles(input_hough, circles_current, CV_HOUGH_GRADIENT, - params_config_.hough_accum_resolution, - params_config_.min_circle_dist, params_config_.canny_edge_th, - hough_accum_th, params_config_.min_radius, - params_config_.max_radius); - - if (circles_current.size() == 0) - not_found_count_ += 1; - else - not_found_count_ = 0; - - double alpha = 0.2; - - for (int ix = 0; ix < circles_current.size(); ix++) { - cv::Point2d center = - cv::Point(circles_current[ix][0], circles_current[ix][1]); - double radius = circles_current[ix][2]; - - for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++) { - cv::Point2d prev_center = - cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]); - double prev_radius = prev_circles[prev_ix][2]; - - cv::Point2d diff = center - prev_center; - double radius_th = std::max(radius, prev_radius) * 0.75; - - if (sqrt(diff.dot(diff)) < radius_th) { - if (abs(radius - prev_radius) < radius_th) { - circles_current[ix] = - circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha); - } - - break; - } - } - - circles_.push_back(circles_current[ix]); - } -} - -void BallDetector::drawOutputImage() { - cv::Point center_position; - int radius = 0; - size_t ii; - - // draws results to output Image - // if (params_config_.debug == true) - // out_image_ = in_image_.clone(); - - for (ii = 0; ii < circles_.size(); ii++) { - { - int this_radius = cvRound(circles_[ii][2]); - if (this_radius > radius) { - radius = this_radius; - center_position = - cv::Point(cvRound(circles_[ii][0]), cvRound(circles_[ii][1])); - } - } - } - cv::circle(out_image_, center_position, 5, cv::Scalar(0, 0, 255), -1, 8, - 0); // circle center in blue - cv::circle(out_image_, center_position, radius, cv::Scalar(0, 0, 255), 3, 8, - 0); // circle outline in blue -} - -} // namespace humanoid_robot_intelligence_control_system_op diff --git a/humanoid_robot_intelligence_control_system_ball_detector/src/ball_detector_node.cpp b/humanoid_robot_intelligence_control_system_ball_detector/src/ball_detector_node.cpp deleted file mode 100644 index 8cecc3b..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/src/ball_detector_node.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_ball_detector/ball_detector.h" - -// node main -int main(int argc, char **argv) { - // init ros - ros::init(argc, argv, "ball_detector_node"); - - // create ros wrapper object - humanoid_robot_intelligence_control_system_op::BallDetector detector; - - // set node loop rate - ros::Rate loop_rate(30); - - // node loop - while (ros::ok()) { - // if new image , do things - if (detector.newImage()) { - detector.process(); - detector.publishImage(); - detector.publishCircles(); - } - - // execute pending callbacks - ros::spinOnce(); - - // relax to fit output rate - loop_rate.sleep(); - } - - // exit program - return 0; -} diff --git a/humanoid_robot_intelligence_control_system_ball_detector/srv/GetParameters.srv b/humanoid_robot_intelligence_control_system_ball_detector/srv/GetParameters.srv deleted file mode 100644 index eea6f98..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/srv/GetParameters.srv +++ /dev/null @@ -1,3 +0,0 @@ - ---- -BallDetectorParams returns diff --git a/humanoid_robot_intelligence_control_system_ball_detector/srv/SetParameters.srv b/humanoid_robot_intelligence_control_system_ball_detector/srv/SetParameters.srv deleted file mode 100644 index 0cb13ed..0000000 --- a/humanoid_robot_intelligence_control_system_ball_detector/srv/SetParameters.srv +++ /dev/null @@ -1,3 +0,0 @@ -BallDetectorParams params ---- -BallDetectorParams returns diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py new file mode 100644 index 0000000..9b89968 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py @@ -0,0 +1,76 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch commandi + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_bringup", "humanoid_robot_intelligence_control_system_bringup.launch"] + args + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + # Create a list to hold all nodes to be launched + nodes_to_launch = [] + + # Add the HUMANOID_ROBOT Manager launch file + nodes_to_launch.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + '$(find humanoid_robot_intelligence_control_system_manager)/launch/', + 'humanoid_robot_intelligence_control_system_manager.launch.py' + ]) + )) + + # Add the UVC camera node + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='usb_cam_node', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 1280, + 'image_height': 720, + 'framerate': 30, + 'camera_frame_id': 'cam_link', + 'camera_name': 'camera' + }] + )) + + # Return the LaunchDescription containing all nodes + return LaunchDescription(nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py new file mode 100644 index 0000000..ac4898f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py @@ -0,0 +1,85 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import FindExecutable, Command + + + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_bringup", "humanoid_robot_intelligence_control_system_bringup_visualization.launch"] + args + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + # Create a list to hold all nodes to be launched + nodes_to_launch = [] + + # Add robot description + nodes_to_launch.append(ExecuteProcess( + cmd=[FindExecutable(name='xacro'), '$(find humanoid_robot_intelligence_control_system_description)/urdf/humanoid_robot_intelligence_control_system.urdf.xacro'], + output='screen' + )) + + # Add joint state publisher + nodes_to_launch.append(Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'use_gui': True}], + remappings=[('/robot/joint_states', '/humanoid_robot_intelligence_control_system/present_joint_states')] + )) + + # Add robot state publisher + nodes_to_launch.append(Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + remappings=[('/joint_states', '/humanoid_robot_intelligence_control_system/present_joint_states')] + )) + + # Add rviz + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', '$(find humanoid_robot_intelligence_control_system_bringup)/rviz/humanoid_robot_intelligence_control_system_bringup.rviz'] + )) + + # Return the LaunchDescription containing all nodes + return LaunchDescription(nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch b/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup.launch similarity index 100% rename from humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch rename to humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup.launch diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch b/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup_visualization.launch similarity index 100% rename from humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch rename to humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup_visualization.launch diff --git a/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt new file mode 100644 index 0000000..7f45c8d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt @@ -0,0 +1,79 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_object_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + message_generation + ) + + generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + message_runtime + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + scripts/object_detection_processor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + scripts/object_detection_processor.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector.launch b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector.launch similarity index 100% rename from humanoid_robot_intelligence_control_system_ball_detector/launch/ball_detector.launch rename to humanoid_robot_intelligence_control_system_object_detector/launch/object_detector.launch diff --git a/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py new file mode 100644 index 0000000..e61d87a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_object_detection.py', + name='object_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch b/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_ball_detector/package.xml b/humanoid_robot_intelligence_control_system_object_detector/package.xml similarity index 96% rename from humanoid_robot_intelligence_control_system_ball_detector/package.xml rename to humanoid_robot_intelligence_control_system_object_detector/package.xml index d1d9c2c..5f82887 100644 --- a/humanoid_robot_intelligence_control_system_ball_detector/package.xml +++ b/humanoid_robot_intelligence_control_system_object_detector/package.xml @@ -16,12 +16,10 @@ the License. --> - humanoid_robot_intelligence_control_system_ball_detector - 0.3.2 + humanoid_robot_intelligence_control_system_object_detector + 0.0.1 - This package implements a circle-like shape detector of the input image - It requires and input image and publish, at frame rate, a marked image - and a stamped array of circle centers and radius. + This Package is for Object Detection, detecting objects like tools, or utilities Apache 2.0 Ronaldson Bellande diff --git a/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py b/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py new file mode 100644 index 0000000..6f44d68 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('object_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('object_detection_result', String, self.object_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def object_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + objects = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_objects(objects) + + def process_detected_objects(self, objects): + output_image = self.cv_image.copy() + for obj in objects: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in object_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch b/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch deleted file mode 100644 index 2893401..0000000 --- a/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py b/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py new file mode 100644 index 0000000..dbdd3ba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py @@ -0,0 +1,121 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_read_write_demo", "humanoid_robot_intelligence_control_system_read_write.launch"] + args + + # Add parameters + roslaunch_command.extend([ + "gazebo:=false", + "gazebo_robot_name:=humanoid_robot_intelligence_control_system", + "offset_file_path:=$(find humanoid_robot_intelligence_control_system_manager)/config/offset.yaml", + "robot_file_path:=$(find humanoid_robot_intelligence_control_system_manager)/config/HUMANOID_ROBOT.robot", + "init_file_path:=$(find humanoid_robot_intelligence_control_system_manager)/config/dxl_init_HUMANOID_ROBOT.yaml", + "device_name:=/dev/ttyUSB0", + "/humanoid_robot_intelligence_control_system/direct_control/default_moving_time:=0.04", + "/humanoid_robot_intelligence_control_system/direct_control/default_moving_angle:=90" + ]) + + # Add nodes + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_manager", "humanoid_robot_intelligence_control_system_manager", "angle_unit:=30", + "humanoid_robot_intelligence_control_system_localization", "humanoid_robot_intelligence_control_system_localization", + "humanoid_robot_intelligence_control_system_read_write_demo", "read_write" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + # Declare launch arguments + gazebo_arg = DeclareLaunchArgument('gazebo', default_value='false') + gazebo_robot_name_arg = DeclareLaunchArgument('gazebo_robot_name', default_value='humanoid_robot_intelligence_control_system') + offset_file_path_arg = DeclareLaunchArgument('offset_file_path', default_value='$(find humanoid_robot_intelligence_control_system_manager)/config/offset.yaml') + robot_file_path_arg = DeclareLaunchArgument('robot_file_path', default_value='$(find humanoid_robot_intelligence_control_system_manager)/config/HUMANOID_ROBOT.robot') + init_file_path_arg = DeclareLaunchArgument('init_file_path', default_value='$(find humanoid_robot_intelligence_control_system_manager)/config/dxl_init_HUMANOID_ROBOT.yaml') + device_name_arg = DeclareLaunchArgument('device_name', default_value='/dev/ttyUSB0') + + # Create a list to hold all nodes to be launched + nodes_to_launch = [] + + # Add HUMANOID_ROBOT Manager node + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_manager', + executable='humanoid_robot_intelligence_control_system_manager', + name='humanoid_robot_intelligence_control_system_manager', + output='screen', + parameters=[ + {'angle_unit': 30}, + {'gazebo': LaunchConfiguration('gazebo')}, + {'gazebo_robot_name': LaunchConfiguration('gazebo_robot_name')}, + {'offset_file_path': LaunchConfiguration('offset_file_path')}, + {'robot_file_path': LaunchConfiguration('robot_file_path')}, + {'init_file_path': LaunchConfiguration('init_file_path')}, + {'device_name': LaunchConfiguration('device_name')}, + {'/humanoid_robot_intelligence_control_system/direct_control/default_moving_time': 0.04}, + {'/humanoid_robot_intelligence_control_system/direct_control/default_moving_angle': 90} + ] + )) + + # Add HUMANOID_ROBOT Localization node + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_localization', + executable='humanoid_robot_intelligence_control_system_localization', + name='humanoid_robot_intelligence_control_system_localization', + output='screen' + )) + + # Add HUMANOID_ROBOT Read-Write demo node + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_read_write_demo', + executable='read_write', + name='humanoid_robot_intelligence_control_system_read_write', + output='screen' + )) + + # Return the LaunchDescription containing all nodes and arguments + return LaunchDescription([ + gazebo_arg, + gazebo_robot_name_arg, + offset_file_path_arg, + robot_file_path_arg, + init_file_path_arg, + device_name_arg + ] + nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch b/humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch new file mode 100644 index 0000000..8bf520d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/setup.py b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py index 723c5aa..317c74c 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/setup.py +++ b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py @@ -1,3 +1,18 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup