BRSRI/bellande_humanoid_robot_intelligence_control_system_architecture
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

latest pushes

Browse Source
This commit is contained in:
Ronaldson Bellande 2024-07-23 13:50:25 -04:00
parent f09ffa1b20
commit 7dbd26da6e
25 changed files with 16 additions and 4046 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py
View File

@ -13,7 +13,6 @@
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
#!/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)

6
humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py
View File

@ -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,

34
humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst
View File

@ -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

153
humanoid_robot_intelligence_control_system_demo/CMakeLists.txt
View File

@ -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}
)

114
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h
View File

@ -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 <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#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<int, std::string> action_sound_table_;
std::vector<int> 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_ */

129
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h
View File

@ -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 <geometry_msgs/Point.h>
#include <math.h>
#include <numeric>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#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_ */

113
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h
View File

@ -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 <geometry_msgs/Point.h>
#include <math.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#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_ */

66
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h
View File

@ -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 <boost/thread.hpp>
#include <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#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_ */

94
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h
View File

@ -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 <geometry_msgs/Point.h>
#include <math.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
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_ */

86
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h
View File

@ -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 <boost/thread.hpp>
#include <ros/package.h>
#include <ros/ros.h>
#include <signal.h>
#include <std_msgs/String.h>
#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_ */

49
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h
View File

@ -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_ */

134
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h
View File

@ -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 <boost/thread.hpp>
#include <eigen3/Eigen/Eigen>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#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<int, std::string> id_joint_table_;
std::map<std::string, int> joint_id_table_;
bool is_grass_;
int wait_count_;
bool on_following_ball_;
bool on_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

83
humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h
View File

@ -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 <boost/thread.hpp>
#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/String.h>
#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_ */

28
humanoid_robot_intelligence_control_system_demo/launch/demo.launch
View File

@ -1,28 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system manager -->
<include
file="$(find humanoid_robot_intelligence_control_system_manager)/launch/humanoid_robot_intelligence_control_system_manager.launch" />
<!-- Camera and Ball detector -->
<include
file="$(find humanoid_robot_intelligence_control_system_ball_detector)/launch/ball_detector_from_usb_cam.launch" />
<!-- camera setting tool -->
<include
file="$(find humanoid_robot_intelligence_control_system_camera_setting_tool)/launch/humanoid_robot_intelligence_control_system_camera_setting_tool.launch" />
<!-- sound player -->
<node pkg="humanoid_robot_intelligence_control_system_player"
type="humanoid_robot_intelligence_control_system_player"
name="humanoid_robot_intelligence_control_system_player" output="screen" />
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system demo -->
<node pkg="humanoid_robot_intelligence_control_system_demo" type="op_demo_node"
name="humanoid_robot_intelligence_control_system_demo" output="screen">
<param name="grass_demo" type="bool" value="False" />
<param name="p_gain" value="0.45" />
<param name="d_gain" value="0.045" />
</node>
</launch>

29
humanoid_robot_intelligence_control_system_demo/launch/self_test.launch
View File

@ -1,29 +0,0 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system manager -->
<include
file="$(find humanoid_robot_intelligence_control_system_manager)/launch/humanoid_robot_intelligence_control_system_manager.launch" />
<!-- Camera and Ball detector -->
<include
file="$(find humanoid_robot_intelligence_control_system_ball_detector)/launch/ball_detector_from_usb_cam.launch" />
<!-- camera setting tool -->
<include
file="$(find humanoid_robot_intelligence_control_system_camera_setting_tool)/launch/humanoid_robot_intelligence_control_system_camera_setting_tool.launch" />
<!-- sound player -->
<node pkg="humanoid_robot_intelligence_control_system_player"
type="humanoid_robot_intelligence_control_system_player"
name="humanoid_robot_intelligence_control_system_player" output="screen" />
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system self
test demo -->
<node pkg="humanoid_robot_intelligence_control_system_demo" type="self_test_node"
name="humanoid_robot_intelligence_control_system_self_test" output="screen">
<param name="grass_demo" type="bool" value="False" />
<param name="p_gain" value="0.45" />
<param name="d_gain" value="0.045" />
</node>
</launch>

180
humanoid_robot_intelligence_control_system_demo/package.xml
View File

