diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py
index 91a9896..6d4beeb 100644
--- a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py
+++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py
@@ -13,7 +13,6 @@
# You should have received a copy of the GNU General Public License
# along with this program. If not, see .
-#!/usr/bin/env python3
import os
import sys
@@ -26,7 +25,7 @@ def ros1_launch_description():
args = sys.argv[1:]
# Construct the ROS 1 launch command
- roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args
+ roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args
roslaunch_command.extend([
"usb_cam", "usb_cam_node", "name:=camera",
@@ -39,21 +38,22 @@ def ros1_launch_description():
])
roslaunch_command.extend([
- "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node"
+ "humanoid_robot_intelligence_control_system_face_tracker", "face_detection_processor.py", "name:=face_detection_processor_node"
])
roslaunch_command.extend([
- "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node"
+ "humanoid_robot_intelligence_control_system_configure", "face_tracker.py", "name:=face_tracker_node"
])
roslaunch_command.extend([
"rviz", "rviz", "name:=rviz",
- "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz"
+ "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz"
])
# Execute the launch command
subprocess.call(roslaunch_command)
+
def ros2_launch_description():
nodes_to_launch = []
@@ -73,17 +73,17 @@ def ros2_launch_description():
))
nodes_to_launch.append(Node(
- package='humanoid_robot_intelligence_control_system_object_detector',
- executable='object_detection_processor.py',
- name='object_detection_processor_node',
+ package='humanoid_robot_intelligence_control_system_face_tracker',
+ executable='face_detection_processor.py',
+ name='face_detection_processor_node',
output='screen',
- parameters=[{'yaml_path': '$(find humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}]
+ parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}]
))
nodes_to_launch.append(Node(
- package='humanoid_robot_intelligence_control_system_object_detector',
- executable='object_tracker.py',
- name='object_tracker_node',
+ package='humanoid_robot_intelligence_control_system_configure',
+ executable='face_tracker.py',
+ name='face_tracker_node',
output='screen',
parameters=[{
'p_gain': 0.4,
@@ -96,7 +96,7 @@ def ros2_launch_description():
package='rviz2',
executable='rviz2',
name='rviz',
- arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz']
+ arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz']
))
return LaunchDescription(nodes_to_launch)
diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py
index 9e0beef..892e0cc 100644
--- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py
+++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py
@@ -42,7 +42,7 @@ def ros1_launch_description():
])
roslaunch_command.extend([
- "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node"
+ "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node"
])
roslaunch_command.extend([
@@ -82,8 +82,8 @@ def ros2_launch_description():
nodes_to_launch.append(Node(
package='humanoid_robot_intelligence_control_system_configure',
- executable='object_follower.py',
- name='object_follower_node',
+ executable='object_tracker.py',
+ name='object_tracker_node',
output='screen',
parameters=[{
'p_gain': 0.4,
diff --git a/humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst
deleted file mode 100644
index 39a2f89..0000000
--- a/humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst
+++ /dev/null
@@ -1,34 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package humanoid_robot_intelligence_control_system_demo
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.2 (2023-10-03)
-------------------
-* Make it compatible for ROS1/ROS2
-* Fix bugs
-* Update package.xml and CMakeList.txt for to the latest versions
-* Contributors & Maintainer: Ronaldson Bellande
-
-0.3.1 (2023-09-27)
-------------------
-* Starting from this point it under a new license
-* Fix errors and Issues
-* Rename Repository for a completely different purpose
-* Make it compatible with ROS/ROS2
-* Upgrade version of all builds and make it more compatible
-* Update package.xml and CMakeList.txt for to the latest versions
-* Contributors & Maintainer: Ronaldson Bellande
-
-0.3.0 (2021-05-05)
-------------------
-* Update package.xml and CMakeList.txt for noetic branch
-* Contributors: Ronaldson Bellande
-
-0.1.0 (2018-04-19)
-------------------
-* first release for ROS Kinetic
-* added launch files in order to move the camera setting to humanoid_robot_intelligence_control_system_camera_setting package
-* added missing package in find_package()
-* refacoring to release
-* split repositoryfrom ROBOTIS-HUMANOID_ROBOT
-* Contributors: Kayman, Zerom, Yoshimaru Tanaka, Pyo
diff --git a/humanoid_robot_intelligence_control_system_demo/CMakeLists.txt b/humanoid_robot_intelligence_control_system_demo/CMakeLists.txt
deleted file mode 100644
index 1a98f2b..0000000
--- a/humanoid_robot_intelligence_control_system_demo/CMakeLists.txt
+++ /dev/null
@@ -1,153 +0,0 @@
-# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
-#
-# Licensed under the Apache License, Version 2.0 (the "License"); you may not
-# use this file except in compliance with the License. You may obtain a copy of
-# the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
-# License for the specific language governing permissions and limitations under
-# the License.
-
-cmake_minimum_required(VERSION 3.8)
-project(humanoid_robot_intelligence_control_system_demo)
-
-
-if($ENV{ROS_VERSION} EQUAL 1)
- find_package(
- catkin REQUIRED COMPONENTS
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
- humanoid_robot_intelligence_control_system_controller_msgs
- humanoid_robot_intelligence_control_system_walking_module_msgs
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
- humanoid_robot_intelligence_control_system_object_detector
- )
- find_package(Boost REQUIRED COMPONENTS thread)
- find_package(Eigen3 REQUIRED)
-else()
- find_package(ament_cmake REQUIRED)
-endif()
-
-
-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")
-
-
-if($ENV{ROS_VERSION} EQUAL 1)
- catkin_package(
- INCLUDE_DIRS include
- CATKIN_DEPENDS
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
- humanoid_robot_intelligence_control_system_controller_msgs
- humanoid_robot_intelligence_control_system_walking_module_msgs
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
- humanoid_robot_intelligence_control_system_object_detector
- DEPENDS Boost EIGEN3
- )
-endif()
-
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${Boost_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIRS}
- ${YAML_CPP_INCLUDE_DIRS}
-)
-
-add_executable(
- op_demo_node
- src/demo_node.cpp
- src/soccer/soccer_demo.cpp
- src/soccer/ball_tracker.cpp
- src/soccer/ball_follower.cpp
- src/action/action_demo.cpp
- src/vision/vision_demo.cpp
- src/vision/face_tracker.cpp
-)
-
-add_dependencies(
- op_demo_node
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(
- op_demo_node
- ${catkin_LIBRARIES}
- ${Boost_LIBRARIES}
- ${Eigen3_LIBRARIES}
- ${YAML_CPP_LIBRARIES}
-)
-
-add_executable(
- self_test_node
- src/test_node.cpp
- src/soccer/soccer_demo.cpp
- src/soccer/ball_tracker.cpp
- src/soccer/ball_follower.cpp
- src/action/action_demo.cpp
- src/vision/vision_demo.cpp
- src/vision/face_tracker.cpp
- src/test/button_test.cpp
- src/test/mic_test.cpp
-)
-
-add_dependencies(
- self_test_node
- ${${PROJECT_NAME}_EXPORTED_TARGETS}
- ${catkin_EXPORTED_TARGETS}
-)
-
-target_link_libraries(
- self_test_node
- ${catkin_LIBRARIES}
- ${Boost_LIBRARIES}
- ${Eigen3_LIBRARIES}
- ${YAML_CPP_LIBRARIES}
-)
-
-install(
- TARGETS op_demo_node self_test_node
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(
- DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-install(
- DIRECTORY data launch list
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h
deleted file mode 100644
index b8ee165..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef ACTION_DEMO_H_
-#define ACTION_DEMO_H_
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_action_module_msgs/IsRunning.h"
-#include "humanoid_robot_intelligence_control_system_demo/op_demo.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-class ActionDemo : public OPDemo {
-public:
- ActionDemo();
- ~ActionDemo();
-
- void setDemoEnable();
- void setDemoDisable();
-
-protected:
- enum ActionCommandIndex {
- BrakeActionCommand = -2,
- StopActionCommand = -1,
- };
-
- enum ActionStatus {
- PlayAction = 1,
- PauseAction = 2,
- StopAction = 3,
- ReadyAction = 4,
- };
-
- const int SPIN_RATE;
- const bool DEBUG_PRINT;
-
- void processThread();
- void callbackThread();
-
- void process();
- void startProcess(const std::string &set_name = "default");
- void resumeProcess();
- void pauseProcess();
- void stopProcess();
-
- void handleStatus();
-
- void parseActionScript(const std::string &path);
- bool parseActionScriptSetName(const std::string &path,
- const std::string &set_name);
-
- bool playActionWithSound(int motion_index);
-
- void playMP3(std::string &path);
- void stopMP3();
-
- void playAction(int motion_index);
- void stopAction();
- void brakeAction();
- bool isActionRunning();
-
- void setModuleToDemo(const std::string &module_name);
- void callServiceSettingModule(const std::string &module_name);
- void actionSetNameCallback(const std_msgs::String::ConstPtr &msg);
- void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
- void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
-
- ros::Publisher module_control_pub_;
- ros::Publisher motion_index_pub_;
- ros::Publisher play_sound_pub_;
-
- ros::Subscriber buttuon_sub_;
- ros::Subscriber demo_command_sub_;
-
- ros::ServiceClient is_running_client_;
- ros::ServiceClient set_joint_module_client_;
-
- std::map action_sound_table_;
- std::vector play_list_;
-
- std::string script_path_;
- std::string play_list_name_;
- int play_index_;
-
- int play_status_;
-};
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
-
-#endif /* ACTION_DEMO_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h
deleted file mode 100644
index cb9ed78..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef BALL_FOLLOWER_H_
-#define BALL_FOLLOWER_H_
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_ball_detector/CircleSetStamped.h"
-#include "humanoid_robot_intelligence_control_system_walking_module_msgs/GetWalkingParam.h"
-#include "humanoid_robot_intelligence_control_system_walking_module_msgs/WalkingParam.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-// following the ball using walking
-class BallFollower {
-public:
- enum {
- NotFound = 0,
- OutOfRange = 1,
- OnRight = 2,
- OnLeft = 3,
- };
-
- BallFollower();
- ~BallFollower();
-
- bool processFollowing(double x_angle, double y_angle, double ball_size);
- void decideBallPositin(double x_angle, double y_angle);
- void waitFollowing();
- void startFollowing();
- void stopFollowing();
- void clearBallPosition() { approach_ball_position_ = NotFound; }
-
- int getBallPosition() { return approach_ball_position_; }
-
- bool isBallInRange() {
- return (approach_ball_position_ == OnRight ||
- approach_ball_position_ == OnLeft);
- }
-
-protected:
- const bool DEBUG_PRINT;
- const double CAMERA_HEIGHT;
- const int NOT_FOUND_THRESHOLD;
- const double FOV_WIDTH;
- const double FOV_HEIGHT;
- const double MAX_FB_STEP;
- const double MAX_RL_TURN;
- const double IN_PLACE_FB_STEP;
- const double MIN_FB_STEP;
- const double MIN_RL_TURN;
- const double UNIT_FB_STEP;
- const double UNIT_RL_TURN;
-
- const double SPOT_FB_OFFSET;
- const double SPOT_RL_OFFSET;
- const double SPOT_ANGLE_OFFSET;
-
- void currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
- void setWalkingCommand(const std::string &command);
- 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);
-
- // ros node handle
- ros::NodeHandle nh_;
-
- // image publisher/subscriber
- ros::Publisher module_control_pub_;
- ros::Publisher head_joint_pub_;
- ros::Publisher head_scan_pub_;
- ros::Publisher set_walking_command_pub_;
- ros::Publisher set_walking_param_pub_;
-
- ros::Publisher motion_index_pub_;
- ros::ServiceClient get_walking_param_client_;
-
- ros::Subscriber ball_position_sub_;
- ros::Subscriber ball_tracking_command_sub_;
- ros::Subscriber current_joint_states_sub_;
-
- // (x, y) is the center position of the ball in image coordinates
- // z is the ball radius
- geometry_msgs::Point ball_position_;
- humanoid_robot_intelligence_control_system_walking_module_msgs::WalkingParam current_walking_param_;
-
- int count_not_found_;
- int count_to_kick_;
- bool on_tracking_;
- int approach_ball_position_;
- double current_pan_, current_tilt_;
- double current_x_move_, current_r_angle_;
- int kick_motion_index_;
- double hip_pitch_offset_;
- ros::Time prev_time_;
-
- double curr_period_time_;
- double accum_period_time_;
-};
-} // namespace humanoid_robot_intelligence_control_system_op
-
-#endif /* BALL_FOLLOWER_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h
deleted file mode 100644
index 9ccceeb..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef BALL_TRACKING_H_
-#define BALL_TRACKING_H_
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_ball_detector/CircleSetStamped.h"
-#include "humanoid_robot_intelligence_control_system_walking_module_msgs/GetWalkingParam.h"
-#include "humanoid_robot_intelligence_control_system_walking_module_msgs/WalkingParam.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-// head tracking for looking the ball
-class BallTracker {
-public:
- enum TrackingStatus {
- NotFound = -1,
- Waiting = 0,
- Found = 1,
-
- };
- BallTracker();
- ~BallTracker();
-
- int processTracking();
-
- void startTracking();
- void stopTracking();
-
- void setUsingHeadScan(bool use_scan);
- void goInit();
-
- double getPanOfBall() {
- // left (+) ~ right (-)
- return current_ball_pan_;
- }
- double getTiltOfBall() {
- // top (+) ~ bottom (-)
- return current_ball_tilt_;
- }
- double getBallSize() { return current_ball_bottom_; }
-
-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 humanoid_robot_intelligence_control_system_ball_detector::CircleSetStamped::ConstPtr &msg);
- void ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg);
- void publishHeadJoint(double pan, double tilt);
- void scanBall();
-
- // ros node handle
- ros::NodeHandle nh_;
-
- // 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_;
- ros::Subscriber ball_tracking_command_sub_;
-
- // (x, y) is the center position of the ball in image coordinates
- // z is the ball radius
- geometry_msgs::Point ball_position_;
-
- int tracking_status_;
- bool use_head_scan_;
- int count_not_found_;
- 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_;
-};
-} // namespace humanoid_robot_intelligence_control_system_op
-
-#endif /* BALL_TRACKING_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h
deleted file mode 100644
index ead7c2b..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef BUTTON_TEST_H_
-#define BUTTON_TEST_H_
-
-#include
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_demo/op_demo.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-class ButtonTest : public OPDemo {
-public:
- ButtonTest();
- ~ButtonTest();
-
- void setDemoEnable();
- void setDemoDisable();
-
-protected:
- const int SPIN_RATE;
-
- void processThread();
- void callbackThread();
-
- void process();
-
- void setRGBLED(int blue, int green, int red);
- void setLED(int led);
-
- void playSound(const std::string &path);
-
- void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
-
- ros::Publisher rgb_led_pub_;
- ros::Publisher play_sound_pub_;
- ros::Subscriber buttuon_sub_;
-
- std::string default_mp3_path_;
- int led_count_;
- int rgb_led_count_;
-};
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
-
-#endif /* BUTTON_TEST_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h
deleted file mode 100644
index 6c2dbbc..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef FACE_TRACKING_H_
-#define FACE_TRACKING_H_
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-// head tracking for looking the ball
-class FaceTracker {
-public:
- enum TrackingStatus {
- NotFound = -1,
- Waiting = 0,
- Found = 1,
-
- };
-
- FaceTracker();
- ~FaceTracker();
-
- int processTracking();
-
- void startTracking();
- void stopTracking();
-
- void setUsingHeadScan(bool use_scan);
- void setFacePosition(geometry_msgs::Point &face_position);
- void goInit(double init_pan, double init_tile);
-
- double getPanOfFace() { return current_face_pan_; }
- double getTiltOfFace() { return current_face_tilt_; }
-
-protected:
- const double FOV_WIDTH;
- const double FOV_HEIGHT;
- const int NOT_FOUND_THRESHOLD;
-
- void facePositionCallback(const geometry_msgs::Point::ConstPtr &msg);
- void faceTrackerCommandCallback(const std_msgs::String::ConstPtr &msg);
- void publishHeadJoint(double pan, double tilt);
- void scanFace();
-
- // ros node handle
- ros::NodeHandle nh_;
-
- // 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::Subscriber face_position_sub_;
- ros::Subscriber face_tracking_command_sub_;
-
- // (x, y) is the center position of the ball in image coordinates
- // z is the face size
- geometry_msgs::Point face_position_;
-
- bool use_head_scan_;
- int count_not_found_;
- bool on_tracking_;
- double current_face_pan_, current_face_tilt_;
-
- int dismissed_count_;
-};
-} // namespace humanoid_robot_intelligence_control_system_op
-
-#endif /* FACE_TRACKING_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h
deleted file mode 100644
index 0477d8d..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef MIC_TEST_H_
-#define MIC_TEST_H_
-
-#include
-#include
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_demo/op_demo.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-class MicTest : public OPDemo {
-public:
- enum Mic_Test_Status {
- Ready = 0,
- AnnounceRecording = 1,
- MicRecording = 2,
- PlayingSound = 3,
- DeleteTempFile = 4,
- DemoCount = 5
- };
-
- MicTest();
- ~MicTest();
-
- void setDemoEnable();
- void setDemoDisable();
-
-protected:
- const int SPIN_RATE;
-
- void processThread();
- void callbackThread();
-
- void process();
-
- void announceTest();
- void recordSound(int recording_time);
- void recordSound();
- void playTestSound(const std::string &path);
- void playSound(const std::string &file_path);
- void deleteSoundFile(const std::string &file_path);
-
- void startTimer(double wait_time);
- void finishTimer();
-
- void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
-
- std::string recording_file_name_;
- std::string default_mp3_path_;
-
- ros::Publisher play_sound_pub_;
- ros::Subscriber buttuon_sub_;
-
- ros::Time start_time_;
- double wait_time_;
- bool is_wait_;
- int record_pid_;
- int play_pid_;
- int test_status_;
-};
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
-
-#endif /* MIC_TEST_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h
deleted file mode 100644
index 5872213..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef OP_DEMO_H_
-#define OP_DEMO_H_
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-class OPDemo {
-public:
- enum Motion_Index {
- InitPose = 1,
- WalkingReady = 9,
- GetUpFront = 122,
- GetUpBack = 123,
- RightKick = 121,
- LeftKick = 120,
- Ceremony = 27,
- ForGrass = 20,
- };
-
- OPDemo() {}
- virtual ~OPDemo() {}
-
- virtual void setDemoEnable() {}
- virtual void setDemoDisable() {}
-
-protected:
- bool enable_;
-};
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
-
-#endif /* OP_DEMO_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h
deleted file mode 100644
index 4af5a5a..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef SOCCER_DEMO_H
-#define SOCCER_DEMO_H
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_action_module_msgs/IsRunning.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
-
-#include "humanoid_robot_intelligence_control_system_demo/ball_follower.h"
-#include "humanoid_robot_intelligence_control_system_demo/ball_tracker.h"
-#include "humanoid_robot_intelligence_control_system_demo/op_demo.h"
-#include "humanoid_robot_intelligence_control_system_math/humanoid_robot_intelligence_control_system_linear_algebra.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-class SoccerDemo : public OPDemo {
-public:
- enum Stand_Status {
- Stand = 0,
- Fallen_Forward = 1,
- Fallen_Behind = 2,
- };
-
- enum Robot_Status {
- Waited = 0,
- TrackingAndFollowing = 1,
- ReadyToKick = 2,
- ReadyToCeremony = 3,
- ReadyToGetup = 4,
- };
-
- SoccerDemo();
- ~SoccerDemo();
-
- void setDemoEnable();
- void setDemoDisable();
-
-protected:
- const double FALL_FORWARD_LIMIT;
- const double FALL_BACK_LIMIT;
- const int SPIN_RATE;
- const bool DEBUG_PRINT;
-
- 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 humanoid_robot_intelligence_control_system_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);
- int getJointCount();
- bool isHeadJoint(const int &id);
- void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
- void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
- void imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg);
-
- void startSoccerMode();
- void stopSoccerMode();
-
- void process();
- void handleKick(int ball_position);
- void handleKick();
- bool handleFallen(int fallen_status);
-
- void playMotion(int motion_index);
- void setRGBLED(int blue, int green, int red);
- bool isActionRunning();
-
- void sendDebugTopic(const std::string &msgs);
-
- BallTracker ball_tracker_;
- BallFollower ball_follower_;
-
- ros::Publisher module_control_pub_;
- ros::Publisher motion_index_pub_;
- ros::Publisher rgb_led_pub_;
- ros::Subscriber buttuon_sub_;
- ros::Subscriber demo_command_sub_;
- ros::Subscriber imu_data_sub_;
-
- ros::Publisher test_pub_;
-
- ros::ServiceClient is_running_client_;
- ros::ServiceClient set_joint_module_client_;
-
- std::map id_joint_table_;
- std::map joint_id_table_;
-
- bool is_grass_;
- int wait_count_;
- bool on_following_ball_;
- bool on_tracking_ball_;
- bool restart_soccer_;
- bool start_following_;
- bool stop_following_;
- bool stop_fallen_check_;
- int robot_status_;
- int tracking_status_;
- int stand_state_;
- double present_pitch_;
-};
-
-} // namespace humanoid_robot_intelligence_control_system_op
-#endif // SOCCER_DEMO_H
diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h
deleted file mode 100644
index a42de50..0000000
--- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#ifndef VISION_DEMO_H_
-#define VISION_DEMO_H_
-
-#include
-#include
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h"
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
-
-#include "humanoid_robot_intelligence_control_system_demo/face_tracker.h"
-#include "humanoid_robot_intelligence_control_system_demo/op_demo.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-class VisionDemo : public OPDemo {
-public:
- VisionDemo();
- ~VisionDemo();
-
- void setDemoEnable();
- void setDemoDisable();
-
-protected:
- const int SPIN_RATE;
- const int TIME_TO_INIT;
-
- void processThread();
- void callbackThread();
-
- void process();
-
- void playMotion(int motion_index);
- void setRGBLED(int blue, int green, int red);
-
- 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 face_tracking_command_pub_;
-
- ros::Subscriber buttuon_sub_;
- ros::Subscriber faceCoord_sub_;
-
- ros::ServiceClient set_joint_module_client_;
- geometry_msgs::Point face_position_;
-
- ros::Time prev_time_;
-
- int tracking_status_;
-};
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
-
-#endif /* VISION_DEMO_H_ */
diff --git a/humanoid_robot_intelligence_control_system_demo/launch/demo.launch b/humanoid_robot_intelligence_control_system_demo/launch/demo.launch
deleted file mode 100644
index 2734717..0000000
--- a/humanoid_robot_intelligence_control_system_demo/launch/demo.launch
+++ /dev/null
@@ -1,28 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/humanoid_robot_intelligence_control_system_demo/launch/self_test.launch b/humanoid_robot_intelligence_control_system_demo/launch/self_test.launch
deleted file mode 100644
index 4751a5f..0000000
--- a/humanoid_robot_intelligence_control_system_demo/launch/self_test.launch
+++ /dev/null
@@ -1,29 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/humanoid_robot_intelligence_control_system_demo/package.xml b/humanoid_robot_intelligence_control_system_demo/package.xml
deleted file mode 100644
index 1288222..0000000
--- a/humanoid_robot_intelligence_control_system_demo/package.xml
+++ /dev/null
@@ -1,180 +0,0 @@
-
-
-
-
- humanoid_robot_intelligence_control_system_demo
- 0.3.2
-
- HUMANOID_ROBOT default demo
- It includes three demontrations(soccer demo, vision demo, action script demo)
-
- Apache 2.0
- Ronaldson Bellande
-
-
- catkin
-
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
-
- humanoid_robot_intelligence_control_system_controller_msgs
-
- humanoid_robot_intelligence_control_system_walking_module_msgs
-
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
-
- humanoid_robot_intelligence_control_system_object_detector
- boost
- eigen
- yaml-cpp
- humanoid_robot_intelligence_control_system_manager
-
- humanoid_robot_intelligence_control_system_camera_setting_tool
-
- humanoid_robot_intelligence_control_system_web_setting_tool
-
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
-
- humanoid_robot_intelligence_control_system_controller_msgs
-
- humanoid_robot_intelligence_control_system_walking_module_msgs
-
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
-
- humanoid_robot_intelligence_control_system_object_detector
- boost
- eigen
- yaml-cpp
-
- humanoid_robot_intelligence_control_system_manager
-
- humanoid_robot_intelligence_control_system_camera_setting_tool
-
- humanoid_robot_intelligence_control_system_web_setting_tool
-
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
-
- humanoid_robot_intelligence_control_system_controller_msgs
-
- humanoid_robot_intelligence_control_system_walking_module_msgs
-
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
-
- humanoid_robot_intelligence_control_system_object_detector
- boost
- eigen
- yaml-cpp
- humanoid_robot_intelligence_control_system_manager
-
- humanoid_robot_intelligence_control_system_camera_setting_tool
-
- humanoid_robot_intelligence_control_system_web_setting_tool
-
-
- ament_cmake
-
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
-
- humanoid_robot_intelligence_control_system_controller_msgs
-
- humanoid_robot_intelligence_control_system_walking_module_msgs
-
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
-
- humanoid_robot_intelligence_control_system_object_detector
- boost
- eigen
- yaml-cpp
- humanoid_robot_intelligence_control_system_manager
-
- humanoid_robot_intelligence_control_system_camera_setting_tool
-
- humanoid_robot_intelligence_control_system_web_setting_tool
-
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
-
- humanoid_robot_intelligence_control_system_controller_msgs
-
- humanoid_robot_intelligence_control_system_walking_module_msgs
-
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
-
- humanoid_robot_intelligence_control_system_object_detector
- boost
- eigen
- yaml-cpp
-
- humanoid_robot_intelligence_control_system_manager
-
- humanoid_robot_intelligence_control_system_camera_setting_tool
-
- humanoid_robot_intelligence_control_system_web_setting_tool
-
- roscpp
- roslib
- std_msgs
- sensor_msgs
- geometry_msgs
-
- humanoid_robot_intelligence_control_system_controller_msgs
-
- humanoid_robot_intelligence_control_system_walking_module_msgs
-
- humanoid_robot_intelligence_control_system_action_module_msgs
- cmake_modules
- humanoid_robot_intelligence_control_system_math
-
- humanoid_robot_intelligence_control_system_object_detector
- boost
- eigen
- yaml-cpp
- humanoid_robot_intelligence_control_system_manager
-
- humanoid_robot_intelligence_control_system_camera_setting_tool
-
- humanoid_robot_intelligence_control_system_web_setting_tool
-
-
diff --git a/humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp b/humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp
deleted file mode 100644
index 523b62a..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp
+++ /dev/null
@@ -1,351 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
-#include "humanoid_robot_intelligence_control_system_demo/action_demo.h"
-#include "humanoid_robot_intelligence_control_system_demo/soccer_demo.h"
-#include "humanoid_robot_intelligence_control_system_demo/vision_demo.h"
-#include "humanoid_robot_intelligence_control_system_math/humanoid_robot_intelligence_control_system_linear_algebra.h"
-
-enum Demo_Status {
- Ready = 0,
- SoccerDemo = 1,
- VisionDemo = 2,
- ActionDemo = 3,
- DemoCount = 4,
-};
-
-void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
-void goInitPose();
-void playSound(const std::string &path);
-void setLED(int led);
-bool checkManagerRunning(std::string &manager_name);
-void dxlTorqueChecker();
-
-void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg);
-
-const int SPIN_RATE = 30;
-const bool DEBUG_PRINT = false;
-
-ros::Publisher init_pose_pub;
-ros::Publisher play_sound_pub;
-ros::Publisher led_pub;
-ros::Publisher dxl_torque_pub;
-
-std::string default_mp3_path = "";
-int current_status = Ready;
-int desired_status = Ready;
-bool apply_desired = false;
-
-// node main
-int main(int argc, char **argv) {
- // init ros
- ros::init(argc, argv, "demo_node");
-
- // create ros wrapper object
- humanoid_robot_intelligence_control_system_op::OPDemo *current_demo = NULL;
- humanoid_robot_intelligence_control_system_op::SoccerDemo *soccer_demo =
- new humanoid_robot_intelligence_control_system_op::SoccerDemo();
- humanoid_robot_intelligence_control_system_op::ActionDemo *action_demo =
- new humanoid_robot_intelligence_control_system_op::ActionDemo();
- humanoid_robot_intelligence_control_system_op::VisionDemo *vision_demo =
- new humanoid_robot_intelligence_control_system_op::VisionDemo();
-
- ros::NodeHandle nh(ros::this_node::getName());
-
- init_pose_pub = nh.advertise(
- "/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
- play_sound_pub = nh.advertise("/play_sound_file", 0);
- led_pub =
- nh.advertise(
- "/humanoid_robot_intelligence_control_system/sync_write_item", 0);
- dxl_torque_pub = nh.advertise(
- "/humanoid_robot_intelligence_control_system/dxl_torque", 0);
- ros::Subscriber buttuon_sub =
- nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button",
- 1, buttonHandlerCallback);
- ros::Subscriber mode_command_sub =
- nh.subscribe("/humanoid_robot_intelligence_control_system/mode_command",
- 1, demoModeCommandCallback);
-
- default_mp3_path =
- ros::package::getPath("humanoid_robot_intelligence_control_system_demo") +
- "/data/mp3/";
-
- ros::start();
-
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- // wait for starting of manager
- std::string manager_name =
- "/humanoid_robot_intelligence_control_system_manager";
- while (ros::ok()) {
- ros::Duration(1.0).sleep();
-
- if (checkManagerRunning(manager_name) == true) {
- break;
- ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect");
- }
- ROS_WARN("Waiting for humanoid_robot_intelligence_control_system manager");
- }
-
- // init procedure
- playSound(default_mp3_path + "Demonstration ready mode.mp3");
- // turn on R/G/B LED
- setLED(0x01 | 0x02 | 0x04);
-
- // node loop
- while (ros::ok()) {
- // process
- if (apply_desired == true) {
- switch (desired_status) {
- case Ready: {
-
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = NULL;
-
- goInitPose();
-
- ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]");
- break;
- }
-
- case SoccerDemo: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = soccer_demo;
- current_demo->setDemoEnable();
-
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo");
- break;
- }
-
- case VisionDemo: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = vision_demo;
- current_demo->setDemoEnable();
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo");
- break;
- }
- case ActionDemo: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = action_demo;
- current_demo->setDemoEnable();
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo");
- break;
- }
-
- default: {
- break;
- }
- }
-
- apply_desired = false;
- current_status = desired_status;
- }
-
- // execute pending callbacks
- ros::spinOnce();
-
- // relax to fit output rate
- loop_rate.sleep();
- }
-
- // exit program
- return 0;
-}
-
-void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
- if (apply_desired == true)
- return;
-
- // in the middle of playing demo
- if (current_status != Ready) {
- if (msg->data == "mode_long") {
- // go to mode selection status
- desired_status = Ready;
- apply_desired = true;
-
- playSound(default_mp3_path + "Demonstration ready mode.mp3");
- setLED(0x01 | 0x02 | 0x04);
- } else if (msg->data == "user_long") {
- // it's using in humanoid_robot_intelligence_control_system_manager
- // torque on and going to init pose
- }
- }
- // ready to start demo
- else {
- if (msg->data == "start") {
- // select current demo
- desired_status =
- (desired_status == Ready) ? desired_status + 1 : desired_status;
- apply_desired = true;
-
- // sound out desired status
- switch (desired_status) {
- case SoccerDemo:
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start soccer demonstration.mp3");
- break;
-
- case VisionDemo:
- dxlTorqueChecker();
- playSound(default_mp3_path +
- "Start vision processing demonstration.mp3");
- break;
-
- case ActionDemo:
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start motion demonstration.mp3");
- break;
-
- default:
- break;
- }
-
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- } else if (msg->data == "mode") {
- // change to next demo
- desired_status = (desired_status + 1) % DemoCount;
- desired_status =
- (desired_status == Ready) ? desired_status + 1 : desired_status;
-
- // sound out desired status and changing LED
- switch (desired_status) {
- case SoccerDemo:
- playSound(default_mp3_path + "Autonomous soccer mode.mp3");
- setLED(0x01);
- break;
-
- case VisionDemo:
- playSound(default_mp3_path + "Vision processing mode.mp3");
- setLED(0x02);
- break;
-
- case ActionDemo:
- playSound(default_mp3_path + "Interactive motion mode.mp3");
- setLED(0x04);
- break;
-
- default:
- break;
- }
-
- ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status);
- }
- }
-}
-
-void goInitPose() {
- std_msgs::String init_msg;
- init_msg.data = "ini_pose";
-
- init_pose_pub.publish(init_msg);
-}
-
-void playSound(const std::string &path) {
- std_msgs::String sound_msg;
- sound_msg.data = path;
-
- play_sound_pub.publish(sound_msg);
-}
-
-void setLED(int led) {
- humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem
- syncwrite_msg;
- syncwrite_msg.item_name = "LED";
- syncwrite_msg.joint_name.push_back("open-cr");
- syncwrite_msg.value.push_back(led);
-
- led_pub.publish(syncwrite_msg);
-}
-
-bool checkManagerRunning(std::string &manager_name) {
- std::vector node_list;
- ros::master::getNodes(node_list);
-
- for (unsigned int node_list_idx = 0; node_list_idx < node_list.size();
- node_list_idx++) {
- if (node_list[node_list_idx] == manager_name)
- return true;
- }
-
- ROS_ERROR("Can't find humanoid_robot_intelligence_control_system_manager");
- return false;
-}
-
-void dxlTorqueChecker() {
- std_msgs::String check_msg;
- check_msg.data = "check";
-
- dxl_torque_pub.publish(check_msg);
-}
-
-void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) {
- // In demo mode
- if (current_status != Ready) {
- if (msg->data == "ready") {
- // go to mode selection status
- desired_status = Ready;
- apply_desired = true;
-
- playSound(default_mp3_path + "Demonstration ready mode.mp3");
- setLED(0x01 | 0x02 | 0x04);
- }
- }
- // In ready mode
- else {
- if (msg->data == "soccer") {
- desired_status = SoccerDemo;
- apply_desired = true;
-
- // play sound
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start soccer demonstration.mp3");
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- } else if (msg->data == "vision") {
- desired_status = VisionDemo;
- apply_desired = true;
-
- // play sound
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start vision processing demonstration.mp3");
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- } else if (msg->data == "action") {
- desired_status = ActionDemo;
- apply_desired = true;
-
- // play sound
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start motion demonstration.mp3");
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- }
- }
-}
diff --git a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp b/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp
deleted file mode 100644
index 98916b3..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/ball_follower.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-BallFollower::BallFollower()
- : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180),
- FOV_HEIGHT(21.6 * M_PI / 180), count_not_found_(0), count_to_kick_(0),
- on_tracking_(false), approach_ball_position_(NotFound),
- kick_motion_index_(83), CAMERA_HEIGHT(0.46), NOT_FOUND_THRESHOLD(50),
- MAX_FB_STEP(40.0 * 0.001), MAX_RL_TURN(15.0 * M_PI / 180),
- IN_PLACE_FB_STEP(-3.0 * 0.001), MIN_FB_STEP(5.0 * 0.001),
- MIN_RL_TURN(5.0 * M_PI / 180), UNIT_FB_STEP(1.0 * 0.001),
- UNIT_RL_TURN(0.5 * M_PI / 180), SPOT_FB_OFFSET(0.0 * 0.001),
- SPOT_RL_OFFSET(0.0 * 0.001), SPOT_ANGLE_OFFSET(0.0),
- hip_pitch_offset_(7.0), current_pan_(-10), current_tilt_(-10),
- current_x_move_(0.005), current_r_angle_(0), curr_period_time_(0.6),
- accum_period_time_(0.0), DEBUG_PRINT(false) {
- current_joint_states_sub_ =
- nh_.subscribe("/humanoid_robot_intelligence_control_system/goal_joint_states", 10,
- &BallFollower::currentJointStatesCallback, this);
-
- set_walking_command_pub_ =
- nh_.advertise("/humanoid_robot_intelligence_control_system/walking/command", 0);
- set_walking_param_pub_ =
- nh_.advertise(
- "/humanoid_robot_intelligence_control_system/walking/set_params", 0);
- get_walking_param_client_ =
- nh_.serviceClient(
- "/humanoid_robot_intelligence_control_system/walking/get_params");
-
- prev_time_ = ros::Time::now();
-}
-
-BallFollower::~BallFollower() {}
-
-void BallFollower::startFollowing() {
- on_tracking_ = true;
- ROS_INFO("Start Ball following");
-
- setWalkingCommand("start");
-
- bool result = getWalkingParam();
- if (result == true) {
- hip_pitch_offset_ = current_walking_param_.hip_pitch_offset;
- curr_period_time_ = current_walking_param_.period_time;
- } else {
- hip_pitch_offset_ = 7.0 * M_PI / 180;
- curr_period_time_ = 0.6;
- }
-}
-
-void BallFollower::stopFollowing() {
- on_tracking_ = false;
- // approach_ball_position_ = NotFound;
- count_to_kick_ = 0;
- // accum_ball_position_ = 0;
- ROS_INFO("Stop Ball following");
-
- setWalkingCommand("stop");
-}
-
-void BallFollower::currentJointStatesCallback(
- const sensor_msgs::JointState::ConstPtr &msg) {
- double pan, tilt;
- int get_count = 0;
-
- for (int ix = 0; ix < msg->name.size(); ix++) {
- if (msg->name[ix] == "head_pan") {
- pan = msg->position[ix];
- get_count += 1;
- } else if (msg->name[ix] == "head_tilt") {
- tilt = msg->position[ix];
- get_count += 1;
- }
-
- if (get_count == 2)
- break;
- }
-
- // check variation
- current_pan_ = pan;
- current_tilt_ = tilt;
-}
-
-void BallFollower::calcFootstep(double target_distance, double target_angle,
- double delta_time, double &fb_move,
- double &rl_angle) {
- // clac fb
- double next_movement = current_x_move_;
- if (target_distance < 0)
- target_distance = 0.0;
-
- double fb_goal = fmin(target_distance * 0.1, MAX_FB_STEP);
- accum_period_time_ += delta_time;
- if (accum_period_time_ > (curr_period_time_ / 4)) {
- accum_period_time_ = 0.0;
- if ((target_distance * 0.1 / 2) < current_x_move_)
- next_movement -= UNIT_FB_STEP;
- else
- next_movement += UNIT_FB_STEP;
- }
- fb_goal = fmin(next_movement, fb_goal);
- fb_move = fmax(fb_goal, MIN_FB_STEP);
- ROS_INFO_COND(DEBUG_PRINT,
- "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f",
- target_distance, fb_move, delta_time);
- ROS_INFO_COND(DEBUG_PRINT, "==============================================");
-
- // calc rl angle
- double rl_goal = 0.0;
- if (fabs(target_angle) * 180 / M_PI > 5.0) {
- double rl_offset = fabs(target_angle) * 0.2;
- rl_goal = fmin(rl_offset, MAX_RL_TURN);
- rl_goal = fmax(rl_goal, MIN_RL_TURN);
- rl_angle = fmin(fabs(current_r_angle_) + UNIT_RL_TURN, rl_goal);
-
- if (target_angle < 0)
- rl_angle *= (-1);
- }
-}
-
-// x_angle : ball position (pan), y_angle : ball position (tilt), ball_size :
-// angle of ball radius
-bool BallFollower::processFollowing(double x_angle, double y_angle,
- double ball_size) {
- ros::Time curr_time = ros::Time::now();
- ros::Duration dur = curr_time - prev_time_;
- double delta_time = dur.nsec * 0.000000001 + dur.sec;
- prev_time_ = curr_time;
-
- count_not_found_ = 0;
- // int ball_position_sum = 0;
-
- // check of getting head joints angle
- if (current_tilt_ == -10 && current_pan_ == -10) {
- ROS_ERROR("Failed to get current angle of head joints.");
- setWalkingCommand("stop");
-
- on_tracking_ = false;
- approach_ball_position_ = NotFound;
- return false;
- }
-
- ROS_INFO_COND(DEBUG_PRINT, " ============== Head | Ball ============== ");
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "== Head Pan : " << (current_pan_ * 180 / M_PI)
- << " | Ball X : "
- << (x_angle * 180 / M_PI));
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "== Head Tilt : " << (current_tilt_ * 180 / M_PI)
- << " | Ball Y : "
- << (y_angle * 180 / M_PI));
-
- approach_ball_position_ = OutOfRange;
-
- double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ -
- hip_pitch_offset_ - ball_size);
-
- double ball_y_angle = (current_tilt_ + y_angle) * 180 / M_PI;
- double ball_x_angle = (current_pan_ + x_angle) * 180 / M_PI;
-
- if (distance_to_ball < 0)
- distance_to_ball *= (-1);
-
- // double distance_to_kick = 0.25;
- double distance_to_kick = 0.22;
-
- // check whether ball is correct position.
- if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0)) {
- count_to_kick_ += 1;
-
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "head pan : " << (current_pan_ * 180 / M_PI)
- << " | ball pan : "
- << (x_angle * 180 / M_PI));
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "head tilt : " << (current_tilt_ * 180 / M_PI)
- << " | ball tilt : "
- << (y_angle * 180 / M_PI));
- ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << ball_x_angle);
-
- ROS_INFO("In range [%d | %f]", count_to_kick_, ball_x_angle);
-
- // ball queue
- // if(ball_position_queue_.size() >= 5)
- // ball_position_queue_.erase(ball_position_queue_.begin());
-
- // ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1);
-
- if (count_to_kick_ > 20) {
- setWalkingCommand("stop");
- on_tracking_ = false;
-
- // check direction of the ball
- // accum_ball_position_ =
- // std::accumulate(ball_position_queue_.begin(),
- // ball_position_queue_.end(), 0);
-
- // if (accum_ball_position_ > 0)
- if (ball_x_angle > 0) {
- ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left
- approach_ball_position_ = OnLeft;
- } else {
- ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : right"); // right
- approach_ball_position_ = OnRight;
- }
-
- return true;
- } else if (count_to_kick_ > 15) {
- // if (ball_x_angle > 0)
- // accum_ball_position_ += 1;
- // else
- // accum_ball_position_ -= 1;
-
- // send message
- setWalkingParam(IN_PLACE_FB_STEP, 0, 0);
-
- return false;
- }
- } else {
- count_to_kick_ = 0;
- // accum_ball_position_ = NotFound;
- }
-
- double fb_move = 0.0, rl_angle = 0.0;
- double distance_to_walk = distance_to_ball - distance_to_kick;
-
- calcFootstep(distance_to_walk, current_pan_, delta_time, fb_move, rl_angle);
-
- // send message
- setWalkingParam(fb_move, 0, rl_angle);
-
- // for debug
- // ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f",
- // distance_to_ball, fb_move, delta_time);
-
- return false;
-}
-
-void BallFollower::decideBallPositin(double x_angle, double y_angle) {
- // check of getting head joints angle
- if (current_tilt_ == -10 && current_pan_ == -10) {
- approach_ball_position_ = NotFound;
- return;
- }
-
- double ball_x_angle = current_pan_ + x_angle;
-
- if (ball_x_angle > 0)
- approach_ball_position_ = OnLeft;
- else
- approach_ball_position_ = OnRight;
-}
-
-void BallFollower::waitFollowing() {
- count_not_found_++;
-
- if (count_not_found_ > NOT_FOUND_THRESHOLD * 0.5)
- setWalkingParam(0.0, 0.0, 0.0);
-}
-
-void BallFollower::setWalkingCommand(const std::string &command) {
- // get param
- if (command == "start") {
- getWalkingParam();
- setWalkingParam(IN_PLACE_FB_STEP, 0, 0, true);
- }
-
- std_msgs::String _command_msg;
- _command_msg.data = command;
- set_walking_command_pub_.publish(_command_msg);
-
- ROS_INFO_STREAM_COND(DEBUG_PRINT, "Send Walking command : " << command);
-}
-
-void BallFollower::setWalkingParam(double x_move, double y_move,
- double rotation_angle, bool balance) {
- current_walking_param_.balance_enable = balance;
- current_walking_param_.x_move_amplitude = x_move + SPOT_FB_OFFSET;
- current_walking_param_.y_move_amplitude = y_move + SPOT_RL_OFFSET;
- current_walking_param_.angle_move_amplitude =
- rotation_angle + SPOT_ANGLE_OFFSET;
-
- set_walking_param_pub_.publish(current_walking_param_);
-
- current_x_move_ = x_move;
- current_r_angle_ = rotation_angle;
-}
-
-bool BallFollower::getWalkingParam() {
- humanoid_robot_intelligence_control_system_walking_module_msgs::GetWalkingParam walking_param_msg;
-
- if (get_walking_param_client_.call(walking_param_msg)) {
- current_walking_param_ = walking_param_msg.response.parameters;
-
- // update ui
- ROS_INFO_COND(DEBUG_PRINT, "Get walking parameters");
-
- return true;
- } else {
- ROS_ERROR("Fail to get walking parameters.");
-
- return false;
- }
-}
-
-} // namespace humanoid_robot_intelligence_control_system_op
diff --git a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp b/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp
deleted file mode 100644
index 9cbeba4..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/ball_tracker.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-BallTracker::BallTracker()
- : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180),
- FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50),
- WAITING_THRESHOLD(5), use_head_scan_(true), count_not_found_(0),
- on_tracking_(false), current_ball_pan_(0), current_ball_tilt_(0),
- x_error_sum_(0), y_error_sum_(0), current_ball_bottom_(0),
- tracking_status_(NotFound), DEBUG_PRINT(false) {
- ros::NodeHandle param_nh("~");
- p_gain_ = param_nh.param("p_gain", 0.4);
- i_gain_ = param_nh.param("i_gain", 0.0);
- d_gain_ = param_nh.param("d_gain", 0.0);
-
- ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", "
- << d_gain_);
-
- head_joint_offset_pub_ = nh_.advertise(
- "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0);
- head_joint_pub_ = nh_.advertise(
- "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0);
- head_scan_pub_ = nh_.advertise(
- "/humanoid_robot_intelligence_control_system/head_control/scan_command", 0);
- // error_pub_ =
- // nh_.advertise("/ball_tracker/errors", 0);
-
- ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1,
- &BallTracker::ballPositionCallback, this);
- ball_tracking_command_sub_ =
- nh_.subscribe("/ball_tracker/command", 1,
- &BallTracker::ballTrackerCommandCallback, this);
-}
-
-BallTracker::~BallTracker() {}
-
-void BallTracker::ballPositionCallback(
- const humanoid_robot_intelligence_control_system_ball_detector::CircleSetStamped::ConstPtr &msg) {
- for (int idx = 0; idx < msg->circles.size(); idx++) {
- if (ball_position_.z >= msg->circles[idx].z)
- continue;
-
- ball_position_ = msg->circles[idx];
- }
-}
-
-void BallTracker::ballTrackerCommandCallback(
- const std_msgs::String::ConstPtr &msg) {
- if (msg->data == "start") {
- startTracking();
- } else if (msg->data == "stop") {
- stopTracking();
- } else if (msg->data == "toggle_start") {
- if (on_tracking_ == false)
- startTracking();
- else
- stopTracking();
- }
-}
-
-void BallTracker::startTracking() {
- on_tracking_ = true;
- ROS_INFO_COND(DEBUG_PRINT, "Start Ball tracking");
-}
-
-void BallTracker::stopTracking() {
- goInit();
-
- on_tracking_ = false;
- ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking");
-
- current_ball_pan_ = 0;
- current_ball_tilt_ = 0;
- x_error_sum_ = 0;
- y_error_sum_ = 0;
-}
-
-void BallTracker::setUsingHeadScan(bool use_scan) { use_head_scan_ = use_scan; }
-
-int BallTracker::processTracking() {
- int tracking_status = Found;
-
- if (on_tracking_ == false) {
- ball_position_.z = 0;
- count_not_found_ = 0;
- return NotFound;
- }
-
- // check ball position
- if (ball_position_.z <= 0) {
- count_not_found_++;
-
- if (count_not_found_ < WAITING_THRESHOLD) {
- if (tracking_status_ == Found || tracking_status_ == Waiting)
- tracking_status = Waiting;
- else
- tracking_status = NotFound;
- } else if (count_not_found_ > NOT_FOUND_THRESHOLD) {
- scanBall();
- count_not_found_ = 0;
- tracking_status = NotFound;
- } else {
- tracking_status = NotFound;
- }
- } else {
- count_not_found_ = 0;
- }
-
- // if ball is found
- // convert ball position to desired angle(rad) of head
- // ball_position : top-left is (-1, -1), bottom-right is (+1, +1)
- // offset_rad : top-left(+, +), bottom-right(-, -)
- double x_error = 0.0, y_error = 0.0, ball_size = 0.0;
-
- switch (tracking_status) {
- case NotFound:
- tracking_status_ = tracking_status;
- current_ball_pan_ = 0;
- current_ball_tilt_ = 0;
- x_error_sum_ = 0;
- y_error_sum_ = 0;
- return tracking_status;
-
- case Waiting:
- tracking_status_ = tracking_status;
- return tracking_status;
-
- case Found:
- x_error = -atan(ball_position_.x * tan(FOV_WIDTH));
- y_error = -atan(ball_position_.y * tan(FOV_HEIGHT));
- ball_size = ball_position_.z;
- break;
-
- default:
- break;
- }
-
- ROS_INFO_STREAM_COND(
- DEBUG_PRINT,
- "--------------------------------------------------------------");
- ROS_INFO_STREAM_COND(DEBUG_PRINT, "Ball position : " << ball_position_.x
- << " | "
- << ball_position_.y);
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "Target angle : " << (x_error * 180 / M_PI) << " | "
- << (y_error * 180 / M_PI));
-
- ros::Time curr_time = ros::Time::now();
- ros::Duration dur = curr_time - prev_time_;
- double delta_time = dur.nsec * 0.000000001 + dur.sec;
- prev_time_ = curr_time;
-
- double x_error_diff = (x_error - current_ball_pan_) / delta_time;
- double y_error_diff = (y_error - current_ball_tilt_) / delta_time;
- x_error_sum_ += x_error;
- y_error_sum_ += y_error;
- double x_error_target =
- x_error * p_gain_ + x_error_diff * d_gain_ + x_error_sum_ * i_gain_;
- double y_error_target =
- y_error * p_gain_ + y_error_diff * d_gain_ + y_error_sum_ * i_gain_;
-
- // std_msgs::Float64MultiArray x_error_msg;
- // x_error_msg.data.push_back(x_error);
- // x_error_msg.data.push_back(x_error_diff);
- // x_error_msg.data.push_back(x_error_sum_);
- // x_error_msg.data.push_back(x_error * p_gain_);
- // x_error_msg.data.push_back(x_error_diff * d_gain_);
- // x_error_msg.data.push_back(x_error_sum_ * i_gain_);
- // x_error_msg.data.push_back(x_error_target);
- // error_pub_.publish(x_error_msg);
-
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "------------------------ "
- << tracking_status
- << " --------------------------------------");
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "error : " << (x_error * 180 / M_PI) << " | "
- << (y_error * 180 / M_PI));
- ROS_INFO_STREAM_COND(DEBUG_PRINT, "error_diff : "
- << (x_error_diff * 180 / M_PI) << " | "
- << (y_error_diff * 180 / M_PI) << " | "
- << delta_time);
- ROS_INFO_STREAM_COND(DEBUG_PRINT,
- "error_sum : " << (x_error_sum_ * 180 / M_PI) << " | "
- << (y_error_sum_ * 180 / M_PI));
- ROS_INFO_STREAM_COND(
- DEBUG_PRINT, "error_target : " << (x_error_target * 180 / M_PI) << " | "
- << (y_error_target * 180 / M_PI)
- << " | P : " << p_gain_ << " | D : "
- << d_gain_ << " | time : " << delta_time);
-
- // move head joint
- publishHeadJoint(x_error_target, y_error_target);
-
- // args for following ball
- current_ball_pan_ = x_error;
- current_ball_tilt_ = y_error;
- current_ball_bottom_ = ball_size;
-
- ball_position_.z = 0;
-
- tracking_status_ = tracking_status;
- return tracking_status;
-}
-
-void BallTracker::publishHeadJoint(double pan, double tilt) {
- double min_angle = 1 * M_PI / 180;
- if (fabs(pan) < min_angle && fabs(tilt) < min_angle)
- return;
-
- sensor_msgs::JointState head_angle_msg;
-
- head_angle_msg.name.push_back("head_pan");
- head_angle_msg.name.push_back("head_tilt");
-
- head_angle_msg.position.push_back(pan);
- head_angle_msg.position.push_back(tilt);
-
- head_joint_offset_pub_.publish(head_angle_msg);
-}
-
-void BallTracker::goInit() {
- sensor_msgs::JointState head_angle_msg;
-
- head_angle_msg.name.push_back("head_pan");
- head_angle_msg.name.push_back("head_tilt");
-
- head_angle_msg.position.push_back(0.0);
- head_angle_msg.position.push_back(0.0);
-
- head_joint_pub_.publish(head_angle_msg);
-}
-
-void BallTracker::scanBall() {
- if (use_head_scan_ == false)
- return;
-
- // check head control module enabled
- // ...
-
- // send message to head control module
- std_msgs::String scan_msg;
- scan_msg.data = "scan";
-
- head_scan_pub_.publish(scan_msg);
-}
-
-} // namespace humanoid_robot_intelligence_control_system_op
diff --git a/humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp b/humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp
deleted file mode 100644
index 3a0bba6..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp
+++ /dev/null
@@ -1,619 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/soccer_demo.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-SoccerDemo::SoccerDemo()
- : FALL_FORWARD_LIMIT(60), FALL_BACK_LIMIT(-60), SPIN_RATE(30),
- DEBUG_PRINT(false), wait_count_(0), on_following_ball_(false),
- on_tracking_ball_(false), restart_soccer_(false), start_following_(false),
- stop_following_(false), stop_fallen_check_(false), robot_status_(Waited),
- stand_state_(Stand), tracking_status_(BallTracker::Waiting),
- present_pitch_(0) {
- // init ros
- enable_ = false;
-
- ros::NodeHandle nh(ros::this_node::getName());
-
- std::string default_path = ros::package::getPath("humanoid_robot_intelligence_control_system_gui_demo") +
- "/config/gui_config.yaml";
- std::string path = nh.param("demo_config", default_path);
- parseJointNameFromYaml(path);
-
- boost::thread queue_thread =
- boost::thread(boost::bind(&SoccerDemo::callbackThread, this));
- boost::thread process_thread =
- boost::thread(boost::bind(&SoccerDemo::processThread, this));
- boost::thread tracking_thread =
- boost::thread(boost::bind(&SoccerDemo::trackingThread, this));
-
- is_grass_ = nh.param("grass_demo", false);
-}
-
-SoccerDemo::~SoccerDemo() {}
-
-void SoccerDemo::setDemoEnable() {
- enable_ = true;
-
- startSoccerMode();
-}
-
-void SoccerDemo::setDemoDisable() {
- // handle disable procedure
- ball_tracker_.stopTracking();
- ball_follower_.stopFollowing();
-
- enable_ = false;
- wait_count_ = 0;
- on_following_ball_ = false;
- on_tracking_ball_ = false;
- restart_soccer_ = false;
- start_following_ = false;
- stop_following_ = false;
- stop_fallen_check_ = false;
-
- tracking_status_ = BallTracker::Waiting;
-}
-
-void SoccerDemo::process() {
- if (enable_ == false)
- return;
-
- // check to start
- if (start_following_ == true) {
- ball_tracker_.startTracking();
- ball_follower_.startFollowing();
- start_following_ = false;
-
- wait_count_ = 1 * SPIN_RATE;
- }
-
- // check to stop
- if (stop_following_ == true) {
- ball_tracker_.stopTracking();
- ball_follower_.stopFollowing();
- stop_following_ = false;
-
- wait_count_ = 0;
- }
-
- if (wait_count_ <= 0) {
- // ball following
- if (on_following_ball_ == true) {
- switch (tracking_status_) {
- case BallTracker::Found:
- ball_follower_.processFollowing(ball_tracker_.getPanOfBall(),
- ball_tracker_.getTiltOfBall(), 0.0);
- break;
-
- case BallTracker::NotFound:
- ball_follower_.waitFollowing();
- break;
-
- default:
- break;
- }
- }
-
- // check fallen states
- switch (stand_state_) {
- case Stand: {
- // check restart soccer
- if (restart_soccer_ == true) {
- restart_soccer_ = false;
- startSoccerMode();
- break;
- }
-
- // check states for kick
- // int ball_position = ball_follower_.getBallPosition();
- bool in_range = ball_follower_.isBallInRange();
-
- if (in_range == true) {
- ball_follower_.stopFollowing();
- handleKick();
- }
- break;
- }
- // fallen state : Fallen_Forward, Fallen_Behind
- default: {
- ball_follower_.stopFollowing();
- handleFallen(stand_state_);
- break;
- }
- }
- } else {
- wait_count_ -= 1;
- }
-}
-
-void SoccerDemo::processThread() {
- bool result = false;
-
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- ball_tracker_.startTracking();
-
- // node loop
- while (ros::ok()) {
- if (enable_ == true)
- process();
-
- // relax to fit output rate
- loop_rate.sleep();
- }
-}
-
-void SoccerDemo::callbackThread() {
- ros::NodeHandle nh(ros::this_node::getName());
-
- // subscriber & publisher
- module_control_pub_ =
- nh.advertise(
- "/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 0);
- motion_index_pub_ =
- nh.advertise("/humanoid_robot_intelligence_control_system/action/page_num", 0);
- rgb_led_pub_ = nh.advertise(
- "/humanoid_robot_intelligence_control_system/sync_write_item", 0);
-
- buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1,
- &SoccerDemo::buttonHandlerCallback, this);
- demo_command_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/demo_command", 1,
- &SoccerDemo::demoCommandCallback, this);
- imu_data_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/imu", 1,
- &SoccerDemo::imuDataCallback, this);
-
- is_running_client_ =
- nh.serviceClient(
- "/humanoid_robot_intelligence_control_system/action/is_running");
- set_joint_module_client_ =
- nh.serviceClient(
- "/humanoid_robot_intelligence_control_system/set_present_joint_ctrl_modules");
-
- test_pub_ = nh.advertise("/debug_text", 0);
-
- while (nh.ok()) {
- ros::spinOnce();
-
- usleep(1000);
- }
-}
-
-void SoccerDemo::trackingThread() {
-
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- ball_tracker_.startTracking();
-
- // node loop
- while (ros::ok()) {
-
- if (enable_ == true && on_tracking_ball_ == true) {
- // ball tracking
- int tracking_status;
-
- tracking_status = ball_tracker_.processTracking();
-
- // set led
- switch (tracking_status) {
- case BallTracker::Found:
- if (tracking_status_ != tracking_status)
- setRGBLED(0x1F, 0x1F, 0x1F);
- break;
-
- case BallTracker::NotFound:
- if (tracking_status_ != tracking_status)
- setRGBLED(0, 0, 0);
- break;
-
- default:
- break;
- }
-
- if (tracking_status != tracking_status_)
- tracking_status_ = tracking_status;
- }
- // relax to fit output rate
- loop_rate.sleep();
- }
-}
-
-void SoccerDemo::setBodyModuleToDemo(const std::string &body_module,
- bool with_head_control) {
- humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule control_msg;
-
- std::string head_module = "head_control_module";
- std::map::iterator joint_iter;
-
- for (joint_iter = id_joint_table_.begin();
- joint_iter != id_joint_table_.end(); ++joint_iter) {
- // check whether joint name contains "head"
- if (joint_iter->second.find("head") != std::string::npos) {
- if (with_head_control == true) {
- control_msg.joint_name.push_back(joint_iter->second);
- control_msg.module_name.push_back(head_module);
- } else
- continue;
- } else {
- control_msg.joint_name.push_back(joint_iter->second);
- control_msg.module_name.push_back(body_module);
- }
- }
-
- // no control
- if (control_msg.joint_name.size() == 0)
- return;
-
- callServiceSettingModule(control_msg);
- std::cout << "enable module of body : " << body_module << std::endl;
-}
-
-void SoccerDemo::setModuleToDemo(const std::string &module_name) {
- if (enable_ == false)
- return;
-
- humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule control_msg;
- std::map::iterator joint_iter;
-
- for (joint_iter = id_joint_table_.begin();
- joint_iter != id_joint_table_.end(); ++joint_iter) {
- control_msg.joint_name.push_back(joint_iter->second);
- control_msg.module_name.push_back(module_name);
- }
-
- // no control
- if (control_msg.joint_name.size() == 0)
- return;
-
- callServiceSettingModule(control_msg);
- std::cout << "enable module : " << module_name << std::endl;
-}
-
-void SoccerDemo::callServiceSettingModule(
- const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule &modules) {
- humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule set_joint_srv;
- set_joint_srv.request.joint_name = modules.joint_name;
- set_joint_srv.request.module_name = modules.module_name;
-
- if (set_joint_module_client_.call(set_joint_srv) == false) {
- ROS_ERROR("Failed to set module");
- return;
- }
-
- return;
-}
-
-void SoccerDemo::parseJointNameFromYaml(const std::string &path) {
- YAML::Node doc;
- try {
- // load yaml
- doc = YAML::LoadFile(path.c_str());
- } catch (const std::exception &e) {
- ROS_ERROR("Fail to load id_joint table yaml.");
- return;
- }
-
- // parse id_joint table
- YAML::Node _id_sub_node = doc["id_joint"];
- for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end();
- ++_it) {
- int _id;
- std::string _joint_name;
-
- _id = _it->first.as();
- _joint_name = _it->second.as();
-
- id_joint_table_[_id] = _joint_name;
- joint_id_table_[_joint_name] = _id;
- }
-}
-
-// joint id -> joint name
-bool SoccerDemo::getJointNameFromID(const int &id, std::string &joint_name) {
- std::map::iterator _iter;
-
- _iter = id_joint_table_.find(id);
- if (_iter == id_joint_table_.end())
- return false;
-
- joint_name = _iter->second;
- return true;
-}
-
-// joint name -> joint id
-bool SoccerDemo::getIDFromJointName(const std::string &joint_name, int &id) {
- std::map::iterator _iter;
-
- _iter = joint_id_table_.find(joint_name);
- if (_iter == joint_id_table_.end())
- return false;
-
- id = _iter->second;
- return true;
-}
-
-int SoccerDemo::getJointCount() { return joint_id_table_.size(); }
-
-bool SoccerDemo::isHeadJoint(const int &id) {
- std::map::iterator _iter;
-
- for (_iter = joint_id_table_.begin(); _iter != joint_id_table_.end();
- ++_iter) {
- if (_iter->first.find("head") != std::string::npos)
- return true;
- }
-
- return false;
-}
-
-void SoccerDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (msg->data == "start") {
- if (on_following_ball_ == true)
- stopSoccerMode();
- else
- startSoccerMode();
- }
-}
-
-void SoccerDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (msg->data == "start") {
- if (on_following_ball_ == true)
- stopSoccerMode();
- else
- startSoccerMode();
- } else if (msg->data == "stop") {
- stopSoccerMode();
- }
-}
-
-// check fallen states
-void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (stop_fallen_check_ == true)
- return;
-
- Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x,
- msg->orientation.y, msg->orientation.z);
- Eigen::MatrixXd rpy_orientation =
- humanoid_robot_intelligence_control_system_framework::convertQuaternionToRPY(orientation);
- rpy_orientation *= (180 / M_PI);
-
- ROS_INFO_COND(DEBUG_PRINT, "Roll : %3.2f, Pitch : %2.2f",
- rpy_orientation.coeff(0, 0), rpy_orientation.coeff(1, 0));
-
- double pitch = rpy_orientation.coeff(1, 0);
-
- double alpha = 0.4;
- if (present_pitch_ == 0)
- present_pitch_ = pitch;
- else
- present_pitch_ = present_pitch_ * (1 - alpha) + pitch * alpha;
-
- if (present_pitch_ > FALL_FORWARD_LIMIT)
- stand_state_ = Fallen_Forward;
- else if (present_pitch_ < FALL_BACK_LIMIT)
- stand_state_ = Fallen_Behind;
- else
- stand_state_ = Stand;
-}
-
-void SoccerDemo::startSoccerMode() {
- setModuleToDemo("action_module");
-
- playMotion(WalkingReady);
-
- setBodyModuleToDemo("walking_module");
-
- ROS_INFO("Start Soccer Demo");
- on_following_ball_ = true;
- on_tracking_ball_ = true;
- start_following_ = true;
-}
-
-void SoccerDemo::stopSoccerMode() {
- ROS_INFO("Stop Soccer Demo");
- on_following_ball_ = false;
- on_tracking_ball_ = false;
- stop_following_ = true;
-}
-
-void SoccerDemo::handleKick(int ball_position) {
- usleep(1500 * 1000);
-
- // change to motion module
- setModuleToDemo("action_module");
-
- if (handleFallen(stand_state_) == true || enable_ == false)
- return;
-
- // kick motion
- switch (ball_position) {
- case humanoid_robot_intelligence_control_system_op::BallFollower::OnRight:
- std::cout << "Kick Motion [R]: " << ball_position << std::endl;
- playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
- break;
-
- case humanoid_robot_intelligence_control_system_op::BallFollower::OnLeft:
- std::cout << "Kick Motion [L]: " << ball_position << std::endl;
- playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
- break;
-
- default:
- break;
- }
-
- on_following_ball_ = false;
- restart_soccer_ = true;
- tracking_status_ = BallTracker::NotFound;
- ball_follower_.clearBallPosition();
-
- usleep(2000 * 1000);
-
- if (handleFallen(stand_state_) == true)
- return;
-
- // ceremony
- // playMotion(Ceremony);
-}
-
-void SoccerDemo::handleKick() {
- usleep(1500 * 1000);
-
- // change to motion module
- setModuleToDemo("action_module");
-
- if (handleFallen(stand_state_) == true || enable_ == false)
- return;
-
- // kick motion
- ball_follower_.decideBallPositin(ball_tracker_.getPanOfBall(),
- ball_tracker_.getTiltOfBall());
- int ball_position = ball_follower_.getBallPosition();
- if (ball_position == BallFollower::NotFound ||
- ball_position == BallFollower::OutOfRange) {
- on_following_ball_ = false;
- restart_soccer_ = true;
- tracking_status_ = BallTracker::NotFound;
- ball_follower_.clearBallPosition();
- return;
- }
-
- switch (ball_position) {
- case humanoid_robot_intelligence_control_system_op::BallFollower::OnRight:
- std::cout << "Kick Motion [R]: " << ball_position << std::endl;
- sendDebugTopic("Kick the ball using Right foot");
- playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
- break;
-
- case humanoid_robot_intelligence_control_system_op::BallFollower::OnLeft:
- std::cout << "Kick Motion [L]: " << ball_position << std::endl;
- sendDebugTopic("Kick the ball using Left foot");
- playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
- break;
-
- default:
- break;
- }
-
- on_following_ball_ = false;
- restart_soccer_ = true;
- tracking_status_ = BallTracker::NotFound;
- ball_follower_.clearBallPosition();
-
- usleep(2000 * 1000);
-
- if (handleFallen(stand_state_) == true)
- return;
-
- // ceremony
- // playMotion(Ceremony);
-}
-
-bool SoccerDemo::handleFallen(int fallen_status) {
- if (fallen_status == Stand) {
- return false;
- }
-
- // change to motion module
- setModuleToDemo("action_module");
-
- // getup motion
- switch (fallen_status) {
- case Fallen_Forward:
- std::cout << "Getup Motion [F]: " << std::endl;
- playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront);
- break;
-
- case Fallen_Behind:
- std::cout << "Getup Motion [B]: " << std::endl;
- playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack);
- break;
-
- default:
- break;
- }
-
- while (isActionRunning() == true)
- usleep(100 * 1000);
-
- usleep(650 * 1000);
-
- if (on_following_ball_ == true)
- restart_soccer_ = true;
-
- // reset state
- on_following_ball_ = false;
-
- return true;
-}
-
-void SoccerDemo::playMotion(int motion_index) {
- std_msgs::Int32 motion_msg;
- motion_msg.data = motion_index;
-
- motion_index_pub_.publish(motion_msg);
-}
-
-void SoccerDemo::setRGBLED(int blue, int green, int red) {
- int led_full_unit = 0x1F;
- int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 |
- (red & led_full_unit);
- humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
- syncwrite_msg.item_name = "LED_RGB";
- syncwrite_msg.joint_name.push_back("open-cr");
- syncwrite_msg.value.push_back(led_value);
-
- rgb_led_pub_.publish(syncwrite_msg);
-}
-
-// check running of action
-bool SoccerDemo::isActionRunning() {
- humanoid_robot_intelligence_control_system_action_module_msgs::IsRunning is_running_srv;
-
- if (is_running_client_.call(is_running_srv) == false) {
- ROS_ERROR("Failed to get action status");
- return true;
- } else {
- if (is_running_srv.response.is_running == true) {
- return true;
- }
- }
-
- return false;
-}
-
-void SoccerDemo::sendDebugTopic(const std::string &msgs) {
- std_msgs::String debug_msg;
- debug_msg.data = msgs;
-
- test_pub_.publish(debug_msg);
-}
-
-} // namespace humanoid_robot_intelligence_control_system_op
diff --git a/humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp b/humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp
deleted file mode 100644
index d6ea5a0..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/button_test.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-ButtonTest::ButtonTest() : SPIN_RATE(30), led_count_(0), rgb_led_count_(0) {
- enable_ = false;
-
- ros::NodeHandle nh(ros::this_node::getName());
-
- boost::thread queue_thread =
- boost::thread(boost::bind(&ButtonTest::callbackThread, this));
- boost::thread process_thread =
- boost::thread(boost::bind(&ButtonTest::processThread, this));
-
- default_mp3_path_ =
- ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/data/mp3/test/";
-}
-
-ButtonTest::~ButtonTest() {}
-
-void ButtonTest::setDemoEnable() {
- enable_ = true;
-
- ROS_INFO("Start Button Test");
-}
-
-void ButtonTest::setDemoDisable() { enable_ = false; }
-
-void ButtonTest::process() {}
-
-void ButtonTest::processThread() {
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- // node loop
- while (ros::ok()) {
- if (enable_ == true)
- process();
-
- // relax to fit output rate
- loop_rate.sleep();
- }
-}
-
-void ButtonTest::callbackThread() {
- ros::NodeHandle nh(ros::this_node::getName());
-
- // subscriber & publisher
- rgb_led_pub_ = nh.advertise(
- "/humanoid_robot_intelligence_control_system/sync_write_item", 0);
- play_sound_pub_ = nh.advertise("/play_sound_file", 0);
-
- buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1,
- &ButtonTest::buttonHandlerCallback, this);
-
- while (nh.ok()) {
- ros::spinOnce();
-
- usleep(1 * 1000);
- }
-}
-
-// button test
-void ButtonTest::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (msg->data == "mode") {
- playSound(default_mp3_path_ + "Mode button pressed.mp3");
- } else if (msg->data == "start") {
- // RGB led color test
- playSound(default_mp3_path_ + "Start button pressed.mp3");
- int rgb_selector[3] = {1, 0, 0};
- setRGBLED((0x1F * rgb_selector[rgb_led_count_ % 3]),
- (0x1F * rgb_selector[(rgb_led_count_ + 1) % 3]),
- (0x1F * rgb_selector[(rgb_led_count_ + 2) % 3]));
- rgb_led_count_ += 1;
- } else if (msg->data == "user") {
- // Monochromatic led color test
- playSound(default_mp3_path_ + "User button pressed.mp3");
- setLED(0x01 << (led_count_++ % 3));
- } else if (msg->data == "mode_long") {
- playSound(default_mp3_path_ + "Mode button long pressed.mp3");
- } else if (msg->data == "start_long") {
- playSound(default_mp3_path_ + "Start button long pressed.mp3");
- } else if (msg->data == "user_long") {
- playSound(default_mp3_path_ + "User button long pressed.mp3");
- }
-}
-
-void ButtonTest::setRGBLED(int blue, int green, int red) {
- int led_full_unit = 0x1F;
- int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 |
- (red & led_full_unit);
- humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
- syncwrite_msg.item_name = "LED_RGB";
- syncwrite_msg.joint_name.push_back("open-cr");
- syncwrite_msg.value.push_back(led_value);
-
- rgb_led_pub_.publish(syncwrite_msg);
-}
-
-void ButtonTest::setLED(int led) {
- humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
- syncwrite_msg.item_name = "LED";
- syncwrite_msg.joint_name.push_back("open-cr");
- syncwrite_msg.value.push_back(led);
-
- rgb_led_pub_.publish(syncwrite_msg);
-}
-
-void ButtonTest::playSound(const std::string &path) {
- std_msgs::String sound_msg;
- sound_msg.data = path;
-
- play_sound_pub_.publish(sound_msg);
-}
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
diff --git a/humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp b/humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp
deleted file mode 100644
index 008755b..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp
+++ /dev/null
@@ -1,238 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/mic_test.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-MicTest::MicTest()
- : SPIN_RATE(30), is_wait_(false), wait_time_(-1), test_status_(Ready),
- record_pid_(-1), play_pid_(-1) {
- enable_ = false;
-
- ros::NodeHandle nh(ros::this_node::getName());
-
- boost::thread queue_thread =
- boost::thread(boost::bind(&MicTest::callbackThread, this));
- boost::thread process_thread =
- boost::thread(boost::bind(&MicTest::processThread, this));
-
- recording_file_name_ = ros::package::getPath("humanoid_robot_intelligence_control_system_demo") +
- "/data/mp3/test/mic-test.wav";
- default_mp3_path_ =
- ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/data/mp3/test/";
-
- start_time_ = ros::Time::now();
-}
-
-MicTest::~MicTest() {}
-
-void MicTest::setDemoEnable() {
- wait_time_ = -1;
- test_status_ = AnnounceRecording;
- enable_ = true;
-
- ROS_INFO("Start Mic test Demo");
-}
-
-void MicTest::setDemoDisable() {
- finishTimer();
-
- test_status_ = Ready;
- enable_ = false;
-}
-
-void MicTest::process() {
- // check status
- // timer
- if (wait_time_ > 0) {
- ros::Duration dur = ros::Time::now() - start_time_;
-
- // check timer
- if (dur.sec >= wait_time_) {
- finishTimer();
- }
- } else if (wait_time_ == -1.0) {
- // handle test process
- switch (test_status_) {
- case Ready:
- // do nothing
- break;
-
- case AnnounceRecording:
- announceTest();
- test_status_ = MicRecording;
- break;
-
- case MicRecording:
- recordSound();
- test_status_ = PlayingSound;
- break;
-
- case PlayingSound:
- playTestSound(recording_file_name_);
- test_status_ = DeleteTempFile;
- break;
-
- case DeleteTempFile:
- deleteSoundFile(recording_file_name_);
- test_status_ = Ready;
- break;
-
- default:
- break;
- }
- }
-}
-
-void MicTest::processThread() {
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- // node loop
- while (ros::ok()) {
- if (enable_ == true)
- process();
-
- // relax to fit output rate
- loop_rate.sleep();
- }
-}
-
-void MicTest::callbackThread() {
- ros::NodeHandle nh(ros::this_node::getName());
-
- // subscriber & publisher
- buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1,
- &MicTest::buttonHandlerCallback, this);
- play_sound_pub_ = nh.advertise("/play_sound_file", 0);
-
- while (nh.ok()) {
- ros::spinOnce();
-
- usleep(1 * 1000);
- }
-}
-
-void MicTest::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (msg->data == "start") {
- // restart mic test
- if (test_status_ != Ready)
- return;
-
- test_status_ = AnnounceRecording;
- } else if (msg->data == "user") {
- is_wait_ = true;
- }
-}
-
-void MicTest::announceTest() {
- // play mic test sound
- playSound(default_mp3_path_ + "Announce mic test.mp3");
-
- usleep(3.4 * 1000 * 1000);
-}
-
-void MicTest::recordSound(int recording_time) {
- ROS_INFO("Start to record");
-
- playSound(default_mp3_path_ + "Start recording.mp3");
-
- usleep(1.5 * 1000 * 1000);
-
- if (record_pid_ != -1)
- kill(record_pid_, SIGKILL);
-
- record_pid_ = fork();
-
- switch (record_pid_) {
- case -1:
- fprintf(stderr, "Fork Failed!! \n");
- ROS_WARN("Fork Failed!! \n");
- break;
-
- case 0: {
- std::stringstream ss;
- ss << "-d " << recording_time;
- execl("/usr/bin/arecord", "arecord", "-Dplughw:1,0", "-fS16_LE", "-c1",
- "-r22050", "-twav", ss.str().c_str(), recording_file_name_.c_str(),
- (char *)0);
- break;
- }
-
- default:
- break;
- }
-
- startTimer(recording_time);
-}
-
-void MicTest::recordSound() { recordSound(5); }
-
-void MicTest::playTestSound(const std::string &path) {
- ROS_INFO("Start to play recording sound");
-
- playSound(default_mp3_path_ + "Start playing.mp3");
-
- usleep(1.3 * 1000 * 1000);
-
- if (play_pid_ != -1)
- kill(play_pid_, SIGKILL);
-
- play_pid_ = fork();
-
- switch (play_pid_) {
- case -1:
- fprintf(stderr, "Fork Failed!! \n");
- ROS_WARN("Fork Failed!! \n");
- break;
-
- case 0:
- execl("/usr/bin/aplay", "aplay", path.c_str(), (char *)0);
- break;
-
- default:
- break;
- }
-
- startTimer(5);
-}
-
-void MicTest::playSound(const std::string &path) {
- std_msgs::String sound_msg;
- sound_msg.data = path;
-
- play_sound_pub_.publish(sound_msg);
-}
-
-void MicTest::deleteSoundFile(const std::string &file_path) {
- remove(file_path.c_str());
- ROS_INFO("Delete temporary file");
-}
-
-void MicTest::startTimer(double wait_time) {
- start_time_ = ros::Time::now();
- wait_time_ = wait_time;
-}
-
-void MicTest::finishTimer() { wait_time_ = -1; }
-
-} /* namespace humanoid_robot_intelligence_control_system_op */
diff --git a/humanoid_robot_intelligence_control_system_demo/src/test_node.cpp b/humanoid_robot_intelligence_control_system_demo/src/test_node.cpp
deleted file mode 100644
index 53e1209..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/test_node.cpp
+++ /dev/null
@@ -1,393 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include
-#include
-#include
-
-#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
-#include "humanoid_robot_intelligence_control_system_demo/action_demo.h"
-#include "humanoid_robot_intelligence_control_system_demo/button_test.h"
-#include "humanoid_robot_intelligence_control_system_demo/mic_test.h"
-#include "humanoid_robot_intelligence_control_system_demo/soccer_demo.h"
-#include "humanoid_robot_intelligence_control_system_demo/vision_demo.h"
-#include "humanoid_robot_intelligence_control_system_math/humanoid_robot_intelligence_control_system_linear_algebra.h"
-
-enum Demo_Status {
- Ready = 0,
- ButtonTest = 1,
- MicTest = 2,
- SoccerDemo = 3,
- VisionDemo = 4,
- ActionDemo = 5,
- DemoCount = 6,
-};
-
-void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
-void goInitPose();
-void playSound(const std::string &path);
-void setLED(int led);
-bool checkManagerRunning(std::string &manager_name);
-void dxlTorqueChecker();
-
-void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg);
-
-const int SPIN_RATE = 30;
-const bool DEBUG_PRINT = true;
-
-ros::Publisher init_pose_pub;
-ros::Publisher play_sound_pub;
-ros::Publisher led_pub;
-ros::Publisher dxl_torque_pub;
-
-std::string default_mp3_path = "";
-int current_status = Ready;
-int desired_status = Ready;
-bool apply_desired = false;
-
-// node main
-int main(int argc, char **argv) {
- // init ros
- ros::init(argc, argv, "self_test_node");
-
- // create ros wrapper object
- humanoid_robot_intelligence_control_system_op::OPDemo *current_demo = NULL;
- humanoid_robot_intelligence_control_system_op::SoccerDemo *soccer_demo =
- new humanoid_robot_intelligence_control_system_op::SoccerDemo();
- humanoid_robot_intelligence_control_system_op::ActionDemo *action_demo =
- new humanoid_robot_intelligence_control_system_op::ActionDemo();
- humanoid_robot_intelligence_control_system_op::VisionDemo *vision_demo =
- new humanoid_robot_intelligence_control_system_op::VisionDemo();
- humanoid_robot_intelligence_control_system_op::ButtonTest *button_test =
- new humanoid_robot_intelligence_control_system_op::ButtonTest();
- humanoid_robot_intelligence_control_system_op::MicTest *mic_test = new humanoid_robot_intelligence_control_system_op::MicTest();
-
- ros::NodeHandle nh(ros::this_node::getName());
-
- init_pose_pub =
- nh.advertise("/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
- play_sound_pub = nh.advertise("/play_sound_file", 0);
- led_pub = nh.advertise(
- "/humanoid_robot_intelligence_control_system/sync_write_item", 0);
- dxl_torque_pub =
- nh.advertise("/humanoid_robot_intelligence_control_system/dxl_torque", 0);
- ros::Subscriber buttuon_sub =
- nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, buttonHandlerCallback);
- ros::Subscriber mode_command_sub =
- nh.subscribe("/humanoid_robot_intelligence_control_system/mode_command", 1, demoModeCommandCallback);
-
- default_mp3_path =
- ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/data/mp3/";
-
- ros::start();
-
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- // wait for starting of manager
- std::string manager_name = "/humanoid_robot_intelligence_control_system_manager";
- while (ros::ok()) {
- ros::Duration(1.0).sleep();
-
- if (checkManagerRunning(manager_name) == true) {
- break;
- ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect");
- }
- ROS_WARN("Waiting for humanoid_robot_intelligence_control_system manager");
- }
-
- // init procedure
- playSound(default_mp3_path + "test/Self test ready mode.mp3");
- setLED(0x01 | 0x02 | 0x04);
-
- // node loop
- while (ros::ok()) {
- // process
- if (apply_desired == true) {
- switch (desired_status) {
- case Ready: {
-
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = NULL;
-
- goInitPose();
-
- ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]");
- break;
- }
-
- case SoccerDemo: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = soccer_demo;
- current_demo->setDemoEnable();
-
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo");
- break;
- }
-
- case VisionDemo: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = vision_demo;
- current_demo->setDemoEnable();
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo");
- break;
- }
- case ActionDemo: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = action_demo;
- current_demo->setDemoEnable();
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo");
- break;
- }
- case ButtonTest: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = button_test;
- current_demo->setDemoEnable();
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test");
- break;
- }
- case MicTest: {
- if (current_demo != NULL)
- current_demo->setDemoDisable();
-
- current_demo = mic_test;
- current_demo->setDemoEnable();
- ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test");
- break;
- }
-
- default: {
- break;
- }
- }
-
- apply_desired = false;
- current_status = desired_status;
- }
-
- // execute pending callbacks
- ros::spinOnce();
-
- // relax to fit output rate
- loop_rate.sleep();
-
- // for debug
- if (checkManagerRunning(manager_name) == false)
- return 0;
- }
-
- // exit program
- return 0;
-}
-
-void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
- if (apply_desired == true)
- return;
-
- // in the middle of playing demo
- if (current_status != Ready) {
- if (msg->data == "mode_long") {
- // go to mode selection status
- desired_status = Ready;
- apply_desired = true;
-
- playSound(default_mp3_path + "test/Self test ready mode.mp3");
- setLED(0x01 | 0x02 | 0x04);
- } else if (msg->data == "user_long") {
- // it's using in humanoid_robot_intelligence_control_system_manager
- // torque on and going to init pose
- }
- }
- // ready to start demo
- else {
- if (msg->data == "start") {
- // select current demo
- desired_status =
- (desired_status == Ready) ? desired_status + 1 : desired_status;
- apply_desired = true;
-
- // sound out desired status
- switch (desired_status) {
- case SoccerDemo:
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start soccer demonstration.mp3");
- break;
-
- case VisionDemo:
- dxlTorqueChecker();
- playSound(default_mp3_path +
- "Start vision processing demonstration.mp3");
- break;
-
- case ActionDemo:
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start motion demonstration.mp3");
- break;
-
- case ButtonTest:
- dxlTorqueChecker();
- playSound(default_mp3_path + "test/Start button test mode.mp3");
- break;
-
- case MicTest:
- dxlTorqueChecker();
- playSound(default_mp3_path + "test/Start mic test mode.mp3");
- break;
-
- default:
- break;
- }
-
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- } else if (msg->data == "mode") {
- // change to next demo
- desired_status = (desired_status + 1) % DemoCount;
- desired_status =
- (desired_status == Ready) ? desired_status + 1 : desired_status;
-
- // sound out desired status and changign LED
- switch (desired_status) {
- case SoccerDemo:
- playSound(default_mp3_path + "Autonomous soccer mode.mp3");
- setLED(0x01);
- break;
-
- case VisionDemo:
- playSound(default_mp3_path + "Vision processing mode.mp3");
- setLED(0x02);
- break;
-
- case ActionDemo:
- playSound(default_mp3_path + "Interactive motion mode.mp3");
- setLED(0x04);
- break;
-
- case ButtonTest:
- playSound(default_mp3_path + "test/Button test mode.mp3");
- setLED(0x01 | 0x02);
- break;
-
- case MicTest:
- playSound(default_mp3_path + "test/Mic test mode.mp3");
- setLED(0x01 | 0x04);
- break;
-
- default:
- break;
- }
-
- ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status);
- }
- }
-}
-
-void goInitPose() {
- std_msgs::String init_msg;
- init_msg.data = "ini_pose";
-
- init_pose_pub.publish(init_msg);
-}
-
-void playSound(const std::string &path) {
- std_msgs::String sound_msg;
- sound_msg.data = path;
-
- play_sound_pub.publish(sound_msg);
-}
-
-void setLED(int led) {
- humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
- syncwrite_msg.item_name = "LED";
- syncwrite_msg.joint_name.push_back("open-cr");
- syncwrite_msg.value.push_back(led);
-
- led_pub.publish(syncwrite_msg);
-}
-
-bool checkManagerRunning(std::string &manager_name) {
- std::vector node_list;
- ros::master::getNodes(node_list);
-
- for (unsigned int node_list_idx = 0; node_list_idx < node_list.size();
- node_list_idx++) {
- if (node_list[node_list_idx] == manager_name)
- return true;
- }
-
- ROS_ERROR("Can't find humanoid_robot_intelligence_control_system_manager");
- return false;
-}
-
-void dxlTorqueChecker() {
- std_msgs::String check_msg;
- check_msg.data = "check";
-
- dxl_torque_pub.publish(check_msg);
-}
-
-void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) {
- // In demo mode
- if (current_status != Ready) {
- if (msg->data == "ready") {
- // go to mode selection status
- desired_status = Ready;
- apply_desired = true;
-
- playSound(default_mp3_path + "Demonstration ready mode.mp3");
- setLED(0x01 | 0x02 | 0x04);
- }
- }
- // In ready mode
- else {
- if (msg->data == "soccer") {
- desired_status = SoccerDemo;
- apply_desired = true;
-
- // play sound
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start soccer demonstration.mp3");
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- } else if (msg->data == "vision") {
- desired_status = VisionDemo;
- apply_desired = true;
-
- // play sound
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start vision processing demonstration.mp3");
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- } else if (msg->data == "action") {
- desired_status = ActionDemo;
- apply_desired = true;
-
- // play sound
- dxlTorqueChecker();
- playSound(default_mp3_path + "Start motion demonstration.mp3");
- ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
- }
- }
-}
diff --git a/humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp b/humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp
deleted file mode 100644
index 20de21e..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp
+++ /dev/null
@@ -1,181 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/face_tracker.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-FaceTracker::FaceTracker()
- : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180),
- FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50),
- use_head_scan_(false), count_not_found_(0), on_tracking_(false) {
- head_joint_offset_pub_ = nh_.advertise(
- "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0);
- head_joint_pub_ = nh_.advertise(
- "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0);
- head_scan_pub_ = nh_.advertise(
- "/humanoid_robot_intelligence_control_system/head_control/scan_command", 0);
-
- face_position_sub_ = nh_.subscribe("/face_position", 1,
- &FaceTracker::facePositionCallback, this);
- // face_tracking_command_sub_ = nh_.subscribe("/humanoid_robot_intelligence_control_system/demo_command",
- // 1, &FaceTracker::faceTrackerCommandCallback, this);
-}
-
-FaceTracker::~FaceTracker() {}
-
-void FaceTracker::facePositionCallback(
- const geometry_msgs::Point::ConstPtr &msg) {
- if (msg->z < 0)
- return;
-
- face_position_ = *msg;
-}
-
-void FaceTracker::faceTrackerCommandCallback(
- const std_msgs::String::ConstPtr &msg) {
- if (msg->data == "start") {
- startTracking();
- } else if (msg->data == "stop") {
- stopTracking();
- } else if (msg->data == "toggle_start") {
- if (on_tracking_ == false)
- startTracking();
- else
- stopTracking();
- }
-}
-
-void FaceTracker::startTracking() {
- on_tracking_ = true;
-
- ROS_INFO("Start Face tracking");
-}
-
-void FaceTracker::stopTracking() {
- on_tracking_ = false;
-
- ROS_INFO("Stop Face tracking");
-}
-
-void FaceTracker::setUsingHeadScan(bool use_scan) { use_head_scan_ = use_scan; }
-
-void FaceTracker::setFacePosition(geometry_msgs::Point &face_position) {
- if (face_position.z > 0) {
- face_position_ = face_position;
- }
-}
-
-void FaceTracker::goInit(double init_pan, double init_tile) {
- sensor_msgs::JointState head_angle_msg;
-
- head_angle_msg.name.push_back("head_pan");
- head_angle_msg.name.push_back("head_tilt");
-
- head_angle_msg.position.push_back(init_pan);
- head_angle_msg.position.push_back(init_tile);
-
- head_joint_pub_.publish(head_angle_msg);
-}
-
-int FaceTracker::processTracking() {
- if (on_tracking_ == false) {
- face_position_.z = 0;
- count_not_found_ = 0;
- // return false;
- return Waiting;
- }
-
- // check ball position
- if (face_position_.z <= 0) {
- count_not_found_++;
-
- if (count_not_found_ == NOT_FOUND_THRESHOLD) {
- scanFace();
- // count_not_found_ = 0;
- return NotFound;
- } else if (count_not_found_ > NOT_FOUND_THRESHOLD) {
- return NotFound;
- } else {
- return Waiting;
- }
-
- // return false;
- }
-
- // if face is detected
- double x_error = -atan(face_position_.x * tan(FOV_WIDTH));
- double y_error = -atan(face_position_.y * tan(FOV_HEIGHT));
-
- face_position_.z = 0;
- count_not_found_ = 0;
-
- double p_gain = 0.6, d_gain = 0.25;
- double x_error_diff = x_error - current_face_pan_;
- double y_error_diff = y_error - current_face_tilt_;
- double x_error_target = x_error * p_gain + x_error_diff * d_gain;
- double y_error_target = y_error * p_gain + y_error_diff * d_gain;
-
- // move head joint
- publishHeadJoint(x_error_target, y_error_target);
-
- current_face_pan_ = x_error;
- current_face_tilt_ = y_error;
-
- // return true;
- return Found;
-}
-
-void FaceTracker::publishHeadJoint(double pan, double tilt) {
- double min_angle = 1 * M_PI / 180;
- if (fabs(pan) < min_angle && fabs(tilt) < min_angle) {
- dismissed_count_ += 1;
- return;
- }
- std::cout << "Target angle[" << dismissed_count_ << "] : " << pan << " | "
- << tilt << std::endl;
-
- dismissed_count_ = 0;
-
- sensor_msgs::JointState head_angle_msg;
-
- head_angle_msg.name.push_back("head_pan");
- head_angle_msg.name.push_back("head_tilt");
-
- head_angle_msg.position.push_back(pan);
- head_angle_msg.position.push_back(tilt);
-
- head_joint_offset_pub_.publish(head_angle_msg);
-}
-
-void FaceTracker::scanFace() {
- if (use_head_scan_ == false)
- return;
-
- // check head control module enabled
- // ...
-
- // send message to head control module
- std_msgs::String scan_msg;
- scan_msg.data = "scan";
-
- head_scan_pub_.publish(scan_msg);
- // ROS_INFO("Scan the ball");
-}
-
-} // namespace humanoid_robot_intelligence_control_system_op
diff --git a/humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp b/humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp
deleted file mode 100644
index 01e0047..0000000
--- a/humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp
+++ /dev/null
@@ -1,227 +0,0 @@
-/*******************************************************************************
- * Copyright 2017 ROBOTIS CO., LTD.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-
-/* Author: Kayman Jung */
-
-#include "humanoid_robot_intelligence_control_system_demo/vision_demo.h"
-
-namespace humanoid_robot_intelligence_control_system_op {
-
-VisionDemo::VisionDemo()
- : SPIN_RATE(30), TIME_TO_INIT(10), tracking_status_(FaceTracker::Waiting) {
- enable_ = false;
-
- ros::NodeHandle nh(ros::this_node::getName());
-
- boost::thread queue_thread =
- boost::thread(boost::bind(&VisionDemo::callbackThread, this));
- boost::thread process_thread =
- boost::thread(boost::bind(&VisionDemo::processThread, this));
-}
-
-VisionDemo::~VisionDemo() {
- // TODO Auto-generated destructor stub
-}
-
-void VisionDemo::setDemoEnable() {
- // set prev time for timer
- prev_time_ = ros::Time::now();
-
- // change to motion module
- setModuleToDemo("action_module");
-
- playMotion(InitPose);
-
- usleep(1500 * 1000);
-
- setModuleToDemo("head_control_module");
-
- enable_ = true;
-
- // send command to start face_tracking
- std_msgs::Bool command;
- command.data = enable_;
- face_tracking_command_pub_.publish(command);
-
- face_tracker_.startTracking();
-
- ROS_INFO("Start Vision Demo");
-}
-
-void VisionDemo::setDemoDisable() {
-
- face_tracker_.stopTracking();
- tracking_status_ = FaceTracker::Waiting;
- enable_ = false;
-
- std_msgs::Bool command;
- command.data = enable_;
- face_tracking_command_pub_.publish(command);
-}
-
-void VisionDemo::process() {
- int tracking_status = face_tracker_.processTracking();
-
- switch (tracking_status) {
- case FaceTracker::Found:
- if (tracking_status_ != tracking_status)
- setRGBLED(0x1F, 0x1F, 0x1F);
- prev_time_ = ros::Time::now();
- break;
-
- case FaceTracker::NotFound: {
- if (tracking_status_ != tracking_status)
- setRGBLED(0, 0, 0);
-
- ros::Time curr_time = ros::Time::now();
- ros::Duration dur = curr_time - prev_time_;
- if (dur.sec > TIME_TO_INIT) {
- face_tracker_.goInit(0, 0);
- prev_time_ = curr_time;
- }
- break;
- }
- default:
- break;
- }
-
- if (tracking_status != FaceTracker::Waiting)
- tracking_status_ = tracking_status;
-}
-
-void VisionDemo::processThread() {
- // set node loop rate
- ros::Rate loop_rate(SPIN_RATE);
-
- // node loop
- while (ros::ok()) {
- if (enable_ == true)
- process();
-
- // relax to fit output rate
- loop_rate.sleep();
- }
-}
-
-void VisionDemo::callbackThread() {
- ros::NodeHandle nh(ros::this_node::getName());
-
- // subscriber & publisher
- module_control_pub_ =
- nh.advertise("/humanoid_robot_intelligence_control_system/enable_ctrl_module", 0);
- motion_index_pub_ =
- nh.advertise("/humanoid_robot_intelligence_control_system/action/page_num", 0);
- rgb_led_pub_ = nh.advertise(
- "/humanoid_robot_intelligence_control_system/sync_write_item", 0);
- face_tracking_command_pub_ =
- nh.advertise("/face_tracking/command", 0);
-
- buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1,
- &VisionDemo::buttonHandlerCallback, this);
- faceCoord_sub_ =
- nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this);
-
- set_joint_module_client_ =
- nh.serviceClient(
- "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules");
-
- while (nh.ok()) {
- ros::spinOnce();
-
- usleep(1 * 1000);
- }
-}
-
-void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (msg->data == "start") {
-
- } else if (msg->data == "mode") {
- }
-}
-
-void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- if (msg->data == "start") {
-
- } else if (msg->data == "stop") {
- }
-}
-
-void VisionDemo::setModuleToDemo(const std::string &module_name) {
- callServiceSettingModule(module_name);
- ROS_INFO_STREAM("enable module : " << module_name);
-}
-
-void VisionDemo::callServiceSettingModule(const std::string &module_name) {
- humanoid_robot_intelligence_control_system_controller_msgs::SetModule set_module_srv;
- set_module_srv.request.module_name = module_name;
-
- if (set_joint_module_client_.call(set_module_srv) == false) {
- ROS_ERROR("Failed to set module");
- return;
- }
-
- return;
-}
-
-void VisionDemo::facePositionCallback(
- const std_msgs::Int32MultiArray::ConstPtr &msg) {
- if (enable_ == false)
- return;
-
- // face is detected
- if (msg->data.size() >= 10) {
- // center of face
- face_position_.x =
- (msg->data[6] + msg->data[8] * 0.5) / msg->data[2] * 2 - 1;
- face_position_.y =
- (msg->data[7] + msg->data[9] * 0.5) / msg->data[3] * 2 - 1;
- face_position_.z = msg->data[8] * 0.5 + msg->data[9] * 0.5;
-
- face_tracker_.setFacePosition(face_position_);
- } else {
- face_position_.x = 0;
- face_position_.y = 0;
- face_position_.z = 0;
- return;
- }
-}
-
-void VisionDemo::playMotion(int motion_index) {
- std_msgs::Int32 motion_msg;
- motion_msg.data = motion_index;
-
- motion_index_pub_.publish(motion_msg);
-}
-
-void VisionDemo::setRGBLED(int blue, int green, int red) {
- int led_full_unit = 0x1F;
- int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 |
- (red & led_full_unit);
- humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
- syncwrite_msg.item_name = "LED_RGB";
- syncwrite_msg.joint_name.push_back("open-cr");
- syncwrite_msg.value.push_back(led_value);
-
- rgb_led_pub_.publish(syncwrite_msg);
-}
-
-} /* namespace humanoid_robot_intelligence_control_system_op */