Merge branch 'develop' of https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo into develop
This commit is contained in:
Kayman
2
.robotis_op3_demo.rosinstall
Normal file
2
.robotis_op3_demo.rosinstall
Normal file
@@ -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}
|
||||
35
.travis.yml
Normal file
35
.travis.yml
Normal file
@@ -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
|
||||
|
||||
227
LICENSE
227
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.
|
||||
|
||||
36
README.md
Normal file
36
README.md
Normal file
@@ -0,0 +1,36 @@
|
||||
# ROBOTIS OP3
|
||||
<img src="https://github.com/ROBOTIS-GIT/emanual/blob/master/assets/images/platform/op3/default_op3.jpg" width="40%" />
|
||||
|
||||
## 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/)
|
||||
@@ -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
|
||||
################################################################################
|
||||
@@ -1,13 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="config_path" default="$(find ball_detector)/launch/ball_detector_params.yaml"/>
|
||||
|
||||
<!-- ball detector -->
|
||||
<node pkg="ball_detector" type="ball_detector_node" name="ball_detector_node" args="" output="screen">
|
||||
<rosparam command="load" file="$(arg config_path)"/>
|
||||
<param name="yaml_path" type="string" value="$(arg config_path)"/>
|
||||
<remap from="/ball_detector_node/image_in" to="/usb_cam_node/image_raw" />
|
||||
<remap from="/ball_detector_node/cameraInfo_in" to="/usb_cam_node/camera_info" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- UVC camera -->
|
||||
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
|
||||
<param name="video_device" type="string" value="/dev/video0" />
|
||||
<param name="image_width" type="int" value="800" />
|
||||
<param name="image_height" type="int" value="600" />
|
||||
<!-- <param name="image_width" type="int" value="1600" />
|
||||
<param name="image_height" type="int" value="896" /> -->
|
||||
<param name="framerate " type="int" value="30" />
|
||||
<param name="camera_name" type="string" value="camera" />
|
||||
<param name="autofocus" type="bool" value="False" />
|
||||
<param name="autoexposure" type="bool" value="False" />
|
||||
<param name="auto_white_balance" type="bool" value="False" />
|
||||
<param name="gain" value="255" />
|
||||
<param name="brightness" value="64" />
|
||||
<param name="exposure" value="80" />
|
||||
</node>
|
||||
<!-- <param name="gain" value="255" />
|
||||
<param name="auto_exposure" type="bool" value="False" />
|
||||
<param name="exposure_absolute" value="1000" />
|
||||
<param name="brightness" value="127" />
|
||||
<param name="auto_white_balance" type="bool" value="False" />
|
||||
<param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
|
||||
<!-- <param name="exposure_absolute" value="1000" /> -->
|
||||
<!-- <param name="brightness" value="64" /> -->
|
||||
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
|
||||
<!-- <param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector.launch" />
|
||||
|
||||
</launch>
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- UVC camera -->
|
||||
<node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera_node" output="screen">
|
||||
<param name="device" type="string" value="/dev/video0" />
|
||||
<param name="width" type="int" value="800" />
|
||||
<param name="height" type="int" value="600" />
|
||||
<param name="fps" type="int" value="30" />
|
||||
<param name="auto_gain" value="false" />
|
||||
<param name="gain" type="int" value="120" />
|
||||
<param name="exposure" value="100" />
|
||||
</node>
|
||||
<!-- <param name="gain" value="255" />
|
||||
<param name="auto_exposure" type="bool" value="False" />
|
||||
<param name="exposure_absolute" value="1000" />
|
||||
<param name="brightness" value="127" />
|
||||
<param name="auto_white_balance" type="bool" value="False" />
|
||||
<param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
|
||||
<!-- <param name="exposure_absolute" value="1000" /> -->
|
||||
<!-- <param name="brightness" value="64" /> -->
|
||||
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
|
||||
<!-- <param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector.launch" />
|
||||
</launch>
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>ball_detector</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
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.
|
||||
</description>
|
||||
<license>BSD</license>
|
||||
<author email="kmjung@robotis.com">kayman</author>
|
||||
<maintainer email="pyo@robotis.com">Pyo</maintainer>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>cv_bridge</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>image_transport</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>dynamic_reconfigure</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<run_depend>cv_bridge</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>image_transport</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>dynamic_reconfigure</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
</package>
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
12
op3_ball_detector/CHANGELOG.rst
Normal file
12
op3_ball_detector/CHANGELOG.rst
Normal file
@@ -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
|
||||
136
op3_ball_detector/CMakeLists.txt
Normal file
136
op3_ball_detector/CMakeLists.txt
Normal file
@@ -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 <yaml-cpp/yaml.h>'
|
||||
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
|
||||
################################################################################
|
||||
4
ball_detector/cfg/detector_params.cfg → op3_ball_detector/cfg/DetectorParams.cfg
Normal file → Executable file
4
ball_detector/cfg/detector_params.cfg → op3_ball_detector/cfg/DetectorParams.cfg
Normal file → Executable file
@@ -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"))
|
||||
4
ball_detector/cfg/detector_params_blue.cfg → op3_ball_detector/cfg/DetectorParamsBlue.cfg
Normal file → Executable file
4
ball_detector/cfg/detector_params_blue.cfg → op3_ball_detector/cfg/DetectorParamsBlue.cfg
Normal file → Executable file
@@ -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"))
|
||||
4
ball_detector/cfg/detector_params_red.cfg → op3_ball_detector/cfg/DetectorParamsRed.cfg
Normal file → Executable file
4
ball_detector/cfg/detector_params_red.cfg → op3_ball_detector/cfg/DetectorParamsRed.cfg
Normal file → Executable file
@@ -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"))
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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 <string>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
//ros dependencies
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
|
||||
#include "ball_detector/circleSetStamped.h"
|
||||
#include "ball_detector/ball_detector_config.h"
|
||||
#include "ball_detector/detectorParamsConfig.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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<ball_detector::detectorParamsConfig> param_server_;
|
||||
dynamic_reconfigure::Server<ball_detector::detectorParamsConfig>::CallbackType callback_fnc_;
|
||||
dynamic_reconfigure::Server<op3_ball_detector::DetectorParamsConfig> param_server_;
|
||||
dynamic_reconfigure::Server<op3_ball_detector::DetectorParamsConfig>::CallbackType callback_fnc_;
|
||||
};
|
||||
|
||||
} // namespace robotis_op
|
||||
@@ -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 */
|
||||
|
||||
13
op3_ball_detector/launch/ball_detector.launch
Normal file
13
op3_ball_detector/launch/ball_detector.launch
Normal file
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="config_path" default="$(find op3_ball_detector)/config/ball_detector_params.yaml"/>
|
||||
|
||||
<!-- ball detector -->
|
||||
<node pkg="op3_ball_detector" type="ball_detector_node" name="ball_detector_node" args="" output="screen">
|
||||
<rosparam command="load" file="$(arg config_path)"/>
|
||||
<param name="yaml_path" type="string" value="$(arg config_path)"/>
|
||||
<remap from="/ball_detector_node/image_in" to="/usb_cam_node/image_raw" />
|
||||
<remap from="/ball_detector_node/cameraInfo_in" to="/usb_cam_node/camera_info" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
27
op3_ball_detector/launch/ball_detector_from_usb_cam.launch
Normal file
27
op3_ball_detector/launch/ball_detector_from_usb_cam.launch
Normal file
@@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- UVC camera -->
|
||||
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
|
||||
<param name="video_device" type="string" value="/dev/video0" />
|
||||
<param name="image_width" type="int" value="1280" />
|
||||
<param name="image_height" type="int" value="720" />
|
||||
<param name="framerate " type="int" value="30" />
|
||||
<param name="camera_frame_id" type="string" value="cam_link" />
|
||||
<param name="camera_name" type="string" value="camera" />
|
||||
<!-- <param name="autofocus" type="bool" value="False" /> -->
|
||||
<!-- <param name="autoexposure" type="bool" value="False" /> -->
|
||||
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
|
||||
<!-- <param name="gain" value="255" /> -->
|
||||
<!-- <param name="brightness" value="64" /> -->
|
||||
<!-- <param name="exposure" value="80" /> -->
|
||||
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
|
||||
<!-- <param name="exposure_absolute" value="1000" /> -->
|
||||
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
|
||||
<!-- <param name="white_balance_temperature" value="2800" /> -->
|
||||
<!-- <param name="camera_info_url" type="string" value="file://$(find ar_pose)/data/camera_1280720.yaml" /> -->
|
||||
</node>
|
||||
|
||||
<!-- ball detector -->
|
||||
<include file="$(find op3_ball_detector)/launch/ball_detector.launch" />
|
||||
</launch>
|
||||
30
op3_ball_detector/launch/ball_detector_from_uvc.launch
Normal file
30
op3_ball_detector/launch/ball_detector_from_uvc.launch
Normal file
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- UVC camera -->
|
||||
<node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera_node" output="screen">
|
||||
<param name="device" type="string" value="/dev/video0" />
|
||||
<param name="width" type="int" value="800" />
|
||||
<param name="height" type="int" value="600" />
|
||||
<param name="fps" type="int" value="30" />
|
||||
<param name="auto_gain" value="false" />
|
||||
<param name="gain" type="int" value="120" />
|
||||
<param name="exposure" value="100" />
|
||||
</node>
|
||||
<!-- <param name="gain" value="255" />
|
||||
<param name="auto_exposure" type="bool" value="False" />
|
||||
<param name="exposure_absolute" value="1000" />
|
||||
<param name="brightness" value="127" />
|
||||
<param name="auto_white_balance" type="bool" value="False" />
|
||||
<param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
|
||||
<!-- <param name="exposure_absolute" value="1000" /> -->
|
||||
<!-- <param name="brightness" value="64" /> -->
|
||||
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
|
||||
<!-- <param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- ball detector -->
|
||||
<include file="$(find op3_ball_detector)/launch/ball_detector.launch" />
|
||||
</launch>
|
||||
|
||||
17
op3_ball_detector/msg/BallDetectorParams.msg
Normal file
17
op3_ball_detector/msg/BallDetectorParams.msg
Normal file
@@ -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
|
||||
35
op3_ball_detector/package.xml
Normal file
35
op3_ball_detector/package.xml
Normal file
@@ -0,0 +1,35 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>op3_ball_detector</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
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.
|
||||
</description>
|
||||
<license>Apache 2.0</license>
|
||||
<author email="kmjung@robotis.com">Kayman</author>
|
||||
<author email="zerom@robotis.com">Zerom</author>
|
||||
<maintainer email="pyo@robotis.com">Pyo</maintainer>
|
||||
<url type="website">http://wiki.ros.org/op3_ball_detector</url>
|
||||
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
|
||||
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
|
||||
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>roslib</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>boost</depend>
|
||||
<depend>opencv3</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_export_depend>message_runtime</build_export_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<exec_depend>usb_cam</exec_depend>
|
||||
<exec_depend>uvc_camera</exec_depend>
|
||||
</package>
|
||||
@@ -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 <yaml-cpp/yaml.h>
|
||||
#include <fstream>
|
||||
|
||||
#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<ball_detector::circleSetStamped>("circle_set", 100);
|
||||
circles_pub_ = nh_.advertise<op3_ball_detector::CircleSetStamped>("circle_set", 100);
|
||||
camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("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<op3_ball_detector::BallDetectorParams>("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<int>();
|
||||
params_config_.gaussian_blur_sigma = doc["gaussian_blur_sigma"].as<double>();
|
||||
params_config_.canny_edge_th = doc["canny_edge_th"].as<double>();
|
||||
params_config_.hough_accum_resolution = doc["hough_accum_resolution"].as<double>();
|
||||
params_config_.min_circle_dist = doc["min_circle_dist"].as<double>();
|
||||
params_config_.hough_accum_th = doc["hough_accum_th"].as<double>();
|
||||
params_config_.min_radius = doc["min_radius"].as<int>();
|
||||
params_config_.max_radius = doc["max_radius"].as<int>();
|
||||
params_config_.filter_threshold.h_min = doc["filter_h_min"].as<int>();
|
||||
params_config_.filter_threshold.h_max = doc["filter_h_max"].as<int>();
|
||||
params_config_.filter_threshold.s_min = doc["filter_s_min"].as<int>();
|
||||
params_config_.filter_threshold.s_max = doc["filter_s_max"].as<int>();
|
||||
params_config_.filter_threshold.v_min = doc["filter_v_min"].as<int>();
|
||||
params_config_.filter_threshold.v_max = doc["filter_v_max"].as<int>();
|
||||
params_config_.use_second_filter = doc["use_second_filter"].as<bool>();
|
||||
params_config_.filter2_threshold.h_min = doc["filter2_h_min"].as<int>();
|
||||
params_config_.filter2_threshold.h_max = doc["filter2_h_max"].as<int>();
|
||||
params_config_.filter2_threshold.s_min = doc["filter2_s_min"].as<int>();
|
||||
params_config_.filter2_threshold.s_max = doc["filter2_s_max"].as<int>();
|
||||
params_config_.filter2_threshold.v_min = doc["filter2_v_min"].as<int>();
|
||||
params_config_.filter2_threshold.v_max = doc["filter2_v_max"].as<int>();
|
||||
params_config_.ellipse_size = doc["ellipse_size"].as<int>();
|
||||
params_config_.debug = doc["filter_debug"].as<bool>();
|
||||
|
||||
// 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<cv::Vec3f> circles_current;
|
||||
std::vector<cv::Vec3f> 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++)
|
||||
{
|
||||
55
op3_ball_detector/src/ball_detector_node.cpp
Normal file
55
op3_ball_detector/src/ball_detector_node.cpp
Normal file
@@ -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;
|
||||
}
|
||||
|
||||
3
op3_ball_detector/srv/GetParameters.srv
Normal file
3
op3_ball_detector/srv/GetParameters.srv
Normal file
@@ -0,0 +1,3 @@
|
||||
|
||||
---
|
||||
BallDetectorParams returns
|
||||
3
op3_ball_detector/srv/SetParameters.srv
Normal file
3
op3_ball_detector/srv/SetParameters.srv
Normal file
@@ -0,0 +1,3 @@
|
||||
BallDetectorParams params
|
||||
---
|
||||
BallDetectorParams returns
|
||||
11
op3_bringup/CHANGELOG.rst
Normal file
11
op3_bringup/CHANGELOG.rst
Normal file
@@ -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
|
||||
42
op3_bringup/CMakeLists.txt
Normal file
42
op3_bringup/CMakeLists.txt
Normal file
@@ -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
|
||||
################################################################################
|
||||
15
op3_bringup/launch/op3_bringup.launch
Normal file
15
op3_bringup/launch/op3_bringup.launch
Normal file
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<!-- OP3 Manager -->
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch" />
|
||||
|
||||
<!-- UVC camera -->
|
||||
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
|
||||
<param name="video_device" type="string" value="/dev/video0" />
|
||||
<param name="image_width" type="int" value="1280" />
|
||||
<param name="image_height" type="int" value="720" />
|
||||
<param name="framerate " type="int" value="30" />
|
||||
<param name="camera_frame_id" type="string" value="cam_link" />
|
||||
<param name="camera_name" type="string" value="camera" />
|
||||
</node>
|
||||
</launch>
|
||||
18
op3_bringup/launch/op3_bringup_visualization.launch
Normal file
18
op3_bringup/launch/op3_bringup_visualization.launch
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py '$(find op3_description)/urdf/robotis_op3.urdf.xacro'"/>
|
||||
|
||||
<!-- Send fake joint values and monitoring present joint angle -->
|
||||
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
|
||||
<param name="use_gui" value="TRUE"/>
|
||||
<rosparam param="/source_list">[/robotis/present_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Combine joint values -->
|
||||
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
|
||||
<remap from="/joint_states" to="/robotis/present_joint_states" />
|
||||
</node>
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find op3_bringup)/rviz/op3_bringup.rviz"/>
|
||||
</launch>
|
||||
23
op3_bringup/package.xml
Normal file
23
op3_bringup/package.xml
Normal file
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>op3_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
This package is a demo for first time users.
|
||||
There is an example in the demo where you can run and visualize the robot.
|
||||
</description>
|
||||
<license>Apache 2.0</license>
|
||||
<author email="kmjung@robotis.com">Kayman</author>
|
||||
<maintainer email="pyo@robotis.com">Pyo</maintainer>
|
||||
<url type="website">http://wiki.ros.org/op3_bringup</url>
|
||||
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
|
||||
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
|
||||
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<exec_depend>op3_manager</exec_depend>
|
||||
<exec_depend>op3_description</exec_depend>
|
||||
<exec_depend>usb_cam</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>rviz</exec_depend>
|
||||
</package>
|
||||
357
op3_bringup/rviz/op3_bringup.rviz
Normal file
357
op3_bringup/rviz/op3_bringup.rviz
Normal file
@@ -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: <Fixed 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: <Fixed 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
|
||||
12
op3_demo/CHANGELOG.rst
Normal file
12
op3_demo/CHANGELOG.rst
Normal file
@@ -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
|
||||
@@ -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 <yaml-cpp/yaml.h>'
|
||||
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
|
||||
################################################################################
|
||||
################################################################################
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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 <boost/thread.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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<int, std::string> action_sound_table_;
|
||||
std::vector<int> play_list_;
|
||||
|
||||
@@ -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 <math.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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_;
|
||||
|
||||
|
||||
@@ -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 <math.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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_;
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
@@ -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 <boost/thread.hpp>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include "op3_demo/op_demo.h"
|
||||
|
||||
#include "robotis_controller_msgs/SyncWriteItem.h"
|
||||
|
||||
namespace robotis_op
|
||||
|
||||
@@ -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 <math.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
namespace robotis_op
|
||||
{
|
||||
|
||||
@@ -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 <signal.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include "op3_demo/op_demo.h"
|
||||
|
||||
#include "robotis_controller_msgs/SyncWriteItem.h"
|
||||
|
||||
namespace robotis_op
|
||||
|
||||
@@ -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 */
|
||||
|
||||
|
||||
@@ -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 <std_msgs/String.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <eigen3/Eigen/Eigen>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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<int, std::string> id_joint_table_;
|
||||
std::map<std::string, int> 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_;
|
||||
|
||||
@@ -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 <boost/thread.hpp>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <std_msgs/Int32MultiArray.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#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_;
|
||||
};
|
||||
|
||||
|
||||
@@ -1,12 +1,10 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- robotis op3 manager -->
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
|
||||
<include file="$(find op3_ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
|
||||
|
||||
<!-- face tracking -->
|
||||
<include file="$(find op3_demo)/launch/face_detection_op3.launch" />
|
||||
@@ -16,10 +14,15 @@
|
||||
|
||||
<!-- sound player -->
|
||||
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
|
||||
|
||||
|
||||
<!-- web setting -->
|
||||
<include file="$(find op3_web_setting_tool)/launch/web_setting_server.launch" />
|
||||
|
||||
<!-- robotis op3 demo -->
|
||||
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen">
|
||||
<param name="grass_demo" type="bool" value="False" />
|
||||
<param name="p_gain" value="0.45" />
|
||||
<param name="d_gain" value="0.045" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
|
||||
@@ -7,15 +7,16 @@
|
||||
<arg name="face_cascade_name_4" default="$(find face_detection)/include/face_detection/lbpCascades/lbpcascade_frontalface.xml" />
|
||||
|
||||
<node pkg="face_detection" type="face_tracking" name="face_tracking"
|
||||
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">
|
||||
<param name="imageInput" type="string" value="/usb_cam_node/image_raw" />
|
||||
<param name="displayed_Image" type="int" value="0" />
|
||||
<!-- <param name="publish" type="int" value="2" /> -->
|
||||
<param name="publish" type="int" value="3" />
|
||||
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">
|
||||
<param name="imageInput" type="string" value="/usb_cam_node/image_raw" />
|
||||
<param name="displayed_Image" type="int" value="0" />
|
||||
<!-- <param name="publish" type="int" value="2" /> -->
|
||||
<param name="publish" type="int" value="3" />
|
||||
<param name="start_condition" type="bool" value="false" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -1,23 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- robotis op3 manager -->
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
|
||||
<!-- robotis op3 manager -->
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
|
||||
<!-- Camera and Ball detector -->
|
||||
<include file="$(find op3_ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
|
||||
|
||||
<!-- face tracking -->
|
||||
<include file="$(find op3_demo)/launch/face_detection_op3.launch" />
|
||||
<!-- face tracking -->
|
||||
<include file="$(find op3_demo)/launch/face_detection_op3.launch" />
|
||||
|
||||
<!-- camera setting tool -->
|
||||
<include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
|
||||
<!-- camera setting tool -->
|
||||
<include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
|
||||
|
||||
<!-- sound player -->
|
||||
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
|
||||
<!-- sound player -->
|
||||
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
|
||||
|
||||
<!-- robotis op3 self test demo -->
|
||||
<node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen"/>
|
||||
<!-- web setting -->
|
||||
<include file="$(find op3_web_setting_tool)/launch/web_setting_server.launch" />
|
||||
|
||||
<!-- robotis op3 self test demo -->
|
||||
<node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen">
|
||||
<param name="grass_demo" type="bool" value="False" />
|
||||
<param name="p_gain" value="0.45" />
|
||||
<param name="d_gain" value="0.045" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
|
||||
23
op3_demo/list/action_script.yaml
Normal file
23
op3_demo/list/action_script.yaml
Normal file
@@ -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]
|
||||
@@ -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]
|
||||
@@ -1,28 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<package format="2">
|
||||
<name>op3_demo</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
op3 default demo
|
||||
OP3 default demo
|
||||
It includes three demontrations(soccer demo, vision demo, action script demo)
|
||||
</description>
|
||||
<license>BSD</license>
|
||||
<author email="kmjung@robotis.com">kayman</author>
|
||||
<license>Apache 2.0</license>
|
||||
<author email="kmjung@robotis.com">Kayman</author>
|
||||
<maintainer email="pyo@robotis.com">Pyo</maintainer>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>roslib</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>ball_detector</build_depend>
|
||||
<build_depend>op3_walking_module_msgs</build_depend>
|
||||
<build_depend>cmake_modules</build_depend>
|
||||
<build_depend>robotis_math</build_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>roslib</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>ball_detector</run_depend>
|
||||
<run_depend>op3_walking_module_msgs</run_depend>
|
||||
<run_depend>cmake_modules</run_depend>
|
||||
<run_depend>robotis_math</run_depend>
|
||||
</package>
|
||||
<url type="website">http://wiki.ros.org/op3_demo</url>
|
||||
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
|
||||
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
|
||||
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>roslib</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>robotis_controller_msgs</depend>
|
||||
<depend>op3_walking_module_msgs</depend>
|
||||
<depend>op3_action_module_msgs</depend>
|
||||
<depend>cmake_modules</depend>
|
||||
<depend>robotis_math</depend>
|
||||
<depend>op3_ball_detector</depend>
|
||||
<depend>boost</depend>
|
||||
<depend>eigen</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
<exec_depend>op3_manager</exec_depend>
|
||||
<exec_depend>op3_camera_setting_tool</exec_depend>
|
||||
<exec_depend>op3_web_setting_tool</exec_depend>
|
||||
<exec_depend>ros_madplay_player</exec_depend>
|
||||
<!-- <exec_depend>face_detection</exec_depend> -->
|
||||
</package>
|
||||
|
||||
@@ -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]
|
||||
@@ -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<std::string>("action_script", default_path);
|
||||
|
||||
std::string default_play_list = "default";
|
||||
play_list_name_ = nh.param<std::string>("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<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
|
||||
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetModule>("/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 */
|
||||
|
||||
@@ -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_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
|
||||
dxl_torque_pub = nh.advertise<std_msgs::String>("/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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user