@ -1,180 +0,0 @@
<?xml version="1.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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_demo</name>
<version>0.3.2</version>
<description>
HUMANOID_ROBOT default demo
It includes three demontrations(soccer demo, vision demo, action script demo)
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 1">roslib</build_depend>
<build_depend condition="$ROS_VERSION == 1">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">geometry_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_walking_module_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_action_module_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">cmake_modules</build_depend>
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_math</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_object_detector</build_depend>
<build_depend condition="$ROS_VERSION == 1">boost</build_depend>
<build_depend condition="$ROS_VERSION == 1">eigen</build_depend>
<build_depend condition="$ROS_VERSION == 1">yaml-cpp</build_depend>
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_manager</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_camera_setting_tool</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_web_setting_tool</build_depend>
<build_export_depend condition="$ROS_VERSION == 1">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">roslib</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">geometry_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_walking_module_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_action_module_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">cmake_modules</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_math</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_object_detector</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">boost</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">eigen</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">yaml-cpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_manager</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_camera_setting_tool</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_web_setting_tool</build_export_depend>
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">roslib</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">geometry_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_walking_module_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_action_module_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">cmake_modules</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_math</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_object_detector</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">boost</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">eigen</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">yaml-cpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_manager</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_camera_setting_tool</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_web_setting_tool</exec_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend condition="$ROS_VERSION == 2">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">roslib</build_depend>
<build_depend condition="$ROS_VERSION == 2">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">geometry_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_walking_module_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_action_module_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">cmake_modules</build_depend>
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_math</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_object_detector</build_depend>
<build_depend condition="$ROS_VERSION == 2">boost</build_depend>
<build_depend condition="$ROS_VERSION == 2">eigen</build_depend>
<build_depend condition="$ROS_VERSION == 2">yaml-cpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_manager</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_camera_setting_tool</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_web_setting_tool</build_depend>
<build_export_depend condition="$ROS_VERSION == 2">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">roslib</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">geometry_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_walking_module_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_action_module_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">cmake_modules</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_math</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_object_detector</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">boost</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">eigen</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">yaml-cpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_manager</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_camera_setting_tool</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_web_setting_tool</build_export_depend>
<exec_depend condition="$ROS_VERSION == 2">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">roslib</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">geometry_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_walking_module_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_action_module_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">cmake_modules</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_math</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_object_detector</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">boost</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">eigen</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">yaml-cpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_manager</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_camera_setting_tool</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_web_setting_tool</exec_depend>
</package>

351
humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp
View File

@ -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 <ros/ros.h>
#include <std_msgs/String.h>
#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<std_msgs::String>(
"/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub =
nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::
SyncWriteItem>(
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>(
"/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<std::string> 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);
}
}
}

325
humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp
View File

@ -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<std_msgs::String>("/humanoid_robot_intelligence_control_system/walking/command", 0);
set_walking_param_pub_ =
nh_.advertise<humanoid_robot_intelligence_control_system_walking_module_msgs::WalkingParam>(
"/humanoid_robot_intelligence_control_system/walking/set_params", 0);
get_walking_param_client_ =
nh_.serviceClient<humanoid_robot_intelligence_control_system_walking_module_msgs::GetWalkingParam>(
"/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

267
humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp
View File

@ -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<sensor_msgs::JointState>(
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>(
"/humanoid_robot_intelligence_control_system/head_control/scan_command", 0);
// error_pub_ =
// nh_.advertise<std_msgs::Float64MultiArray>("/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

619
humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp
View File

@ -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<std::string>("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<bool>("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_controller_msgs::JointCtrlModule>(
"/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 0);
motion_index_pub_ =
nh.advertise<std_msgs::Int32>("/humanoid_robot_intelligence_control_system/action/page_num", 0);
rgb_led_pub_ = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
"/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_module_msgs::IsRunning>(
"/humanoid_robot_intelligence_control_system/action/is_running");
set_joint_module_client_ =
nh.serviceClient<humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule>(
"/humanoid_robot_intelligence_control_system/set_present_joint_ctrl_modules");
test_pub_ = nh.advertise<std_msgs::String>("/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<int, std::string>::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<int, std::string>::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<int>();
_joint_name = _it->second.as<std::string>();
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<int, std::string>::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<std::string, int>::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<std::string, int>::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

137
humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp
View File

@ -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_controller_msgs::SyncWriteItem>(
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
play_sound_pub_ = nh.advertise<std_msgs::String>("/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 */

238
humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp
View File

@ -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<std_msgs::String>("/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 */

393
humanoid_robot_intelligence_control_system_demo/src/test_node.cpp
View File

@ -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 <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#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<std_msgs::String>("/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
dxl_torque_pub =
nh.advertise<std_msgs::String>("/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<std::string> 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);
}
}
}

181
humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp
View File

@ -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<sensor_msgs::JointState>(
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>(
"/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

227
humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp
View File

@ -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<std_msgs::String>("/humanoid_robot_intelligence_control_system/enable_ctrl_module", 0);
motion_index_pub_ =
nh.advertise<std_msgs::Int32>("/humanoid_robot_intelligence_control_system/action/page_num", 0);
rgb_led_pub_ = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
face_tracking_command_pub_ =
nh.advertise<std_msgs::Bool>("/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_controller_msgs::SetModule>(
"/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 */