diff --git a/.robotis_op3_demo.rosinstall b/.robotis_op3_demo.rosinstall new file mode 100644 index 0000000..146fa30 --- /dev/null +++ b/.robotis_op3_demo.rosinstall @@ -0,0 +1,2 @@ +- git: {local-name: robotis_op3_tools, uri: 'https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Tools.git', version: master} +- git: {local-name: humanoid_navigation, uri: 'https://github.com/ROBOTIS-GIT/humanoid_navigation.git', version: master} \ No newline at end of file diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..1e400fd --- /dev/null +++ b/.travis.yml @@ -0,0 +1,35 @@ +# 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: + - "2.7" +compiler: + - gcc +notifications: + email: + on_success: change + on_failure: always + recipients: + - pyo@robotis.com +env: + matrix: + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=xenial +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_op3_demo.rosinstall" +# - ROS_DISTRO=melodic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=bionic +# - ROS_DISTRO=melodic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=stretch +branches: + only: + - master + - develop + - kinetic-devel +install: + - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config +script: + - source .ci_config/travis.sh + \ No newline at end of file diff --git a/LICENSE b/LICENSE index 5298325..c0ee812 100644 --- a/LICENSE +++ b/LICENSE @@ -1,26 +1,201 @@ -Software License Agreement (BSD License) - -Copyright (c) 2014, ROBOTIS Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of ROBOTIS nor the names of its contributors may be - used to endorse or promote products derived from this software - without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY ROBOTIS "AS IS" AND ANY EXPRESS OR IMPLIED -WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. -IN NO EVENT SHALL ROBOTIS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; -OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR -OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF -ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + 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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..cd34832 --- /dev/null +++ b/README.md @@ -0,0 +1,36 @@ +# ROBOTIS OP3 + + +## ROS Packages for ROBOTIS OP3 Demo +|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:| +|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo)|-| + +## ROBOTIS e-Manual for ROBOTIS OP3 +- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) + +## Wiki for robotis_op3_demo Packages +- http://wiki.ros.org/robotis_op3_demo (metapackage) +- http://wiki.ros.org/op3_ball_detector +- http://wiki.ros.org/op3_bringup +- http://wiki.ros.org/op3_demo + +## Open Source related to ROBOTIS OP3 +- [robotis_op3](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3) +- [robotis_op3_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-msgs) +- [robotis_op3_common](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Common) +- [robotis_op3_tools](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Tools) +- [robotis_op3_demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) +- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) +- [robotis_utility](https://github.com/ROBOTIS-GIT/ROBOTIS-Utility) +- [robotis_math](https://github.com/ROBOTIS-GIT/ROBOTIS-Math) +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) +- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) + +## Documents and Videos related to ROBOTIS OP3 +- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) +- [ROBOTIS e-Manual for ROBOTIS THORMANG3](http://emanual.robotis.com/docs/en/platform/thormang3/introduction/) +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) diff --git a/ball_detector/CMakeLists.txt b/ball_detector/CMakeLists.txt deleted file mode 100644 index a25b556..0000000 --- a/ball_detector/CMakeLists.txt +++ /dev/null @@ -1,105 +0,0 @@ -################################################################################ -# CMake -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(ball_detector) - -################################################################################ -# Packages -################################################################################ -find_package(catkin REQUIRED COMPONENTS - cv_bridge - geometry_msgs - image_transport - roscpp - rospy - std_msgs - dynamic_reconfigure - message_generation -) - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ -add_message_files( - FILES - circleSetStamped.msg -) - -generate_messages( - DEPENDENCIES - geometry_msgs std_msgs -) - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ -generate_dynamic_reconfigure_options(cfg/detector_params.cfg) - - -################################################################################ -# Catkin specific configuration -################################################################################ -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS cv_bridge geometry_msgs image_transport roscpp rospy std_msgs dynamic_reconfigure -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(ball_detector_node - src/ball_detector.cpp - src/ball_detector_node.cpp) - -add_dependencies(ball_detector_node ${PROJECT_NAME}_gencfg) - -target_link_libraries(ball_detector_node - ${catkin_LIBRARIES} - yaml-cpp -) - -################################################################################ -# Install -################################################################################ - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ball_detector ball_detector_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - - -################################################################################ -# Test -################################################################################ \ No newline at end of file diff --git a/ball_detector/launch/ball_detector.launch b/ball_detector/launch/ball_detector.launch deleted file mode 100644 index 05cc278..0000000 --- a/ball_detector/launch/ball_detector.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/ball_detector/launch/ball_detector_from_usb_cam.launch b/ball_detector/launch/ball_detector_from_usb_cam.launch deleted file mode 100644 index 2ae61c6..0000000 --- a/ball_detector/launch/ball_detector_from_usb_cam.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ball_detector/launch/ball_detector_from_uvc.launch b/ball_detector/launch/ball_detector_from_uvc.launch deleted file mode 100644 index 4ef7b43..0000000 --- a/ball_detector/launch/ball_detector_from_uvc.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ball_detector/package.xml b/ball_detector/package.xml deleted file mode 100644 index 62618a7..0000000 --- a/ball_detector/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - ball_detector - 0.1.0 - - 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. - - BSD - kayman - Pyo - - catkin - cv_bridge - geometry_msgs - image_transport - roscpp - rospy - std_msgs - dynamic_reconfigure - message_generation - cv_bridge - geometry_msgs - image_transport - roscpp - rospy - std_msgs - dynamic_reconfigure - message_runtime - diff --git a/ball_detector/src/ball_detector_node.cpp b/ball_detector/src/ball_detector_node.cpp deleted file mode 100644 index d54bb4e..0000000 --- a/ball_detector/src/ball_detector_node.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "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 - robotis_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/op3_ball_detector/CHANGELOG.rst b/op3_ball_detector/CHANGELOG.rst new file mode 100644 index 0000000..d00eb27 --- /dev/null +++ b/op3_ball_detector/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package op3_ball_detector +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2018-04-19) +------------------ +* first release for ROS Kinetic +* added launch files in order to move the camera setting to op3_camera_setting package +* added missing package in find_package() +* refacoring to release +* split repositoryfrom ROBOTIS-OP3 +* Contributors: Kayman, Zerom, Pyo diff --git a/op3_ball_detector/CMakeLists.txt b/op3_ball_detector/CMakeLists.txt new file mode 100644 index 0000000..9a1693d --- /dev/null +++ b/op3_ball_detector/CMakeLists.txt @@ -0,0 +1,136 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(op3_ball_detector) + +################################################################################ +# Find catkin packages and libraries for catkin and system dependencies +################################################################################ +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 3 REQUIRED) + +## Resolve system dependency on yaml-cpp, which apparently does not +## provide a CMake find_package() module. +## Insert your header file compatible specified path like '#include ' +find_package(PkgConfig REQUIRED) +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") + +################################################################################ +# Setup for python modules and scripts +################################################################################ + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ +add_message_files( + FILES + CircleSetStamped.msg + BallDetectorParams.msg +) + +add_service_files( + FILES + GetParameters.srv + SetParameters.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ +generate_dynamic_reconfigure_options( + cfg/DetectorParams.cfg +) + +################################################################################ +# Declare catkin specific configuration to be passed to dependent projects +################################################################################## +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 +) + +################################################################################ +# Build +################################################################################ +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 +################################################################################ +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} +) + +################################################################################ +# Test +################################################################################ diff --git a/ball_detector/cfg/detector_params.cfg b/op3_ball_detector/cfg/DetectorParams.cfg old mode 100644 new mode 100755 similarity index 95% rename from ball_detector/cfg/detector_params.cfg rename to op3_ball_detector/cfg/DetectorParams.cfg index 2d61d6c..e627907 --- a/ball_detector/cfg/detector_params.cfg +++ b/op3_ball_detector/cfg/DetectorParams.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python -PACKAGE='ball_detector' +PACKAGE='op3_ball_detector' from dynamic_reconfigure.parameter_generator_catkin import * @@ -30,4 +30,4 @@ 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")) +exit(gen.generate(PACKAGE, "ball_detector_node", "DetectorParams")) diff --git a/ball_detector/cfg/detector_params_blue.cfg b/op3_ball_detector/cfg/DetectorParamsBlue.cfg old mode 100644 new mode 100755 similarity index 95% rename from ball_detector/cfg/detector_params_blue.cfg rename to op3_ball_detector/cfg/DetectorParamsBlue.cfg index 2d61d6c..391e99f --- a/ball_detector/cfg/detector_params_blue.cfg +++ b/op3_ball_detector/cfg/DetectorParamsBlue.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python -PACKAGE='ball_detector' +PACKAGE='op3_ball_detector' from dynamic_reconfigure.parameter_generator_catkin import * @@ -30,4 +30,4 @@ 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")) +exit(gen.generate(PACKAGE, "ball_detector_node", "DetectorParamsBlue")) diff --git a/ball_detector/cfg/detector_params_red.cfg b/op3_ball_detector/cfg/DetectorParamsRed.cfg old mode 100644 new mode 100755 similarity index 95% rename from ball_detector/cfg/detector_params_red.cfg rename to op3_ball_detector/cfg/DetectorParamsRed.cfg index 4fa6a18..6d8dbad --- a/ball_detector/cfg/detector_params_red.cfg +++ b/op3_ball_detector/cfg/DetectorParamsRed.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python -PACKAGE='ball_detector' +PACKAGE='op3_ball_detector' from dynamic_reconfigure.parameter_generator_catkin import * @@ -30,4 +30,4 @@ 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", "detectorParams")) +exit(gen.generate(PACKAGE, "ball_detector_node", "DetectorParamsRed")) diff --git a/ball_detector/launch/ball_detector_params.yaml b/op3_ball_detector/config/ball_detector_params.yaml similarity index 67% rename from ball_detector/launch/ball_detector_params.yaml rename to op3_ball_detector/config/ball_detector_params.yaml index 86bd8e9..49cc203 100644 --- a/ball_detector/launch/ball_detector_params.yaml +++ b/op3_ball_detector/config/ball_detector_params.yaml @@ -7,17 +7,17 @@ hough_accum_th: 28 min_radius: 20 max_radius: 300 filter_h_min: 350 -filter_h_max: 20 -filter_s_min: 230 +filter_h_max: 15 +filter_s_min: 200 filter_s_max: 255 -filter_v_min: 30 +filter_v_min: 60 filter_v_max: 255 use_second_filter: false -filter2_h_min: 40 -filter2_h_max: 345 +filter2_h_min: 30 +filter2_h_max: 355 filter2_s_min: 0 -filter2_s_max: 44 -filter2_v_min: 90 +filter2_s_max: 40 +filter2_v_min: 200 filter2_v_max: 255 -ellipse_size: 3 +ellipse_size: 2 filter_debug: false \ No newline at end of file diff --git a/ball_detector/launch/ball_detector_params_default.yaml b/op3_ball_detector/config/ball_detector_params_default.yaml similarity index 52% rename from ball_detector/launch/ball_detector_params_default.yaml rename to op3_ball_detector/config/ball_detector_params_default.yaml index 7310922..49cc203 100644 --- a/ball_detector/launch/ball_detector_params_default.yaml +++ b/op3_ball_detector/config/ball_detector_params_default.yaml @@ -1,23 +1,23 @@ gaussian_blur_size: 7 -gaussian_blur_sigma: 2.52 -canny_edge_th: 100.5 +gaussian_blur_sigma: 2 +canny_edge_th: 100 hough_accum_resolution: 1 -min_circle_dist: 28.5 -hough_accum_th: 26.6 -min_radius: 25 -max_radius: 150 +min_circle_dist: 100 +hough_accum_th: 28 +min_radius: 20 +max_radius: 300 filter_h_min: 350 -filter_h_max: 20 -filter_s_min: 90 +filter_h_max: 15 +filter_s_min: 200 filter_s_max: 255 -filter_v_min: 86 +filter_v_min: 60 filter_v_max: 255 -use_second_filter: true +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: 1 +ellipse_size: 2 filter_debug: false \ No newline at end of file diff --git a/ball_detector/launch/ball_detector_params_op.yaml b/op3_ball_detector/config/ball_detector_params_op.yaml similarity index 100% rename from ball_detector/launch/ball_detector_params_op.yaml rename to op3_ball_detector/config/ball_detector_params_op.yaml diff --git a/ball_detector/include/ball_detector/ball_detector.h b/op3_ball_detector/include/op3_ball_detector/ball_detector.h similarity index 58% rename from ball_detector/include/ball_detector/ball_detector.h rename to op3_ball_detector/include/op3_ball_detector/ball_detector.h index 578d9be..c429222 100644 --- a/ball_detector/include/ball_detector/ball_detector.h +++ b/op3_ball_detector/include/op3_ball_detector/ball_detector.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -35,21 +21,27 @@ #include -#include -#include - -//ros dependencies #include +#include #include -#include -#include +#include #include #include #include +#include +#include -#include "ball_detector/circleSetStamped.h" -#include "ball_detector/ball_detector_config.h" -#include "ball_detector/detectorParamsConfig.h" +#include +#include +#include +#include + +#include "op3_ball_detector/ball_detector_config.h" + +#include "op3_ball_detector/DetectorParamsConfig.h" +#include "op3_ball_detector/CircleSetStamped.h" +#include "op3_ball_detector/GetParameters.h" +#include "op3_ball_detector/SetParameters.h" namespace robotis_op { @@ -81,19 +73,28 @@ class BallDetector //callbacks to camera info subscription void cameraInfoCallback(const sensor_msgs::CameraInfo & msg); - void dynParamCallback(ball_detector::detectorParamsConfig &config, uint32_t level); + void dynParamCallback(op3_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(op3_ball_detector::SetParameters::Request &req, op3_ball_detector::SetParameters::Response &res); + bool getParamCallback(op3_ball_detector::GetParameters::Request &req, op3_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 @@ -113,7 +114,7 @@ class BallDetector int not_found_count_; //circle set publisher - ball_detector::circleSetStamped circles_msg_; + op3_ball_detector::CircleSetStamped circles_msg_; ros::Publisher circles_pub_; //camera info subscriber @@ -126,6 +127,13 @@ class BallDetector 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_; @@ -148,8 +156,8 @@ class BallDetector cv::Mat in_image_; cv::Mat out_image_; - dynamic_reconfigure::Server param_server_; - dynamic_reconfigure::Server::CallbackType callback_fnc_; + dynamic_reconfigure::Server param_server_; + dynamic_reconfigure::Server::CallbackType callback_fnc_; }; } // namespace robotis_op diff --git a/ball_detector/include/ball_detector/ball_detector_config.h b/op3_ball_detector/include/op3_ball_detector/ball_detector_config.h similarity index 62% rename from ball_detector/include/ball_detector/ball_detector_config.h rename to op3_ball_detector/include/op3_ball_detector/ball_detector_config.h index 7182697..6062145 100644 --- a/ball_detector/include/ball_detector/ball_detector_config.h +++ b/op3_ball_detector/include/op3_ball_detector/ball_detector_config.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ diff --git a/op3_ball_detector/launch/ball_detector.launch b/op3_ball_detector/launch/ball_detector.launch new file mode 100644 index 0000000..35f57d9 --- /dev/null +++ b/op3_ball_detector/launch/ball_detector.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/op3_ball_detector/launch/ball_detector_from_usb_cam.launch b/op3_ball_detector/launch/ball_detector_from_usb_cam.launch new file mode 100644 index 0000000..7f3ae14 --- /dev/null +++ b/op3_ball_detector/launch/ball_detector_from_usb_cam.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/op3_ball_detector/launch/ball_detector_from_uvc.launch b/op3_ball_detector/launch/ball_detector_from_uvc.launch new file mode 100644 index 0000000..636caaa --- /dev/null +++ b/op3_ball_detector/launch/ball_detector_from_uvc.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/op3_ball_detector/msg/BallDetectorParams.msg b/op3_ball_detector/msg/BallDetectorParams.msg new file mode 100644 index 0000000..cd1c210 --- /dev/null +++ b/op3_ball_detector/msg/BallDetectorParams.msg @@ -0,0 +1,17 @@ +# 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/ball_detector/msg/circleSetStamped.msg b/op3_ball_detector/msg/CircleSetStamped.msg similarity index 100% rename from ball_detector/msg/circleSetStamped.msg rename to op3_ball_detector/msg/CircleSetStamped.msg diff --git a/op3_ball_detector/package.xml b/op3_ball_detector/package.xml new file mode 100644 index 0000000..d507794 --- /dev/null +++ b/op3_ball_detector/package.xml @@ -0,0 +1,35 @@ + + + op3_ball_detector + 0.1.0 + + 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. + + Apache 2.0 + Kayman + Zerom + Pyo + http://wiki.ros.org/op3_ball_detector + http://emanual.robotis.com/docs/en/platform/op3/introduction/ + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + dynamic_reconfigure + cv_bridge + image_transport + boost + opencv3 + yaml-cpp + message_generation + message_runtime + message_runtime + usb_cam + uvc_camera + diff --git a/ball_detector/launch/ball_detector.rviz b/op3_ball_detector/rviz/ball_detector.rviz similarity index 100% rename from ball_detector/launch/ball_detector.rviz rename to op3_ball_detector/rviz/ball_detector.rviz diff --git a/ball_detector/src/ball_detector.cpp b/op3_ball_detector/src/ball_detector.cpp similarity index 63% rename from ball_detector/src/ball_detector.cpp rename to op3_ball_detector/src/ball_detector.cpp index 77b00b9..871a1e8 100644 --- a/ball_detector/src/ball_detector.cpp +++ b/op3_ball_detector/src/ball_detector.cpp @@ -1,50 +1,35 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 -#include "ball_detector/ball_detector.h" +#include "op3_ball_detector/ball_detector.h" namespace robotis_op { BallDetector::BallDetector() - : nh_(ros::this_node::getName()), - it_(this->nh_), - enable_(true), - params_config_(), - init_param_(false), - not_found_count_(0) + : 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_); @@ -86,7 +71,7 @@ BallDetector::BallDetector() //sets publishers image_pub_ = it_.advertise("image_out", 100); - circles_pub_ = nh_.advertise("circle_set", 100); + circles_pub_ = nh_.advertise("circle_set", 100); camera_info_pub_ = nh_.advertise("camera_info", 100); //sets subscribers @@ -101,6 +86,13 @@ BallDetector::BallDetector() 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; @@ -132,14 +124,25 @@ void BallDetector::process() if (cv_img_ptr_sub_ != NULL) { - //sets input image - setInputImage(cv_img_ptr_sub_->image); + cv::Mat img_hsv, img_filtered; - // test image filtering - filterImage(); + // set input image + setInputImage(cv_img_ptr_sub_->image, img_hsv); + + // image filtering + filterImage(img_hsv, img_filtered); //detect circles - houghDetection(this->img_encoding_); + houghDetection2(img_filtered); + +// // set input image +// setInputImage(cv_img_ptr_sub_->image); + +// // image filtering +// filterImage(); + +// //detect circles +// houghDetection(this->img_encoding_); } } @@ -154,15 +157,15 @@ void BallDetector::publishImage() 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; + 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()); @@ -191,8 +194,8 @@ void BallDetector::publishCircles() // left(-1), right(+1) for (int idx = 0; idx < circles_.size(); idx++) { - circles_msg_.circles[idx].x = circles_[idx][0] / in_image_.cols * 2 - 1; // x (-1 ~ 1) - circles_msg_.circles[idx].y = circles_[idx][1] / in_image_.rows * 2 - 1; // y (-1 ~ 1) + 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 } @@ -230,7 +233,7 @@ void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr & msg) return; } -void BallDetector::dynParamCallback(ball_detector::detectorParamsConfig &config, uint32_t level) +void BallDetector::dynParamCallback(op3_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; @@ -274,6 +277,146 @@ void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo & msg) 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(op3_ball_detector::SetParameters::Request &req, op3_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(op3_ball_detector::GetParameters::Request &req, op3_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() +{ + op3_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) @@ -349,6 +492,14 @@ void BallDetector::setInputImage(const cv::Mat & inIm) 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(); @@ -389,6 +540,42 @@ void BallDetector::filterImage() 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. @@ -574,6 +761,68 @@ void BallDetector::houghDetection(const unsigned int imgEncoding) } } +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; @@ -581,8 +830,8 @@ void BallDetector::drawOutputImage() size_t ii; //draws results to output Image - if (params_config_.debug == true) - out_image_ = in_image_.clone(); +// if (params_config_.debug == true) +// out_image_ = in_image_.clone(); for (ii = 0; ii < circles_.size(); ii++) { diff --git a/op3_ball_detector/src/ball_detector_node.cpp b/op3_ball_detector/src/ball_detector_node.cpp new file mode 100644 index 0000000..117127d --- /dev/null +++ b/op3_ball_detector/src/ball_detector_node.cpp @@ -0,0 +1,55 @@ +/******************************************************************************* +* 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 "op3_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 + robotis_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/op3_ball_detector/srv/GetParameters.srv b/op3_ball_detector/srv/GetParameters.srv new file mode 100644 index 0000000..eea6f98 --- /dev/null +++ b/op3_ball_detector/srv/GetParameters.srv @@ -0,0 +1,3 @@ + +--- +BallDetectorParams returns diff --git a/op3_ball_detector/srv/SetParameters.srv b/op3_ball_detector/srv/SetParameters.srv new file mode 100644 index 0000000..0cb13ed --- /dev/null +++ b/op3_ball_detector/srv/SetParameters.srv @@ -0,0 +1,3 @@ +BallDetectorParams params +--- +BallDetectorParams returns diff --git a/op3_bringup/CHANGELOG.rst b/op3_bringup/CHANGELOG.rst new file mode 100644 index 0000000..bcad6b2 --- /dev/null +++ b/op3_bringup/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package op3_bringup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2018-04-19) +------------------ +* first release for ROS Kinetic +* updated CMakeLists.txt and package.xml of op3_bringup +* changed rviz config file +* refacoring to release +* Contributors: Kayman, Pyo diff --git a/op3_bringup/CMakeLists.txt b/op3_bringup/CMakeLists.txt new file mode 100644 index 0000000..b171b00 --- /dev/null +++ b/op3_bringup/CMakeLists.txt @@ -0,0 +1,42 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(op3_bringup) + +################################################################################ +# Find catkin packages and libraries for catkin and system dependencies +################################################################################ +find_package(catkin REQUIRED) + +################################################################################ +# Setup for python modules and scripts +################################################################################ + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Declare catkin specific configuration to be passed to dependent projects +################################################################################ +catkin_package() + +################################################################################ +# Build +################################################################################ + +################################################################################ +# Install +################################################################################ +install(DIRECTORY launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ \ No newline at end of file diff --git a/op3_bringup/launch/op3_bringup.launch b/op3_bringup/launch/op3_bringup.launch new file mode 100644 index 0000000..a3eacfb --- /dev/null +++ b/op3_bringup/launch/op3_bringup.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/op3_bringup/launch/op3_bringup_visualization.launch b/op3_bringup/launch/op3_bringup_visualization.launch new file mode 100644 index 0000000..5ad06e0 --- /dev/null +++ b/op3_bringup/launch/op3_bringup_visualization.launch @@ -0,0 +1,18 @@ + + + + + + + + [/robotis/present_joint_states] + + + + + + + + + + diff --git a/op3_bringup/package.xml b/op3_bringup/package.xml new file mode 100644 index 0000000..3161ae0 --- /dev/null +++ b/op3_bringup/package.xml @@ -0,0 +1,23 @@ + + + op3_bringup + 0.1.0 + + This package is a demo for first time users. + There is an example in the demo where you can run and visualize the robot. + + Apache 2.0 + Kayman + Pyo + http://wiki.ros.org/op3_bringup + http://emanual.robotis.com/docs/en/platform/op3/introduction/ + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + op3_manager + op3_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + diff --git a/op3_bringup/rviz/op3_bringup.rviz b/op3_bringup/rviz/op3_bringup.rviz new file mode 100644 index 0000000..cffe130 --- /dev/null +++ b/op3_bringup/rviz/op3_bringup.rviz @@ -0,0 +1,357 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 352 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - 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.0299999993 + 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: Links in Alphabetic Order + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cam_gazebo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ank_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ank_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_el_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_knee_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sho_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sho_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ank_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ank_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_el_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_knee_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sho_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sho_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Image + Enabled: true + Image Topic: /usb_cam_node/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + body_link: + Value: true + cam_gazebo_link: + Value: true + cam_link: + Value: true + head_pan_link: + Value: true + head_tilt_link: + Value: true + l_ank_pitch_link: + Value: true + l_ank_roll_link: + Value: true + l_el_link: + Value: true + l_hip_pitch_link: + Value: true + l_hip_roll_link: + Value: true + l_hip_yaw_link: + Value: true + l_knee_link: + Value: true + l_sho_pitch_link: + Value: true + l_sho_roll_link: + Value: true + r_ank_pitch_link: + Value: true + r_ank_roll_link: + Value: true + r_el_link: + Value: true + r_hip_pitch_link: + Value: true + r_hip_roll_link: + Value: true + r_hip_yaw_link: + Value: true + r_knee_link: + Value: true + r_sho_pitch_link: + Value: true + r_sho_roll_link: + Value: true + world: + Value: true + Marker Scale: 0.200000003 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + world: + body_link: + head_pan_link: + head_tilt_link: + cam_gazebo_link: + {} + cam_link: + {} + l_hip_yaw_link: + l_hip_roll_link: + l_hip_pitch_link: + l_knee_link: + l_ank_pitch_link: + l_ank_roll_link: + {} + l_sho_pitch_link: + l_sho_roll_link: + l_el_link: + {} + r_hip_yaw_link: + r_hip_roll_link: + r_hip_pitch_link: + r_knee_link: + r_ank_pitch_link: + r_ank_roll_link: + {} + r_sho_pitch_link: + r_sho_roll_link: + r_el_link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.200000003 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: true + History Length: 1 + Name: Imu + Topic: /robotis/open_cr/imu + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: body_link + 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: 2.32116389 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.275397927 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.68858385 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000023300000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000001f1000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000023a000001610000001800ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000358000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000022400fffffffb0000000800540069006d00650100000000000004500000000000000000000005470000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 0 diff --git a/op3_demo/CHANGELOG.rst b/op3_demo/CHANGELOG.rst new file mode 100644 index 0000000..c28f305 --- /dev/null +++ b/op3_demo/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package op3_demo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2018-04-19) +------------------ +* first release for ROS Kinetic +* added launch files in order to move the camera setting to op3_camera_setting package +* added missing package in find_package() +* refacoring to release +* split repositoryfrom ROBOTIS-OP3 +* Contributors: Kayman, Zerom, Yoshimaru Tanaka, Pyo diff --git a/op3_demo/CMakeLists.txt b/op3_demo/CMakeLists.txt index dc1fdea..381f872 100644 --- a/op3_demo/CMakeLists.txt +++ b/op3_demo/CMakeLists.txt @@ -1,24 +1,51 @@ ################################################################################ -# CMake +# Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 2.8.3) project(op3_demo) ################################################################################ -# Packages +# Find catkin packages and libraries for catkin and system dependencies ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp roslib + std_msgs sensor_msgs - ball_detector + geometry_msgs + robotis_controller_msgs op3_walking_module_msgs + op3_action_module_msgs cmake_modules robotis_math + op3_ball_detector ) -find_package(Eigen REQUIRED) +find_package(Boost REQUIRED COMPONENTS thread) +find_package(Eigen3 REQUIRED) +## Resolve system dependency on yaml-cpp, which apparently does not +## provide a CMake find_package() module. +## Insert your header file compatible specified path like '#include ' +find_package(PkgConfig REQUIRED) +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") + +################################################################################ +# Setup for python modules and scripts +################################################################################ ################################################################################ # Declare ROS messages, services and actions @@ -29,11 +56,23 @@ find_package(Eigen REQUIRED) ################################################################################ ################################################################################ -# Catkin specific configuration +# Declare catkin specific configuration to be passed to dependent projects ################################################################################ catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp sensor_msgs + CATKIN_DEPENDS + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + robotis_controller_msgs + op3_walking_module_msgs + op3_action_module_msgs + cmake_modules + robotis_math + op3_ball_detector + DEPENDS Boost EIGEN3 ) ################################################################################ @@ -42,7 +81,9 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} ) add_executable(op_demo_node @@ -55,11 +96,16 @@ add_executable(op_demo_node src/vision/face_tracker.cpp ) -add_dependencies(op_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(op_demo_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(op_demo_node ${catkin_LIBRARIES} - yaml-cpp + ${Boost_LIBRARIES} + ${Eigen3_LIBRARIES} + ${YAML_CPP_LIBRARIES} ) add_executable(self_test_node @@ -74,48 +120,33 @@ add_executable(self_test_node src/test/mic_test.cpp ) -add_dependencies(self_test_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(self_test_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(self_test_node ${catkin_LIBRARIES} - yaml-cpp + ${Boost_LIBRARIES} + ${Eigen3_LIBRARIES} + ${YAML_CPP_LIBRARIES} ) ################################################################################ # Install ################################################################################ +install(TARGETS op_demo_node self_test_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ball_tracking ball_tracking_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) +install(DIRECTORY data launch list + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) ################################################################################ # Test -################################################################################ \ No newline at end of file +################################################################################ diff --git a/op3_demo/Data/mp3/Autonomous soccer mode.mp3 b/op3_demo/Data/mp3/Autonomous soccer mode.mp3 deleted file mode 100644 index ef1f878..0000000 Binary files a/op3_demo/Data/mp3/Autonomous soccer mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Bye bye.mp3 b/op3_demo/Data/mp3/Bye bye.mp3 deleted file mode 100644 index c853dca..0000000 Binary files a/op3_demo/Data/mp3/Bye bye.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Clap please.mp3 b/op3_demo/Data/mp3/Clap please.mp3 deleted file mode 100644 index 5dcaf62..0000000 Binary files a/op3_demo/Data/mp3/Clap please.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Demonstration ready mode.mp3 b/op3_demo/Data/mp3/Demonstration ready mode.mp3 deleted file mode 100644 index 6f73a75..0000000 Binary files a/op3_demo/Data/mp3/Demonstration ready mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Headstand.mp3 b/op3_demo/Data/mp3/Headstand.mp3 deleted file mode 100644 index 6864057..0000000 Binary files a/op3_demo/Data/mp3/Headstand.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Interactive motion mode.mp3 b/op3_demo/Data/mp3/Interactive motion mode.mp3 deleted file mode 100644 index db2f2c9..0000000 Binary files a/op3_demo/Data/mp3/Interactive motion mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Intro01.mp3 b/op3_demo/Data/mp3/Intro01.mp3 deleted file mode 100644 index 60a5051..0000000 Binary files a/op3_demo/Data/mp3/Intro01.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Intro02.mp3 b/op3_demo/Data/mp3/Intro02.mp3 deleted file mode 100644 index b1c745c..0000000 Binary files a/op3_demo/Data/mp3/Intro02.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Intro03.mp3 b/op3_demo/Data/mp3/Intro03.mp3 deleted file mode 100644 index cc30740..0000000 Binary files a/op3_demo/Data/mp3/Intro03.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Introduction.mp3 b/op3_demo/Data/mp3/Introduction.mp3 deleted file mode 100644 index 87c699d..0000000 Binary files a/op3_demo/Data/mp3/Introduction.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Left kick.mp3 b/op3_demo/Data/mp3/Left kick.mp3 deleted file mode 100644 index 142b79c..0000000 Binary files a/op3_demo/Data/mp3/Left kick.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/No.mp3 b/op3_demo/Data/mp3/No.mp3 deleted file mode 100644 index 44b4aab..0000000 Binary files a/op3_demo/Data/mp3/No.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Oops.mp3 b/op3_demo/Data/mp3/Oops.mp3 deleted file mode 100644 index 3ee002c..0000000 Binary files a/op3_demo/Data/mp3/Oops.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Right kick.mp3 b/op3_demo/Data/mp3/Right kick.mp3 deleted file mode 100644 index a428de8..0000000 Binary files a/op3_demo/Data/mp3/Right kick.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Sensor calibration complete.mp3 b/op3_demo/Data/mp3/Sensor calibration complete.mp3 deleted file mode 100644 index 9f9ed9e..0000000 Binary files a/op3_demo/Data/mp3/Sensor calibration complete.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Sensor calibration fail.mp3 b/op3_demo/Data/mp3/Sensor calibration fail.mp3 deleted file mode 100644 index a0e3ff9..0000000 Binary files a/op3_demo/Data/mp3/Sensor calibration fail.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Shoot.mp3 b/op3_demo/Data/mp3/Shoot.mp3 deleted file mode 100644 index 1bd6c54..0000000 Binary files a/op3_demo/Data/mp3/Shoot.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Sit down.mp3 b/op3_demo/Data/mp3/Sit down.mp3 deleted file mode 100644 index d7f25da..0000000 Binary files a/op3_demo/Data/mp3/Sit down.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Stand up.mp3 b/op3_demo/Data/mp3/Stand up.mp3 deleted file mode 100644 index 821361e..0000000 Binary files a/op3_demo/Data/mp3/Stand up.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Start motion demonstration.mp3 b/op3_demo/Data/mp3/Start motion demonstration.mp3 deleted file mode 100644 index 3fb573b..0000000 Binary files a/op3_demo/Data/mp3/Start motion demonstration.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Start soccer demonstration.mp3 b/op3_demo/Data/mp3/Start soccer demonstration.mp3 deleted file mode 100644 index 8820520..0000000 Binary files a/op3_demo/Data/mp3/Start soccer demonstration.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Start vision processing demonstration.mp3 b/op3_demo/Data/mp3/Start vision processing demonstration.mp3 deleted file mode 100644 index 3daa867..0000000 Binary files a/op3_demo/Data/mp3/Start vision processing demonstration.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/System shutdown.mp3 b/op3_demo/Data/mp3/System shutdown.mp3 deleted file mode 100644 index bdc2be4..0000000 Binary files a/op3_demo/Data/mp3/System shutdown.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Thank you.mp3 b/op3_demo/Data/mp3/Thank you.mp3 deleted file mode 100644 index 6778cb1..0000000 Binary files a/op3_demo/Data/mp3/Thank you.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Vision processing mode.mp3 b/op3_demo/Data/mp3/Vision processing mode.mp3 deleted file mode 100644 index fd4d9bd..0000000 Binary files a/op3_demo/Data/mp3/Vision processing mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Wow.mp3 b/op3_demo/Data/mp3/Wow.mp3 deleted file mode 100644 index 92abee2..0000000 Binary files a/op3_demo/Data/mp3/Wow.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Yes go.mp3 b/op3_demo/Data/mp3/Yes go.mp3 deleted file mode 100644 index d3ebfb2..0000000 Binary files a/op3_demo/Data/mp3/Yes go.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/Yes.mp3 b/op3_demo/Data/mp3/Yes.mp3 deleted file mode 100644 index 7a515d0..0000000 Binary files a/op3_demo/Data/mp3/Yes.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/intro_test.mp3 b/op3_demo/Data/mp3/intro_test.mp3 deleted file mode 100644 index 38a4d53..0000000 Binary files a/op3_demo/Data/mp3/intro_test.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Announce mic test.mp3 b/op3_demo/Data/mp3/test/Announce mic test.mp3 deleted file mode 100644 index 4940e78..0000000 Binary files a/op3_demo/Data/mp3/test/Announce mic test.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Button test mode.mp3 b/op3_demo/Data/mp3/test/Button test mode.mp3 deleted file mode 100644 index d2d5ca0..0000000 Binary files a/op3_demo/Data/mp3/test/Button test mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Mic test mode.mp3 b/op3_demo/Data/mp3/test/Mic test mode.mp3 deleted file mode 100644 index 2e23402..0000000 Binary files a/op3_demo/Data/mp3/test/Mic test mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Mode button long pressed.mp3 b/op3_demo/Data/mp3/test/Mode button long pressed.mp3 deleted file mode 100644 index 01c0353..0000000 Binary files a/op3_demo/Data/mp3/test/Mode button long pressed.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Mode button pressed.mp3 b/op3_demo/Data/mp3/test/Mode button pressed.mp3 deleted file mode 100644 index fc56508..0000000 Binary files a/op3_demo/Data/mp3/test/Mode button pressed.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Self test ready mode.mp3 b/op3_demo/Data/mp3/test/Self test ready mode.mp3 deleted file mode 100644 index 0b3a9c1..0000000 Binary files a/op3_demo/Data/mp3/test/Self test ready mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Start button long pressed.mp3 b/op3_demo/Data/mp3/test/Start button long pressed.mp3 deleted file mode 100644 index 1562ae9..0000000 Binary files a/op3_demo/Data/mp3/test/Start button long pressed.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Start button pressed.mp3 b/op3_demo/Data/mp3/test/Start button pressed.mp3 deleted file mode 100644 index a4348bd..0000000 Binary files a/op3_demo/Data/mp3/test/Start button pressed.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Start button test mode.mp3 b/op3_demo/Data/mp3/test/Start button test mode.mp3 deleted file mode 100644 index 9bf71e9..0000000 Binary files a/op3_demo/Data/mp3/test/Start button test mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Start mic test mode.mp3 b/op3_demo/Data/mp3/test/Start mic test mode.mp3 deleted file mode 100644 index fca8044..0000000 Binary files a/op3_demo/Data/mp3/test/Start mic test mode.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Start playing.mp3 b/op3_demo/Data/mp3/test/Start playing.mp3 deleted file mode 100644 index f1c9693..0000000 Binary files a/op3_demo/Data/mp3/test/Start playing.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/Start recording.mp3 b/op3_demo/Data/mp3/test/Start recording.mp3 deleted file mode 100644 index 39d336c..0000000 Binary files a/op3_demo/Data/mp3/test/Start recording.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/User button long pressed.mp3 b/op3_demo/Data/mp3/test/User button long pressed.mp3 deleted file mode 100644 index 7994fde..0000000 Binary files a/op3_demo/Data/mp3/test/User button long pressed.mp3 and /dev/null differ diff --git a/op3_demo/Data/mp3/test/User button pressed.mp3 b/op3_demo/Data/mp3/test/User button pressed.mp3 deleted file mode 100644 index f4e19d0..0000000 Binary files a/op3_demo/Data/mp3/test/User button pressed.mp3 and /dev/null differ diff --git a/op3_demo/include/op3_demo/action_demo.h b/op3_demo/include/op3_demo/action_demo.h index e53b9c4..4231406 100644 --- a/op3_demo/include/op3_demo/action_demo.h +++ b/op3_demo/include/op3_demo/action_demo.h @@ -1,48 +1,35 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 ACTION_DEMO_H_ #define ACTION_DEMO_H_ -#include -#include - #include #include #include #include +#include +#include + #include "op3_demo/op_demo.h" #include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/SetModule.h" #include "op3_action_module_msgs/IsRunning.h" namespace robotis_op @@ -69,6 +56,7 @@ class ActionDemo : public OPDemo PlayAction = 1, PauseAction = 2, StopAction = 3, + ReadyAction = 4, }; const int SPIN_RATE; @@ -99,17 +87,20 @@ class ActionDemo : public OPDemo bool isActionRunning(); void setModuleToDemo(const std::string &module_name); - + void callServiceSettingModule(const std::string &module_name); void actionSetNameCallback(const std_msgs::String::ConstPtr& msg); void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); + void demoCommandCallback(const std_msgs::String::ConstPtr &msg); ros::Publisher module_control_pub_; ros::Publisher motion_index_pub_; ros::Publisher play_sound_pub_; ros::Subscriber buttuon_sub_; + ros::Subscriber demo_command_sub_; ros::ServiceClient is_running_client_; + ros::ServiceClient set_joint_module_client_; std::map action_sound_table_; std::vector play_list_; diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index 86b10fe..29857db 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -34,16 +20,16 @@ #define BALL_FOLLOWER_H_ #include -#include - #include #include #include #include #include +#include +#include #include "robotis_controller_msgs/JointCtrlModule.h" -#include "ball_detector/circleSetStamped.h" +#include "op3_ball_detector/CircleSetStamped.h" #include "op3_walking_module_msgs/WalkingParam.h" #include "op3_walking_module_msgs/GetWalkingParam.h" @@ -57,8 +43,9 @@ class BallFollower enum { NotFound = 0, - OnRight = 1, - OnLeft = 2, + OutOfRange = 1, + OnRight = 2, + OnLeft = 3, }; BallFollower(); @@ -97,8 +84,8 @@ class BallFollower void setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance = true); bool getWalkingParam(); void calcFootstep(double target_distance, double target_angle, double delta_time, - double& fb_move, double& rl_angle); - + double& fb_move, double& rl_angle); + //ros node handle ros::NodeHandle nh_; diff --git a/op3_demo/include/op3_demo/ball_tracker.h b/op3_demo/include/op3_demo/ball_tracker.h index 5c95762..383c809 100644 --- a/op3_demo/include/op3_demo/ball_tracker.h +++ b/op3_demo/include/op3_demo/ball_tracker.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -34,16 +20,16 @@ #define BALL_TRACKING_H_ #include -#include - #include #include #include #include #include +#include +#include #include "robotis_controller_msgs/JointCtrlModule.h" -#include "ball_detector/circleSetStamped.h" +#include "op3_ball_detector/CircleSetStamped.h" #include "op3_walking_module_msgs/WalkingParam.h" #include "op3_walking_module_msgs/GetWalkingParam.h" @@ -53,7 +39,7 @@ namespace robotis_op // head tracking for looking the ball class BallTracker { - public: +public: enum TrackingStatus { NotFound = -1, @@ -70,6 +56,7 @@ class BallTracker void stopTracking(); void setUsingHeadScan(bool use_scan); + void goInit(); double getPanOfBall() { @@ -86,14 +73,14 @@ class BallTracker return current_ball_bottom_; } - protected: +protected: const double FOV_WIDTH; const double FOV_HEIGHT; const int NOT_FOUND_THRESHOLD; const int WAITING_THRESHOLD; const bool DEBUG_PRINT; - void ballPositionCallback(const ball_detector::circleSetStamped::ConstPtr &msg); + void ballPositionCallback(const op3_ball_detector::CircleSetStamped::ConstPtr &msg); void ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg); void publishHeadJoint(double pan, double tilt); void scanBall(); @@ -103,9 +90,12 @@ class BallTracker //image publisher/subscriber ros::Publisher module_control_pub_; + ros::Publisher head_joint_offset_pub_; ros::Publisher head_joint_pub_; ros::Publisher head_scan_pub_; + // ros::Publisher error_pub_; + ros::Publisher motion_index_pub_; ros::Subscriber ball_position_sub_; @@ -121,7 +111,9 @@ class BallTracker bool on_tracking_; double current_ball_pan_, current_ball_tilt_; double current_ball_bottom_; + double x_error_sum_, y_error_sum_; ros::Time prev_time_; + double p_gain_, d_gain_, i_gain_; }; } diff --git a/op3_demo/include/op3_demo/button_test.h b/op3_demo/include/op3_demo/button_test.h index 2647911..84e8bee 100644 --- a/op3_demo/include/op3_demo/button_test.h +++ b/op3_demo/include/op3_demo/button_test.h @@ -1,46 +1,30 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 BUTTON_TEST_H_ #define BUTTON_TEST_H_ -#include - #include #include #include +#include #include "op3_demo/op_demo.h" - #include "robotis_controller_msgs/SyncWriteItem.h" namespace robotis_op diff --git a/op3_demo/include/op3_demo/face_tracker.h b/op3_demo/include/op3_demo/face_tracker.h index 75389ee..3289375 100644 --- a/op3_demo/include/op3_demo/face_tracker.h +++ b/op3_demo/include/op3_demo/face_tracker.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -34,14 +20,14 @@ #define FACE_TRACKING_H_ #include -#include - #include #include +#include #include #include #include #include +#include namespace robotis_op { diff --git a/op3_demo/include/op3_demo/mic_test.h b/op3_demo/include/op3_demo/mic_test.h index aea0295..144fe96 100644 --- a/op3_demo/include/op3_demo/mic_test.h +++ b/op3_demo/include/op3_demo/mic_test.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -34,14 +20,12 @@ #define MIC_TEST_H_ #include -#include - #include #include #include +#include #include "op3_demo/op_demo.h" - #include "robotis_controller_msgs/SyncWriteItem.h" namespace robotis_op diff --git a/op3_demo/include/op3_demo/op_demo.h b/op3_demo/include/op3_demo/op_demo.h index ee699b0..25641a0 100644 --- a/op3_demo/include/op3_demo/op_demo.h +++ b/op3_demo/include/op3_demo/op_demo.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index 85f5837..f5f6df4 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -37,13 +23,18 @@ #include #include #include +#include +#include + +#include "op3_action_module_msgs/IsRunning.h" +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/SetJointModule.h" #include "op3_demo/op_demo.h" #include "op3_demo/ball_tracker.h" #include "op3_demo/ball_follower.h" #include "robotis_math/robotis_linear_algebra.h" -#include "op3_action_module_msgs/IsRunning.h" -#include "robotis_controller_msgs/SyncWriteItem.h" namespace robotis_op { @@ -81,9 +72,11 @@ class SoccerDemo : public OPDemo void processThread(); void callbackThread(); + void trackingThread(); void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true); void setModuleToDemo(const std::string &module_name); + void callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules); void parseJointNameFromYaml(const std::string &path); bool getJointNameFromID(const int &id, std::string &joint_name); bool getIDFromJointName(const std::string &joint_name, int &id); @@ -115,13 +108,15 @@ class SoccerDemo : public OPDemo ros::Subscriber imu_data_sub_; ros::ServiceClient is_running_client_; + ros::ServiceClient set_joint_module_client_; std::map id_joint_table_; std::map joint_id_table_; bool is_grass_; int wait_count_; - bool on_following_ball_; + bool on_following_ball_; + bool on_tracking_ball_; bool restart_soccer_; bool start_following_; bool stop_following_; diff --git a/op3_demo/include/op3_demo/vision_demo.h b/op3_demo/include/op3_demo/vision_demo.h index f652335..20cd1ad 100644 --- a/op3_demo/include/op3_demo/vision_demo.h +++ b/op3_demo/include/op3_demo/vision_demo.h @@ -1,50 +1,36 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 VISION_DEMO_H_ #define VISION_DEMO_H_ -#include - #include #include #include #include +#include + +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/SetModule.h" #include "op3_demo/op_demo.h" #include "op3_demo/face_tracker.h" -#include "robotis_controller_msgs/SyncWriteItem.h" - namespace robotis_op { @@ -70,21 +56,24 @@ class VisionDemo : public OPDemo void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg); + void demoCommandCallback(const std_msgs::String::ConstPtr &msg); void setModuleToDemo(const std::string &module_name); + void callServiceSettingModule(const std::string &module_name); FaceTracker face_tracker_; ros::Publisher module_control_pub_; ros::Publisher motion_index_pub_; - ros::Publisher rgb_led_pub_; + ros::Publisher rgb_led_pub_; + ros::Publisher face_tracking_command_pub_; ros::Subscriber buttuon_sub_; ros::Subscriber faceCoord_sub_; + ros::ServiceClient set_joint_module_client_; geometry_msgs::Point face_position_; - bool is_tracking_; int tracking_status_; }; diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch index 29b9f67..d6d26b4 100644 --- a/op3_demo/launch/demo.launch +++ b/op3_demo/launch/demo.launch @@ -1,12 +1,10 @@ - - - + @@ -16,10 +14,15 @@ - + + + + + + diff --git a/op3_demo/launch/face_detection_op3.launch b/op3_demo/launch/face_detection_op3.launch index 155b3cd..d42ab41 100644 --- a/op3_demo/launch/face_detection_op3.launch +++ b/op3_demo/launch/face_detection_op3.launch @@ -7,15 +7,16 @@ - - - - + args="$(arg face_cascade_name_0) + $(arg face_cascade_name_1) + $(arg face_cascade_name_2) + $(arg face_cascade_name_3) + $(arg face_cascade_name_4)" + output="screen"> + + + + + diff --git a/op3_demo/launch/self_test.launch b/op3_demo/launch/self_test.launch index 0f34bca..00c9acb 100644 --- a/op3_demo/launch/self_test.launch +++ b/op3_demo/launch/self_test.launch @@ -1,23 +1,29 @@ + + - - - - - + + - - + + - - + + - - + + - - + + + + + + + + + diff --git a/op3_demo/list/action_script.yaml b/op3_demo/list/action_script.yaml new file mode 100644 index 0000000..21ce72b --- /dev/null +++ b/op3_demo/list/action_script.yaml @@ -0,0 +1,23 @@ +# combination action page number and mp3 file path +action_and_sound: + 4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Thank you.mp3" + 41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Introduction.mp3" + 24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Wow.mp3" + 23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Yes go.mp3" + 15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Sit down.mp3" + 1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Stand up.mp3" + 54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Clap please.mp3" + 27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Oops.mp3" + 38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Bye bye.mp3" +# 101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Oops.mp3" + 110 : "" + 111 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Intro01.mp3" + 115 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Intro02.mp3" + 118 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Intro03.mp3" + +# play list +prev_default: [4, 41, 24, 23, 15, 1, 54, 27, 38] +default: [4, 110, 111, 115, 118, 24, 54, 27, 38] + +# example of play list +#certification: [101] \ No newline at end of file diff --git a/op3_demo/script/action_script_bk.yaml b/op3_demo/list/action_script_bk.yaml similarity index 72% rename from op3_demo/script/action_script_bk.yaml rename to op3_demo/list/action_script_bk.yaml index a18155d..bc401fb 100644 --- a/op3_demo/script/action_script_bk.yaml +++ b/op3_demo/list/action_script_bk.yaml @@ -1,15 +1,15 @@ # combination action page number and mp3 file path action_and_sound: - 4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3" - 41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3" - 24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3" - 23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3" - 15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3" - 1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3" - 54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3" - 27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3" - 38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3" - 101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3" + 4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Thank you.mp3" + 41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Introduction.mp3" + 24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Wow.mp3" + 23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Yes go.mp3" + 15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Sit down.mp3" + 1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Stand up.mp3" + 54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Clap please.mp3" + 27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Oops.mp3" + 38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Bye bye.mp3" + 101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/data/mp3/Oops.mp3" # play list default: [4, 41, 24, 23, 15, 1, 54, 27, 38] diff --git a/op3_demo/package.xml b/op3_demo/package.xml index b376b44..1a0785c 100644 --- a/op3_demo/package.xml +++ b/op3_demo/package.xml @@ -1,28 +1,36 @@ - + op3_demo 0.1.0 - op3 default demo + OP3 default demo It includes three demontrations(soccer demo, vision demo, action script demo) - BSD - kayman + Apache 2.0 + Kayman Pyo - - catkin - roscpp - roslib - sensor_msgs - ball_detector - op3_walking_module_msgs - cmake_modules - robotis_math - roscpp - roslib - sensor_msgs - ball_detector - op3_walking_module_msgs - cmake_modules - robotis_math - \ No newline at end of file + http://wiki.ros.org/op3_demo + http://emanual.robotis.com/docs/en/platform/op3/introduction/ + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + robotis_controller_msgs + op3_walking_module_msgs + op3_action_module_msgs + cmake_modules + robotis_math + op3_ball_detector + boost + eigen + yaml-cpp + op3_manager + op3_camera_setting_tool + op3_web_setting_tool + ros_madplay_player + + diff --git a/op3_demo/script/action_script.yaml b/op3_demo/script/action_script.yaml deleted file mode 100644 index 077e820..0000000 --- a/op3_demo/script/action_script.yaml +++ /dev/null @@ -1,23 +0,0 @@ -# combination action page number and mp3 file path -action_and_sound: - 4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3" - 41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3" - 24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3" - 23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3" - 15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3" - 1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3" - 54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3" - 27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3" - 38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3" -# 101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3" - 110 : "" - 111 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro01.mp3" - 115 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro02.mp3" - 118 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro03.mp3" - -# play list -prev_default: [4, 41, 24, 23, 15, 1, 54, 27, 38] -default: [4, 110, 111, 115, 118, 24, 54, 27, 38] - -# example of play list -#certification: [101] \ No newline at end of file diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index b257ef5..660390c 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -45,12 +31,14 @@ ActionDemo::ActionDemo() ros::NodeHandle nh(ros::this_node::getName()); - std::string default_path = ros::package::getPath("op3_demo") + "/script/action_script.yaml"; + std::string default_path = ros::package::getPath("op3_demo") + "/list/action_script.yaml"; script_path_ = nh.param("action_script", default_path); std::string default_play_list = "default"; play_list_name_ = nh.param("action_script_play_list", default_play_list); + demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &ActionDemo::demoCommandCallback, this); + parseActionScript (script_path_); boost::thread queue_thread = boost::thread(boost::bind(&ActionDemo::callbackThread, this)); @@ -65,8 +53,6 @@ void ActionDemo::setDemoEnable() { setModuleToDemo("action_module"); - usleep(10 * 1000); - enable_ = true; ROS_INFO_COND(DEBUG_PRINT, "Start ActionScript Demo"); @@ -81,7 +67,7 @@ void ActionDemo::setDemoDisable() stopProcess(); enable_ = false; - + ROS_WARN("Set Action demo disable"); play_list_.resize(0); } @@ -122,6 +108,8 @@ void ActionDemo::process() stopMP3(); brakeAction(); + play_status_ = ReadyAction; + break; } @@ -130,6 +118,8 @@ void ActionDemo::process() stopMP3(); stopAction(); + play_status_ = ReadyAction; + break; } @@ -189,6 +179,7 @@ void ActionDemo::callbackThread() buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ActionDemo::buttonHandlerCallback, this); is_running_client_ = nh.serviceClient("/robotis/action/is_running"); + set_joint_module_client_ = nh.serviceClient("/robotis/set_present_ctrl_modules"); while (nh.ok()) { @@ -367,11 +358,37 @@ void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) void ActionDemo::setModuleToDemo(const std::string &module_name) { - std_msgs::String control_msg; - control_msg.data = "action_module"; + callServiceSettingModule(module_name); + ROS_INFO_STREAM("enable module : " << module_name); +} - module_control_pub_.publish(control_msg); - std::cout << "enable module : " << module_name << std::endl; +void ActionDemo::callServiceSettingModule(const std::string &module_name) +{ + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; + + if (set_joint_module_client_.call(set_module_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } + + return ; +} + +void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + if (enable_ == false) + return; + + if (msg->data == "start") + { + resumeProcess(); + } + else if (msg->data == "stop") + { + pauseProcess(); + } } } /* namespace robotis_op */ diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index 1be5377..300a90b 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -55,6 +41,8 @@ void setLED(int led); bool checkManagerRunning(std::string& manager_name); void dxlTorqueChecker(); +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); + const int SPIN_RATE = 30; const bool DEBUG_PRINT = false; @@ -87,8 +75,9 @@ int main(int argc, char **argv) led_pub = nh.advertise("/robotis/sync_write_item", 0); dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); - default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/"; + default_mp3_path = ros::package::getPath("op3_demo") + "/data/mp3/"; ros::start(); @@ -327,3 +316,55 @@ void dxlTorqueChecker() dxl_torque_pub.publish(check_msg); } + +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + // In demo mode + if (current_status != Ready) + { + if (msg->data == "ready") + { + // go to mode selection status + desired_status = Ready; + apply_desired = true; + + playSound(default_mp3_path + "Demonstration ready mode.mp3"); + setLED(0x01 | 0x02 | 0x04); + } + } + // In ready mode + else + { + if(msg->data == "soccer") + { + desired_status = SoccerDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start soccer demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "vision") + { + desired_status = VisionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start vision processing demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "action") + { + desired_status = ActionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start motion demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + } +} + diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index c5b0e84..eec94e9 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -37,7 +23,7 @@ namespace robotis_op BallFollower::BallFollower() : nh_(ros::this_node::getName()), - FOV_WIDTH(26.4 * M_PI / 180), + FOV_WIDTH(35.2 * M_PI / 180), FOV_HEIGHT(21.6 * M_PI / 180), count_not_found_(0), count_to_kick_(0), @@ -230,7 +216,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ "head tilt : " << (current_tilt_ * 180 / M_PI) << " | ball tilt : " << (y_angle * 180 / M_PI)); ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << accum_ball_position_); - ROS_INFO("In range [%d]", count_to_kick_); + ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_); if (count_to_kick_ > 20) { diff --git a/op3_demo/src/soccer/ball_tracker.cpp b/op3_demo/src/soccer/ball_tracker.cpp index 3f4cde0..a1706ad 100644 --- a/op3_demo/src/soccer/ball_tracker.cpp +++ b/op3_demo/src/soccer/ball_tracker.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -36,22 +22,33 @@ namespace robotis_op { BallTracker::BallTracker() - : nh_(ros::this_node::getName()), - FOV_WIDTH(26.4 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), - NOT_FOUND_THRESHOLD(50), - WAITING_THRESHOLD(5), - use_head_scan_(true), - count_not_found_(0), - on_tracking_(false), - current_ball_pan_(0), - current_ball_tilt_(0), - current_ball_bottom_(0), - tracking_status_(NotFound), - DEBUG_PRINT(false) + : nh_(ros::this_node::getName()), + FOV_WIDTH(35.2 * M_PI / 180), + FOV_HEIGHT(21.6 * M_PI / 180), + NOT_FOUND_THRESHOLD(50), + WAITING_THRESHOLD(5), + use_head_scan_(true), + count_not_found_(0), + on_tracking_(false), + current_ball_pan_(0), + current_ball_tilt_(0), + x_error_sum_(0), + y_error_sum_(0), + current_ball_bottom_(0), + tracking_status_(NotFound), + DEBUG_PRINT(false) { - head_joint_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); + ros::NodeHandle param_nh("~"); + p_gain_ = param_nh.param("p_gain", 0.4); + i_gain_ = param_nh.param("i_gain", 0.0); + d_gain_ = param_nh.param("d_gain", 0.0); + + ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", " << d_gain_); + + head_joint_offset_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); + head_joint_pub_ = nh_.advertise("/robotis/head_control/set_joint_states", 0); head_scan_pub_ = nh_.advertise("/robotis/head_control/scan_command", 0); + // error_pub_ = nh_.advertise("/ball_tracker/errors", 0); ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, &BallTracker::ballPositionCallback, this); ball_tracking_command_sub_ = nh_.subscribe("/ball_tracker/command", 1, &BallTracker::ballTrackerCommandCallback, @@ -63,7 +60,7 @@ BallTracker::~BallTracker() } -void BallTracker::ballPositionCallback(const ball_detector::circleSetStamped::ConstPtr &msg) +void BallTracker::ballPositionCallback(const op3_ball_detector::CircleSetStamped::ConstPtr &msg) { for (int idx = 0; idx < msg->circles.size(); idx++) { @@ -101,12 +98,15 @@ void BallTracker::startTracking() void BallTracker::stopTracking() { + goInit(); + on_tracking_ = false; ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking"); - double x_error = -atan(ball_position_.x * tan(FOV_WIDTH)); - double y_error = -atan(ball_position_.y * tan(FOV_HEIGHT)); - publishHeadJoint(x_error, y_error); + current_ball_pan_ = 0; + current_ball_tilt_ = 0; + x_error_sum_ = 0; + y_error_sum_ = 0; } void BallTracker::setUsingHeadScan(bool use_scan) @@ -161,24 +161,26 @@ int BallTracker::processTracking() switch (tracking_status) { - case NotFound: - tracking_status_ = tracking_status; - return tracking_status; + case NotFound: + tracking_status_ = tracking_status; + current_ball_pan_ = 0; + current_ball_tilt_ = 0; + x_error_sum_ = 0; + y_error_sum_ = 0; + return tracking_status; - case Waiting: - x_error = current_ball_pan_ * 0.7; - y_error = current_ball_tilt_ * 0.7; - ball_size = current_ball_bottom_; - break; + case Waiting: + tracking_status_ = tracking_status; + return tracking_status; - case Found: - x_error = -atan(ball_position_.x * tan(FOV_WIDTH)); - y_error = -atan(ball_position_.y * tan(FOV_HEIGHT)); - ball_size = ball_position_.z; - break; + case Found: + x_error = -atan(ball_position_.x * tan(FOV_WIDTH)); + y_error = -atan(ball_position_.y * tan(FOV_HEIGHT)); + ball_size = ball_position_.z; + break; - default: - break; + default: + break; } ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------"); @@ -190,21 +192,34 @@ int BallTracker::processTracking() double delta_time = dur.nsec * 0.000000001 + dur.sec; prev_time_ = curr_time; - // double p_gain = 0.7, d_gain = 0.05; - double p_gain = 0.75, d_gain = 0.04; double x_error_diff = (x_error - current_ball_pan_) / delta_time; double y_error_diff = (y_error - current_ball_tilt_) / delta_time; - double x_error_target = x_error * p_gain + x_error_diff * d_gain; - double y_error_target = y_error * p_gain + y_error_diff * d_gain; + x_error_sum_ += x_error; + y_error_sum_ += y_error; + double x_error_target = x_error * p_gain_ + x_error_diff * d_gain_ + x_error_sum_ * i_gain_; + double y_error_target = y_error * p_gain_ + y_error_diff * d_gain_ + y_error_sum_ * i_gain_; - ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------"); + // std_msgs::Float64MultiArray x_error_msg; + // x_error_msg.data.push_back(x_error); + // x_error_msg.data.push_back(x_error_diff); + // x_error_msg.data.push_back(x_error_sum_); + // x_error_msg.data.push_back(x_error * p_gain_); + // x_error_msg.data.push_back(x_error_diff * d_gain_); + // x_error_msg.data.push_back(x_error_sum_ * i_gain_); + // x_error_msg.data.push_back(x_error_target); + // error_pub_.publish(x_error_msg); + + ROS_INFO_STREAM_COND(DEBUG_PRINT, "------------------------ " << tracking_status << " --------------------------------------"); ROS_INFO_STREAM_COND(DEBUG_PRINT, "error : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI)); ROS_INFO_STREAM_COND( - DEBUG_PRINT, - "error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | " << delta_time); + DEBUG_PRINT, + "error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | " << delta_time); ROS_INFO_STREAM_COND( - DEBUG_PRINT, - "error_target : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI) << " | P : " << p_gain << " | D : " << d_gain); + DEBUG_PRINT, + "error_sum : " << (x_error_sum_ * 180 / M_PI) << " | " << (y_error_sum_ * 180 / M_PI)); + ROS_INFO_STREAM_COND( + DEBUG_PRINT, + "error_target : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI) << " | P : " << p_gain_ << " | D : " << d_gain_ << " | time : " << delta_time); // move head joint publishHeadJoint(x_error_target, y_error_target); @@ -215,7 +230,6 @@ int BallTracker::processTracking() current_ball_bottom_ = ball_size; ball_position_.z = 0; - //count_not_found_ = 0; tracking_status_ = tracking_status; return tracking_status; @@ -235,6 +249,19 @@ void BallTracker::publishHeadJoint(double pan, double tilt) head_angle_msg.position.push_back(pan); head_angle_msg.position.push_back(tilt); + head_joint_offset_pub_.publish(head_angle_msg); +} + +void BallTracker::goInit() +{ + sensor_msgs::JointState head_angle_msg; + + head_angle_msg.name.push_back("head_pan"); + head_angle_msg.name.push_back("head_tilt"); + + head_angle_msg.position.push_back(0.0); + head_angle_msg.position.push_back(0.0); + head_joint_pub_.publish(head_angle_msg); } diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index 394c3a1..37e2c3f 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -36,32 +22,34 @@ namespace robotis_op { SoccerDemo::SoccerDemo() - : FALL_FORWARD_LIMIT(60), - FALL_BACK_LIMIT(-60), - SPIN_RATE(30), - DEBUG_PRINT(false), - wait_count_(0), - on_following_ball_(false), - restart_soccer_(false), - start_following_(false), - stop_following_(false), - stop_fallen_check_(false), - robot_status_(Waited), - stand_state_(Stand), - tracking_status_(BallTracker::Waiting), - present_pitch_(0) + : FALL_FORWARD_LIMIT(60), + FALL_BACK_LIMIT(-60), + SPIN_RATE(30), + DEBUG_PRINT(false), + wait_count_(0), + on_following_ball_(false), + on_tracking_ball_(false), + restart_soccer_(false), + start_following_(false), + stop_following_(false), + stop_fallen_check_(false), + robot_status_(Waited), + stand_state_(Stand), + tracking_status_(BallTracker::Waiting), + present_pitch_(0) { //init ros enable_ = false; ros::NodeHandle nh(ros::this_node::getName()); - std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/demo_config.yaml"; + std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/gui_config.yaml"; std::string path = nh.param("demo_config", default_path); parseJointNameFromYaml(path); boost::thread queue_thread = boost::thread(boost::bind(&SoccerDemo::callbackThread, this)); boost::thread process_thread = boost::thread(boost::bind(&SoccerDemo::processThread, this)); + boost::thread tracking_thread = boost::thread(boost::bind(&SoccerDemo::trackingThread, this)); is_grass_ = nh.param("grass_demo", false); } @@ -80,8 +68,6 @@ void SoccerDemo::setDemoEnable() void SoccerDemo::setDemoDisable() { - setModuleToDemo("base_module"); - // handle disable procedure ball_tracker_.stopTracking(); ball_follower_.stopFollowing(); @@ -89,6 +75,7 @@ void SoccerDemo::setDemoDisable() enable_ = false; wait_count_ = 0; on_following_ball_ = false; + on_tracking_ball_ = false; restart_soccer_ = false; start_following_ = false; stop_following_ = false; @@ -99,11 +86,8 @@ void SoccerDemo::setDemoDisable() void SoccerDemo::process() { - // ball tracking - bool is_tracked; - int tracking_status; - - tracking_status = ball_tracker_.processTracking(); + if(enable_ == false) + return; // check to start if (start_following_ == true) @@ -115,9 +99,6 @@ void SoccerDemo::process() wait_count_ = 1 * SPIN_RATE; } - //for debug - //return; - // check to stop if (stop_following_ == true) { @@ -133,57 +114,50 @@ void SoccerDemo::process() // ball following if (on_following_ball_ == true) { - switch(tracking_status) + switch(tracking_status_) { - case BallTracker::Found: - ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0); - if(tracking_status_ != tracking_status) - setRGBLED(0x1F, 0x1F, 0x1F); - break; + case BallTracker::Found: + ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0); + break; - case BallTracker::NotFound: - ball_follower_.waitFollowing(); - if(tracking_status_ != tracking_status) - setRGBLED(0, 0, 0); - break; + case BallTracker::NotFound: + ball_follower_.waitFollowing(); + break; - default: - break; + default: + break; } - - if(tracking_status != tracking_status_) - tracking_status_ = tracking_status; } // check fallen states switch (stand_state_) { - case Stand: + case Stand: + { + // check restart soccer + if (restart_soccer_ == true) { - // check restart soccer - if (restart_soccer_ == true) - { - restart_soccer_ = false; - startSoccerMode(); - break; - } - - // check states for kick - int ball_position = ball_follower_.getBallPosition(); - if (ball_position != robotis_op::BallFollower::NotFound) - { - ball_follower_.stopFollowing(); - handleKick(ball_position); - } + restart_soccer_ = false; + startSoccerMode(); break; } - // fallen state : Fallen_Forward, Fallen_Behind - default: + + // check states for kick + int ball_position = ball_follower_.getBallPosition(); + if (ball_position != robotis_op::BallFollower::NotFound) { ball_follower_.stopFollowing(); - handleFallen(stand_state_); - break; + handleKick(ball_position); } + break; + } + // fallen state : Fallen_Forward, Fallen_Behind + default: + { + ball_follower_.stopFollowing(); + handleFallen(stand_state_); + break; + } } } else @@ -222,10 +196,11 @@ void SoccerDemo::callbackThread() rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &SoccerDemo::buttonHandlerCallback, this); - demo_command_sub_ = nh.subscribe("/ball_tracker/command", 1, &SoccerDemo::demoCommandCallback, this); + demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &SoccerDemo::demoCommandCallback, this); imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this); is_running_client_ = nh.serviceClient("/robotis/action/is_running"); + set_joint_module_client_ = nh.serviceClient("/robotis/set_present_joint_ctrl_modules"); while (nh.ok()) { @@ -235,6 +210,50 @@ void SoccerDemo::callbackThread() } } +void SoccerDemo::trackingThread() +{ + + //set node loop rate + ros::Rate loop_rate(SPIN_RATE); + + ball_tracker_.startTracking(); + + //node loop + while (ros::ok()) + { + + if(enable_ == true && on_tracking_ball_ == true) + { + // ball tracking + int tracking_status; + + tracking_status = ball_tracker_.processTracking(); + + // set led + switch(tracking_status) + { + case BallTracker::Found: + if(tracking_status_ != tracking_status) + setRGBLED(0x1F, 0x1F, 0x1F); + break; + + case BallTracker::NotFound: + if(tracking_status_ != tracking_status) + setRGBLED(0, 0, 0); + break; + + default: + break; + } + + if(tracking_status != tracking_status_) + tracking_status_ = tracking_status; + } + //relax to fit output rate + loop_rate.sleep(); + } +} + void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control) { robotis_controller_msgs::JointCtrlModule control_msg; @@ -266,12 +285,15 @@ void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_h if (control_msg.joint_name.size() == 0) return; - module_control_pub_.publish(control_msg); + callServiceSettingModule(control_msg); std::cout << "enable module of body : " << body_module << std::endl; } void SoccerDemo::setModuleToDemo(const std::string &module_name) { + if(enable_ == false) + return; + robotis_controller_msgs::JointCtrlModule control_msg; std::map::iterator joint_iter; @@ -285,10 +307,25 @@ void SoccerDemo::setModuleToDemo(const std::string &module_name) if (control_msg.joint_name.size() == 0) return; - module_control_pub_.publish(control_msg); + callServiceSettingModule(control_msg); std::cout << "enable module : " << module_name << std::endl; } +void SoccerDemo::callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules) +{ + robotis_controller_msgs::SetJointModule set_joint_srv; + set_joint_srv.request.joint_name = modules.joint_name; + set_joint_srv.request.module_name = modules.module_name; + + if (set_joint_module_client_.call(set_joint_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } + + return ; +} + void SoccerDemo::parseJointNameFromYaml(const std::string &path) { YAML::Node doc; @@ -428,18 +465,13 @@ void SoccerDemo::startSoccerMode() { setModuleToDemo("action_module"); - usleep(10 * 1000); - playMotion(WalkingReady); - usleep(1500 * 1000); - setBodyModuleToDemo("walking_module"); - usleep(10 * 1000); - ROS_INFO("Start Soccer Demo"); on_following_ball_ = true; + on_tracking_ball_ = true; start_following_ = true; } @@ -447,36 +479,37 @@ void SoccerDemo::stopSoccerMode() { ROS_INFO("Stop Soccer Demo"); on_following_ball_ = false; + on_tracking_ball_ = false; stop_following_ = true; } void SoccerDemo::handleKick(int ball_position) { - usleep(1000 * 1000); + usleep(1500 * 1000); // change to motion module setModuleToDemo("action_module"); - usleep(1500 * 1000); + //usleep(1500 * 1000); - if (handleFallen(stand_state_) == true) + if (handleFallen(stand_state_) == true || enable_ == false) return; // kick motion switch (ball_position) { - case robotis_op::BallFollower::OnRight: - std::cout << "Kick Motion [R]: " << ball_position << std::endl; - playMotion(is_grass_ ? RightKick + ForGrass : RightKick); - break; + case robotis_op::BallFollower::OnRight: + std::cout << "Kick Motion [R]: " << ball_position << std::endl; + playMotion(is_grass_ ? RightKick + ForGrass : RightKick); + break; - case robotis_op::BallFollower::OnLeft: - std::cout << "Kick Motion [L]: " << ball_position << std::endl; - playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); - break; + case robotis_op::BallFollower::OnLeft: + std::cout << "Kick Motion [L]: " << ball_position << std::endl; + playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); + break; - default: - break; + default: + break; } on_following_ball_ = false; @@ -501,23 +534,21 @@ bool SoccerDemo::handleFallen(int fallen_status) // change to motion module setModuleToDemo("action_module"); - usleep(600 * 1000); - // getup motion switch (fallen_status) { - case Fallen_Forward: - std::cout << "Getup Motion [F]: " << std::endl; - playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront); - break; + case Fallen_Forward: + std::cout << "Getup Motion [F]: " << std::endl; + playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront); + break; - case Fallen_Behind: - std::cout << "Getup Motion [B]: " << std::endl; - playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack); - break; + case Fallen_Behind: + std::cout << "Getup Motion [B]: " << std::endl; + playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack); + break; - default: - break; + default: + break; } while(isActionRunning() == true) diff --git a/op3_demo/src/test/button_test.cpp b/op3_demo/src/test/button_test.cpp index f5c2d61..7a78c6a 100644 --- a/op3_demo/src/test/button_test.cpp +++ b/op3_demo/src/test/button_test.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -47,7 +33,7 @@ ButtonTest::ButtonTest() boost::thread queue_thread = boost::thread(boost::bind(&ButtonTest::callbackThread, this)); boost::thread process_thread = boost::thread(boost::bind(&ButtonTest::processThread, this)); - default_mp3_path_ = ros::package::getPath("op3_demo") + "/Data/mp3/test/"; + default_mp3_path_ = ros::package::getPath("op3_demo") + "/data/mp3/test/"; } ButtonTest::~ButtonTest() diff --git a/op3_demo/src/test/mic_test.cpp b/op3_demo/src/test/mic_test.cpp index dd0edfd..4e56954 100644 --- a/op3_demo/src/test/mic_test.cpp +++ b/op3_demo/src/test/mic_test.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -50,8 +36,8 @@ MicTest::MicTest() boost::thread queue_thread = boost::thread(boost::bind(&MicTest::callbackThread, this)); boost::thread process_thread = boost::thread(boost::bind(&MicTest::processThread, this)); - recording_file_name_ = ros::package::getPath("op3_demo") + "/Data/mp3/test/mic-test.wav"; - default_mp3_path_ = ros::package::getPath("op3_demo") + "/Data/mp3/test/"; + recording_file_name_ = ros::package::getPath("op3_demo") + "/data/mp3/test/mic-test.wav"; + default_mp3_path_ = ros::package::getPath("op3_demo") + "/data/mp3/test/"; start_time_ = ros::Time::now(); } diff --git a/op3_demo/src/test_node.cpp b/op3_demo/src/test_node.cpp index b1ab4a2..8d4768d 100644 --- a/op3_demo/src/test_node.cpp +++ b/op3_demo/src/test_node.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -58,12 +44,17 @@ void goInitPose(); void playSound(const std::string &path); void setLED(int led); bool checkManagerRunning(std::string& manager_name); +void dxlTorqueChecker(); + +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); const int SPIN_RATE = 30; +const bool DEBUG_PRINT = true; ros::Publisher init_pose_pub; ros::Publisher play_sound_pub; ros::Publisher led_pub; +ros::Publisher dxl_torque_pub; std::string default_mp3_path = ""; int current_status = Ready; @@ -89,9 +80,11 @@ int main(int argc, char **argv) init_pose_pub = nh.advertise("/robotis/base/ini_pose", 0); play_sound_pub = nh.advertise("/play_sound_file", 0); led_pub = nh.advertise("/robotis/sync_write_item", 0); + dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); - default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/"; + default_mp3_path = ros::package::getPath("op3_demo") + "/data/mp3/"; ros::start(); @@ -107,7 +100,7 @@ int main(int argc, char **argv) if (checkManagerRunning(manager_name) == true) { break; - ROS_INFO("Succeed to connect"); + ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); } ROS_WARN("Waiting for op3 manager"); } @@ -134,7 +127,7 @@ int main(int argc, char **argv) goInitPose(); - ROS_INFO("[Go to Demo READY!]"); + ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); break; } @@ -146,7 +139,7 @@ int main(int argc, char **argv) current_demo = soccer_demo; current_demo->setDemoEnable(); - ROS_INFO("[Start] Soccer Demo"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); break; } @@ -157,7 +150,7 @@ int main(int argc, char **argv) current_demo = vision_demo; current_demo->setDemoEnable(); - ROS_INFO("[Start] Vision Demo"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); break; } case ActionDemo: @@ -167,7 +160,7 @@ int main(int argc, char **argv) current_demo = action_demo; current_demo->setDemoEnable(); - ROS_INFO("[Start] Action Demo"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); break; } case ButtonTest: @@ -177,7 +170,7 @@ int main(int argc, char **argv) current_demo = button_test; current_demo->setDemoEnable(); - ROS_INFO("[Start] Button Test"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test"); break; } case MicTest: @@ -187,7 +180,7 @@ int main(int argc, char **argv) current_demo = mic_test; current_demo->setDemoEnable(); - ROS_INFO("[Start] Mic Test"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test"); break; } @@ -207,6 +200,10 @@ int main(int argc, char **argv) //relax to fit output rate loop_rate.sleep(); + + // for debug + if (checkManagerRunning(manager_name) == false) + return 0; } //exit program @@ -215,6 +212,9 @@ int main(int argc, char **argv) void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) { + if(apply_desired == true) + return; + // in the middle of playing demo if (current_status != Ready) { @@ -246,22 +246,27 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) switch (desired_status) { case SoccerDemo: + dxlTorqueChecker(); playSound(default_mp3_path + "Start soccer demonstration.mp3"); break; case VisionDemo: + dxlTorqueChecker(); playSound(default_mp3_path + "Start vision processing demonstration.mp3"); break; case ActionDemo: + dxlTorqueChecker(); playSound(default_mp3_path + "Start motion demonstration.mp3"); break; case ButtonTest: + dxlTorqueChecker(); playSound(default_mp3_path + "test/Start button test mode.mp3"); break; case MicTest: + dxlTorqueChecker(); playSound(default_mp3_path + "test/Start mic test mode.mp3"); break; @@ -269,7 +274,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) break; } - ROS_INFO("= Start Demo Mode : %d", desired_status); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); } else if (msg->data == "mode") { @@ -309,7 +314,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) break; } - ROS_INFO("= Demo Mode : %d", desired_status); + ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status); } } } @@ -354,3 +359,63 @@ bool checkManagerRunning(std::string& manager_name) ROS_ERROR("Can't find op3_manager"); return false; } + +void dxlTorqueChecker() +{ + std_msgs::String check_msg; + check_msg.data = "check"; + + dxl_torque_pub.publish(check_msg); +} + +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + // In demo mode + if (current_status != Ready) + { + if (msg->data == "ready") + { + // go to mode selection status + desired_status = Ready; + apply_desired = true; + + playSound(default_mp3_path + "Demonstration ready mode.mp3"); + setLED(0x01 | 0x02 | 0x04); + } + } + // In ready mode + else + { + if(msg->data == "soccer") + { + desired_status = SoccerDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start soccer demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "vision") + { + desired_status = VisionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start vision processing demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "action") + { + desired_status = ActionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start motion demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + } +} + diff --git a/op3_demo/src/vision/face_tracker.cpp b/op3_demo/src/vision/face_tracker.cpp index 2345b81..f9b3e24 100644 --- a/op3_demo/src/vision/face_tracker.cpp +++ b/op3_demo/src/vision/face_tracker.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -37,7 +23,7 @@ namespace robotis_op FaceTracker::FaceTracker() : nh_(ros::this_node::getName()), - FOV_WIDTH(26.4 * M_PI / 180), + FOV_WIDTH(35.2 * M_PI / 180), FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50), use_head_scan_(false), @@ -48,8 +34,7 @@ FaceTracker::FaceTracker() head_scan_pub_ = nh_.advertise("/robotis/head_control/scan_command", 0); face_position_sub_ = nh_.subscribe("/face_position", 1, &FaceTracker::facePositionCallback, this); - face_tracking_command_sub_ = nh_.subscribe("/face_tracker/command", 1, &FaceTracker::faceTrackerCommandCallback, - this); + //face_tracking_command_sub_ = nh_.subscribe("/robotis/demo_command", 1, &FaceTracker::faceTrackerCommandCallback, this); } FaceTracker::~FaceTracker() @@ -87,12 +72,14 @@ void FaceTracker::faceTrackerCommandCallback(const std_msgs::String::ConstPtr &m void FaceTracker::startTracking() { on_tracking_ = true; + ROS_INFO("Start Face tracking"); } void FaceTracker::stopTracking() { on_tracking_ = false; + ROS_INFO("Stop Face tracking"); } diff --git a/op3_demo/src/vision/vision_demo.cpp b/op3_demo/src/vision/vision_demo.cpp index ac205fa..1062e5c 100644 --- a/op3_demo/src/vision/vision_demo.cpp +++ b/op3_demo/src/vision/vision_demo.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ +* 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 */ @@ -37,7 +23,6 @@ namespace robotis_op VisionDemo::VisionDemo() : SPIN_RATE(30), - is_tracking_(false), tracking_status_(FaceTracker::Waiting) { enable_ = false; @@ -58,17 +43,19 @@ void VisionDemo::setDemoEnable() // change to motion module setModuleToDemo("action_module"); - usleep(100 * 1000); - playMotion(InitPose); usleep(1500 * 1000); setModuleToDemo("head_control_module"); - usleep(10 * 1000); - enable_ = true; + + // send command to start face_tracking + std_msgs::Bool command; + command.data = enable_; + face_tracking_command_pub_.publish(command); + face_tracker_.startTracking(); ROS_INFO("Start Vision Demo"); @@ -79,9 +66,12 @@ void VisionDemo::setDemoDisable() { face_tracker_.stopTracking(); - is_tracking_ = false; tracking_status_ = FaceTracker::Waiting; enable_ = false; + + std_msgs::Bool command; + command.data = enable_; + face_tracking_command_pub_.publish(command); } void VisionDemo::process() @@ -107,9 +97,6 @@ void VisionDemo::process() if(tracking_status != FaceTracker::Waiting) tracking_status_ = tracking_status; - - //is_tracking_ = is_tracked; - std::cout << "Tracking : " << tracking_status << std::endl; } void VisionDemo::processThread() @@ -136,10 +123,13 @@ void VisionDemo::callbackThread() module_control_pub_ = nh.advertise("/robotis/enable_ctrl_module", 0); motion_index_pub_ = nh.advertise("/robotis/action/page_num", 0); rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); + face_tracking_command_pub_ = nh.advertise("/face_tracking/command", 0); buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &VisionDemo::buttonHandlerCallback, this); faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this); + set_joint_module_client_ = nh.serviceClient("/robotis/set_present_ctrl_modules"); + while (nh.ok()) { ros::spinOnce(); @@ -155,29 +145,7 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) if (msg->data == "start") { -// switch (play_status_) -// { -// case PlayAction: -// { -// pauseProcess(); -// break; -// } -// -// case PauseAction: -// { -// resumeProcess(); -// break; -// } -// -// case StopAction: -// { -// resumeProcess(); -// break; -// } -// -// default: -// break; -// } + } else if (msg->data == "mode") { @@ -185,13 +153,39 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) } } +void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + if (enable_ == false) + return; + + if (msg->data == "start") + { + + } + else if (msg->data == "stop") + { + + } +} + void VisionDemo::setModuleToDemo(const std::string &module_name) { - std_msgs::String control_msg; - control_msg.data = module_name; + callServiceSettingModule(module_name); + ROS_INFO_STREAM("enable module : " << module_name); +} - module_control_pub_.publish(control_msg); - std::cout << "enable module : " << module_name << std::endl; +void VisionDemo::callServiceSettingModule(const std::string &module_name) +{ + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; + + if (set_joint_module_client_.call(set_module_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } + + return ; } void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg) diff --git a/op3_read_write_demo/CMakeLists.txt b/op3_read_write_demo/CMakeLists.txt new file mode 100644 index 0000000..8826d03 --- /dev/null +++ b/op3_read_write_demo/CMakeLists.txt @@ -0,0 +1,75 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(op3_read_write_demo) + +################################################################################ +# Find catkin packages and libraries for catkin and system dependencies +################################################################################ +find_package(catkin REQUIRED COMPONENTS + robotis_controller_msgs + roscpp + sensor_msgs + std_msgs +) + +################################################################################ +# Setup for python modules and scripts +################################################################################ + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Declare catkin specific configuration to be passed to dependent projects +################################################################################ +catkin_package( + INCLUDE_DIRS + CATKIN_DEPENDS + roscpp + robotis_controller_msgs + roscpp + sensor_msgs + std_msgs +) + +################################################################################ +# Build +################################################################################ +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(read_write + src/read_write.cpp +) + +add_dependencies(read_write + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(read_write + ${catkin_LIBRARIES} +) + +################################################################################ +# Install +################################################################################ +install(TARGETS read_write + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/op3_read_write_demo/launch/op3_read_write.launch b/op3_read_write_demo/launch/op3_read_write.launch new file mode 100644 index 0000000..01217ff --- /dev/null +++ b/op3_read_write_demo/launch/op3_read_write.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/op3_read_write_demo/package.xml b/op3_read_write_demo/package.xml new file mode 100644 index 0000000..3d7c712 --- /dev/null +++ b/op3_read_write_demo/package.xml @@ -0,0 +1,23 @@ + + + op3_read_write_demo + 0.0.1 + + The op3_read_write_demo package + + + Apache 2.0 + Kayman + Pyo + http://wiki.ros.org/op3_demo + http://emanual.robotis.com/docs/en/platform/op3/introduction/ + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + roscpp + std_msgs + sensor_msgs + robotis_controller_msgs + op3_manager + + diff --git a/op3_read_write_demo/src/read_write.cpp b/op3_read_write_demo/src/read_write.cpp new file mode 100644 index 0000000..1b87dc4 --- /dev/null +++ b/op3_read_write_demo/src/read_write.cpp @@ -0,0 +1,305 @@ +/******************************************************************************* +* 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 +#include + +#include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/SyncWriteItem.h" + +void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); +void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg); +void readyToDemo(); +void setModule(const std::string& module_name); +void goInitPose(); +void setLED(int led); +bool checkManagerRunning(std::string& manager_name); +void torqueOnAll(); +void torqueOff(const std::string& body_side); + +enum ControlModule +{ + None = 0, + DirectControlModule = 1, + Framework = 2, +}; + +const int SPIN_RATE = 30; +const bool DEBUG_PRINT = false; + +ros::Publisher init_pose_pub; +ros::Publisher sync_write_pub; +ros::Publisher dxl_torque_pub; +ros::Publisher write_joint_pub; +ros::Publisher write_joint_pub2; +ros::Subscriber buttuon_sub; +ros::Subscriber read_joint_sub; + +ros::ServiceClient set_joint_module_client; + +int control_module = None; +bool demo_ready = false; + +//node main +int main(int argc, char **argv) +{ + //init ros + ros::init(argc, argv, "read_write"); + + ros::NodeHandle nh(ros::this_node::getName()); + + init_pose_pub = nh.advertise("/robotis/base/ini_pose", 0); + sync_write_pub = nh.advertise("/robotis/sync_write_item", 0); + dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); + write_joint_pub = nh.advertise("/robotis/set_joint_states", 0); + write_joint_pub2 = nh.advertise("/robotis/direct_control/set_joint_states", 0); + + read_joint_sub = nh.subscribe("/robotis/present_joint_states", 1, jointstatesCallback); + buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + + // service + set_joint_module_client = nh.serviceClient("/robotis/set_present_ctrl_modules"); + + ros::start(); + + //set node loop rate + ros::Rate loop_rate(SPIN_RATE); + + // wait for starting of op3_manager + std::string manager_name = "/op3_manager"; + while (ros::ok()) + { + ros::Duration(1.0).sleep(); + + if (checkManagerRunning(manager_name) == true) + { + break; + ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); + } + ROS_WARN("Waiting for op3 manager"); + } + + readyToDemo(); + + //node loop + while (ros::ok()) + { + // process + + //execute pending callbacks + ros::spinOnce(); + + //relax to fit output rate + loop_rate.sleep(); + } + + //exit program + return 0; +} + +void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) +{ + // starting demo using robotis_controller + if (msg->data == "mode") + { + control_module = Framework; + ROS_INFO("Button : mode | Framework"); + readyToDemo(); + } + // starting demo using direct_control_module + else if (msg->data == "start") + { + control_module = DirectControlModule; + ROS_INFO("Button : start | Direct control module"); + readyToDemo(); + } + // torque on all joints of ROBOTIS-OP3 + else if (msg->data == "user") + { + torqueOnAll(); + control_module = None; + } +} + +void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg) +{ + if(control_module == None) + return; + + sensor_msgs::JointState write_msg; + write_msg.header = msg->header; + + for(int ix = 0; ix < msg->name.size(); ix++) + { + std::string joint_name = msg->name[ix]; + double joint_position = msg->position[ix]; + + // mirror and copy joint angles from right to left + if(joint_name == "r_sho_pitch") + { + write_msg.name.push_back("r_sho_pitch"); + write_msg.position.push_back(joint_position); + write_msg.name.push_back("l_sho_pitch"); + write_msg.position.push_back(-joint_position); + } + if(joint_name == "r_sho_roll") + { + write_msg.name.push_back("r_sho_roll"); + write_msg.position.push_back(joint_position); + write_msg.name.push_back("l_sho_roll"); + write_msg.position.push_back(-joint_position); + } + if(joint_name == "r_el") + { + write_msg.name.push_back("r_el"); + write_msg.position.push_back(joint_position); + write_msg.name.push_back("l_el"); + write_msg.position.push_back(-joint_position); + } + } + + // publish a message to set the joint angles + if(control_module == Framework) + write_joint_pub.publish(write_msg); + else if(control_module == DirectControlModule) + write_joint_pub2.publish(write_msg); +} + +void readyToDemo() +{ + ROS_INFO("Start Read-Write Demo"); + // turn off LED + setLED(0x04); + + torqueOnAll(); + ROS_INFO("Torque on All joints"); + + // send message for going init posture + goInitPose(); + ROS_INFO("Go Init pose"); + + // wait while ROBOTIS-OP3 goes to the init posture. + ros::Duration(4.0).sleep(); + + // turn on R/G/B LED [0x01 | 0x02 | 0x04] + setLED(control_module); + + // change the module for demo + if(control_module == Framework) + { + setModule("none"); + ROS_INFO("Change module to none"); + } + else if(control_module == DirectControlModule) + { + setModule("direct_control_module"); + ROS_INFO("Change module to direct_control_module"); + } + else + return; + + // torque off : right arm + torqueOff("right"); + ROS_INFO("Torque off"); +} + +void goInitPose() +{ + std_msgs::String init_msg; + init_msg.data = "ini_pose"; + + init_pose_pub.publish(init_msg); +} + +void setLED(int led) +{ + robotis_controller_msgs::SyncWriteItem syncwrite_msg; + syncwrite_msg.item_name = "LED"; + syncwrite_msg.joint_name.push_back("open-cr"); + syncwrite_msg.value.push_back(led); + + sync_write_pub.publish(syncwrite_msg); +} + +bool checkManagerRunning(std::string& manager_name) +{ + std::vector node_list; + ros::master::getNodes(node_list); + + for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++) + { + if (node_list[node_list_idx] == manager_name) + return true; + } + + ROS_ERROR("Can't find op3_manager"); + return false; +} + +void setModule(const std::string& module_name) +{ + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; + + if (set_joint_module_client.call(set_module_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } + + return ; +} + +void torqueOnAll() +{ + std_msgs::String check_msg; + check_msg.data = "check"; + + dxl_torque_pub.publish(check_msg); +} + +void torqueOff(const std::string& body_side) +{ + robotis_controller_msgs::SyncWriteItem syncwrite_msg; + int torque_value = 0; + syncwrite_msg.item_name = "torque_enable"; + + if(body_side == "right") + { + syncwrite_msg.joint_name.push_back("r_sho_pitch"); + syncwrite_msg.value.push_back(torque_value); + syncwrite_msg.joint_name.push_back("r_sho_roll"); + syncwrite_msg.value.push_back(torque_value); + syncwrite_msg.joint_name.push_back("r_el"); + syncwrite_msg.value.push_back(torque_value); + } + else if(body_side == "left") + { + syncwrite_msg.joint_name.push_back("l_sho_pitch"); + syncwrite_msg.value.push_back(torque_value); + syncwrite_msg.joint_name.push_back("l_sho_roll"); + syncwrite_msg.value.push_back(torque_value); + syncwrite_msg.joint_name.push_back("l_el"); + syncwrite_msg.value.push_back(torque_value); + } + else + return; + + sync_write_pub.publish(syncwrite_msg); +} diff --git a/robotis_op3_demo/CHANGELOG.rst b/robotis_op3_demo/CHANGELOG.rst new file mode 100644 index 0000000..c761f4a --- /dev/null +++ b/robotis_op3_demo/CHANGELOG.rst @@ -0,0 +1,14 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_op3_demo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2018-04-19) +------------------ +* first release for ROS Kinetic +* added launch files in order to move the camera setting to op3_camera_setting package +* added missing package in find_package() +* updated CMakeLists.txt and package.xml of op3_bringup +* changed rviz config file +* refacoring to release +* split repositoryfrom ROBOTIS-OP3 +* Contributors: Kayman, Zerom, Yoshimaru Tanaka, Pyo diff --git a/robotis_op3_demo/CMakeLists.txt b/robotis_op3_demo/CMakeLists.txt index d88f653..b7e8eeb 100644 --- a/robotis_op3_demo/CMakeLists.txt +++ b/robotis_op3_demo/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_op3_demo) find_package(catkin REQUIRED) -catkin_metapackage() \ No newline at end of file +catkin_metapackage() diff --git a/robotis_op3_demo/package.xml b/robotis_op3_demo/package.xml index 112c0e7..83e6185 100644 --- a/robotis_op3_demo/package.xml +++ b/robotis_op3_demo/package.xml @@ -1,24 +1,20 @@ - + robotis_op3_demo 0.1.0 ROS packages for the robotis_op3_demo (meta package) - - BSD + Apache 2.0 Kayman Pyo - - - - - - catkin - ball_detector - op3_demo - - - - - \ No newline at end of file + http://wiki.ros.org/robotis_op3_demo + http://emanual.robotis.com/docs/en/platform/op3/introduction/ + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo + https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + op3_ball_detector + op3_bringup + op3_demo + +