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|
+|:---:|:---:|:---:|
+|[](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo)|[](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
+
+