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

cleaned up the code.

Browse Source
This commit is contained in:
Kayman 2017-06-08 09:19:28 +09:00
parent 3a6a268028
commit 50378f052f
20 changed files with 105 additions and 546 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ball_detector/include/ball_detector/ball_detector.h
View File

@ -33,7 +33,6 @@
#ifndef _BALL_DETECTOR_H_
#define _BALL_DETECTOR_H_
//std
#include <string>
#include <opencv2/core/core.hpp>

1
ball_detector/src/ball_detector_node.cpp
View File

@ -47,7 +47,6 @@ int main(int argc, char **argv)
//node loop
while (ros::ok())
{
//if new image , do things
if (detector.newImage())
{

158
op3_demo/CMakeLists.txt
View File

@ -1,9 +1,12 @@
################################################################################
# CMake
################################################################################
cmake_minimum_required(VERSION 2.8.3)
project(op3_demo)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
################################################################################
# Packages
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
@ -14,136 +17,41 @@ find_package(catkin REQUIRED COMPONENTS
robotis_math
)
## System dependencies are found with CMake's conventions
find_package(Eigen REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################################################
# Declare ROS messages, services and actions
################################################################################
################################################
## Declare ROS messages, services and actions ##
################################################
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# sensor_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
################################################################################
# Catkin specific configuration
################################################################################
catkin_package(
INCLUDE_DIRS include
# LIBRARIES ball_tracker
CATKIN_DEPENDS roscpp sensor_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(ball_tracking
# src/${PROJECT_NAME}/ball_tracking.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(ball_tracking ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
add_executable(ball_tracker_node
src/ball_tracker_node.cpp
src/soccer/ball_tracker.cpp
)
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(ball_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(ball_tracker_node
${catkin_LIBRARIES}
yaml-cpp
@ -155,11 +63,8 @@ add_executable(soccer_demo_node
src/soccer/ball_follower.cpp
)
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(soccer_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(soccer_demo_node
${catkin_LIBRARIES}
yaml-cpp
@ -175,11 +80,8 @@ add_executable(op_demo_node
src/vision/face_tracker.cpp
)
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(op_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(op_demo_node
${catkin_LIBRARIES}
yaml-cpp
@ -197,19 +99,16 @@ add_executable(self_test_node
src/test/mic_test.cpp
)
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(self_test_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(self_test_node
${catkin_LIBRARIES}
yaml-cpp
)
#############
## Install ##
#############
################################################################################
# Install
################################################################################
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
@ -242,15 +141,6 @@ target_link_libraries(self_test_node
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ball_tracking.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
################################################################################
# Test
################################################################################

5
op3_demo/include/op3_demo/action_demo.h
View File

@ -73,6 +73,7 @@ class ActionDemo : public OPDemo
const int SPIN_RATE;
const int DEMO_INIT_POSE;
const bool DEBUG_PRINT;
void processThread();
void callbackThread();
@ -118,10 +119,6 @@ class ActionDemo : public OPDemo
std::string play_list_name_;
int play_index_;
bool start_play_;
bool stop_play_;
bool pause_play_;
int play_status_;
};

2
op3_demo/include/op3_demo/ball_follower.h
View File

@ -75,6 +75,7 @@ class BallFollower
}
protected:
const bool DEBUG_PRINT;
const double CAMERA_HEIGHT;
const int NOT_FOUND_THRESHOLD;
const double FOV_WIDTH;
@ -95,7 +96,6 @@ class BallFollower
void setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance = true);
bool getWalkingParam();
bool debug_print_;
//ros node handle
ros::NodeHandle nh_;

3
op3_demo/include/op3_demo/ball_tracker.h
View File

@ -87,7 +87,6 @@ class BallTracker
void ballPositionCallback(const ball_detector::circleSetStamped::ConstPtr &msg);
void ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg);
void currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void publishHeadJoint(double pan, double tilt);
void scanBall();
@ -103,7 +102,6 @@ class BallTracker
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
@ -112,7 +110,6 @@ class BallTracker
bool use_head_scan_;
int count_not_found_;
bool on_tracking_;
double current_head_pan_, current_head_tilt_;
double current_ball_pan_, current_ball_tilt_;
double current_ball_bottom_;
ros::Time prev_time_;

17
op3_demo/include/op3_demo/button_test.h
View File

@ -38,11 +38,8 @@
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
//#include <std_msgs/Int32MultiArray.h>
//#include <geometry_msgs/Point.h>
#include "op3_demo/op_demo.h"
//#include "op3_demo/face_tracker.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
@ -66,31 +63,17 @@ class ButtonTest : public OPDemo
void process();
// void playMotion(int motion_index);
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);
// void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg);
// void setModuleToDemo(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 play_sound_pub_;
ros::Subscriber buttuon_sub_;
// ros::Subscriber faceCoord_sub_;
// geometry_msgs::Point face_position_;
// bool is_tracking_;
// int tracking_status_;
std::string default_mp3_path_;
int led_count_;
int rgb_led_count_;

4
op3_demo/include/op3_demo/op_demo.h
View File

@ -43,10 +43,6 @@ class OPDemo
{
InitPose = 1,
WalkingReady = 9,
//GetUpFront = 81,
//GetUpBack = 82,
//RightKick = 83,
//LeftKick = 84,
GetUpFront = 122,
GetUpBack = 123,
RightKick = 121,

6
op3_demo/include/op3_demo/soccer_demo.h
View File

@ -72,9 +72,10 @@ class SoccerDemo : public OPDemo
void setDemoDisable();
protected:
const double FALLEN_FORWARD_LIMIT;
const double FALLEN_BEHIND_LIMIT;
const double FALL_FORWARD_LIMIT;
const double FALL_BACK_LIMIT;
const int SPIN_RATE;
const bool DEBUG_PRINT;
void processThread();
void callbackThread();
@ -110,7 +111,6 @@ class SoccerDemo : public OPDemo
std::map<int, std::string> id_joint_table_;
std::map<std::string, int> joint_id_table_;
bool debug_code_;
int wait_count_;
bool on_following_ball_;
bool restart_soccer_;

47
op3_demo/package.xml
View File

@ -6,52 +6,18 @@
op3 default demo
It includes three demontrations(soccer demo, vision demo, action script demo)
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="kmjung@robotis.com">kayman</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ball_tracking</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<author email="kmjung@robotis.com">kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>ball_detector</build_depend>
<build_depend>op3_walking_module_msgs</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>robotis_math</build_depend>
<build_depend>robotis_math</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>sensor_msgs</run_depend>
@ -59,11 +25,4 @@
<run_depend>op3_walking_module_msgs</run_depend>
<run_depend>cmake_modules</run_depend>
<run_depend>robotis_math</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

26
op3_demo/script/action_script.yaml
View File

@ -1,19 +1,19 @@
# combination action page number and mp3 file path
action_and_sound:
4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3"
41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3"
24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3"
23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3"
15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3"
1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3"
54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3"
27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3"
# 101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3"
41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3"
24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3"
23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3"
15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3"
1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3"
54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3"
27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3"
# 101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
110 : ""
111 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro01.mp3"
115 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro02.mp3"
118 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro03.mp3"
111 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro01.mp3"
115 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro02.mp3"
118 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro03.mp3"
# play list
prev_default: [4, 41, 24, 23, 15, 1, 54, 27, 38]

58
op3_demo/src/action/action_demo.cpp
View File

@ -38,10 +38,8 @@ namespace robotis_op
ActionDemo::ActionDemo()
: SPIN_RATE(30),
DEMO_INIT_POSE(8),
DEBUG_PRINT(false),
play_index_(0),
start_play_(false),
stop_play_(false),
pause_play_(false),
play_status_(StopAction)
{
enable_ = false;
@ -62,7 +60,6 @@ ActionDemo::ActionDemo()
ActionDemo::~ActionDemo()
{
// TODO Auto-generated destructor stub
}
void ActionDemo::setDemoEnable()
@ -73,7 +70,7 @@ void ActionDemo::setDemoEnable()
enable_ = true;
ROS_INFO("Start ActionScript Demo");
ROS_INFO_COND(DEBUG_PRINT, "Start ActionScript Demo");
playAction(DEMO_INIT_POSE);
@ -91,9 +88,6 @@ void ActionDemo::setDemoDisable()
void ActionDemo::process()
{
// check current status
//handleStatus();
switch (play_status_)
{
case PlayAction:
@ -145,50 +139,25 @@ void ActionDemo::process()
}
}
void ActionDemo::handleStatus()
{
if (start_play_ == true)
{
play_status_ = PlayAction;
start_play_ = false;
}
if (pause_play_ == true)
{
play_status_ = PauseAction;
pause_play_ = false;
}
if (stop_play_ == true)
{
play_status_ = StopAction;
stop_play_ = false;
}
}
void ActionDemo::startProcess(const std::string &set_name)
{
parseActionScriptSetName(script_path_, set_name);
start_play_ = true;
play_status_ = PlayAction;
}
void ActionDemo::resumeProcess()
{
start_play_ = true;
play_status_ = PlayAction;
}
void ActionDemo::pauseProcess()
{
pause_play_ = true;
play_status_ = PauseAction;
}
void ActionDemo::stopProcess()
{
stop_play_ = true;
play_index_ = 0;
play_status_ = StopAction;
}
@ -294,7 +263,7 @@ bool ActionDemo::playActionWithSound(int motion_index)
playAction(motion_index);
playMP3(map_it->second);
ROS_INFO_STREAM("action : " << motion_index << ", mp3 path : " << map_it->second);
ROS_INFO_STREAM_COND(DEBUG_PRINT, "action : " << motion_index << ", mp3 path : " << map_it->second);
return true;
}
@ -399,27 +368,6 @@ void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
void ActionDemo::setModuleToDemo(const std::string &module_name)
{
// robotis_controller_msgs::JointCtrlModule control_msg;
//
// // todo : remove hard coding
// for (int ix = 1; ix <= 20; ix++)
// {
// std::string joint_name;
//
// if (getJointNameFromID(ix, joint_name) == false)
// continue;
//
// control_msg.joint_name.push_back(joint_name);
// control_msg.module_name.push_back(module_name);
// }
//
// // no control
// if (control_msg.joint_name.size() == 0)
// return;
//
// module_control_pub_.publish(control_msg);
// std::cout << "enable module : " << module_name << std::endl;
std_msgs::String control_msg;
control_msg.data = "action_module";

42
op3_demo/src/demo_node.cpp
View File

@ -55,6 +55,7 @@ void setLED(int led);
bool checkManagerRunning(std::string& manager_name);
const int SPIN_RATE = 30;
const bool DEBUG_PRINT = false;
ros::Publisher init_pose_pub;
ros::Publisher play_sound_pub;
@ -100,13 +101,14 @@ int main(int argc, char **argv)
if (checkManagerRunning(manager_name) == true)
{
break;
ROS_INFO("Succeed to connect");
ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect");
}
ROS_WARN("Waiting for op3 manager");
}
// init procedure
playSound(default_mp3_path + "Demonstration ready mode.mp3");
// turn on R/G/B LED
setLED(0x01 | 0x02 | 0x04);
//node loop
@ -127,7 +129,7 @@ int main(int argc, char **argv)
goInitPose();
ROS_INFO("[Go to Demo READY!]");
ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]");
break;
}
@ -139,7 +141,7 @@ int main(int argc, char **argv)
current_demo = soccer_demo;
current_demo->setDemoEnable();
ROS_INFO("[Start] Soccer Demo");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo");
break;
}
@ -150,7 +152,7 @@ int main(int argc, char **argv)
current_demo = vision_demo;
current_demo->setDemoEnable();
ROS_INFO("[Start] Vision Demo");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo");
break;
}
case ActionDemo:
@ -160,7 +162,7 @@ int main(int argc, char **argv)
current_demo = action_demo;
current_demo->setDemoEnable();
ROS_INFO("[Start] Action Demo");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo");
break;
}
@ -173,30 +175,6 @@ int main(int argc, char **argv)
apply_desired = false;
current_status = desired_status;
}
else
{
if (current_status != desired_status)
{
// // sound out desired status
// switch (desired_status)
// {
// case SoccerDemo:
// playSound(default_path + "Autonomous soccer mode.mp3");
// break;
//
// case VisionDemo:
// playSound(default_path + "Vision processing mode.mp3");
// break;
//
// case ActionDemo:
// playSound(default_path + "Interactive motion mode.mp3");
// break;
//
// default:
// break;
// }
}
}
//execute pending callbacks
ros::spinOnce();
@ -257,7 +235,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
break;
}
ROS_INFO("= Start Demo Mode : %d", desired_status);
ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
}
else if (msg->data == "mode")
{
@ -265,7 +243,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
desired_status = (desired_status + 1) % DemoCount;
desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status;
// sound out desired status and changign LED
// sound out desired status and changing LED
switch (desired_status)
{
case SoccerDemo:
@ -287,7 +265,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
break;
}
ROS_INFO("= Demo Mode : %d", desired_status);
ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status);
}
}
}

33
op3_demo/src/soccer/ball_follower.cpp
View File

@ -60,7 +60,7 @@ BallFollower::BallFollower()
current_tilt_(-10),
current_x_move_(0.005),
current_r_angle_(0),
debug_print_(false)
DEBUG_PRINT(false)
{
current_joint_states_sub_ = nh_.subscribe("/robotis/goal_joint_states", 10, &BallFollower::currentJointStatesCallback,
this);
@ -157,10 +157,10 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
return false;
}
ROS_INFO_COND(debug_print_, " ============== Head | Ball ============== ");
ROS_INFO_STREAM_COND(debug_print_,
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_,
ROS_INFO_STREAM_COND(DEBUG_PRINT,
"== Head Tilt : " << (current_tilt_ * 180 / M_PI) << " | Ball Y : " << (y_angle * 180 / M_PI));
approach_ball_position_ = NotFound;
@ -179,20 +179,17 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
// check whether ball is correct position.
if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0))
//if ((ball_y_angle < -65) && (fabs(current_pan_) < 25))
{
count_to_kick_ += 1;
ROS_INFO_STREAM_COND(debug_print_,
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_,
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 : " << accum_ball_position_);
ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << accum_ball_position_);
ROS_INFO("In range [%d]", count_to_kick_);
//if (fabs(x_angle) < 10 * M_PI / 180)
//{
if (count_to_kick_ > 20)
{
setWalkingCommand("stop");
@ -201,12 +198,12 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
// check direction of the ball
if (accum_ball_position_ > 0)
{
ROS_INFO("Ready to kick : left"); // left
ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left
approach_ball_position_ = OnLeft;
}
else
{
ROS_INFO("Ready to kick : right"); // right
ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : right"); // right
approach_ball_position_ = OnRight;
}
@ -215,7 +212,6 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
else if (count_to_kick_ > 15)
{
if (ball_x_angle > 0)
//if( current_pan_ > 0)
accum_ball_position_ += 1;
else
accum_ball_position_ -= 1;
@ -225,7 +221,6 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
return false;
}
//}
}
else
{
@ -245,15 +240,15 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
fb_move = fmax(fb_goal, MIN_FB_STEP);
}
ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move, delta_time);
ROS_INFO("==============================================");
ROS_INFO_COND(DEBUG_PRINT, "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move,
delta_time);
ROS_INFO_COND(DEBUG_PRINT, "==============================================");
// calc rl angle
double rl_goal, rl_angle;
if (fabs(current_pan_) * 180 / M_PI > 5.0)
{
double rl_offset = fabs(current_pan_) * 0.3;
//double rl_offset = fabs(ball_x_angle) * 0.3;
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);
@ -289,7 +284,7 @@ void BallFollower::setWalkingCommand(const std::string &command)
_command_msg.data = command;
set_walking_command_pub_.publish(_command_msg);
ROS_INFO_STREAM("Send Walking command : " << command);
ROS_INFO_STREAM_COND(DEBUG_PRINT, "Send Walking command : " << command);
}
void BallFollower::setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance)
@ -314,7 +309,7 @@ bool BallFollower::getWalkingParam()
current_walking_param_ = walking_param_msg.response.parameters;
// update ui
ROS_INFO("Get walking parameters");
ROS_INFO_COND(DEBUG_PRINT, "Get walking parameters");
return true;
}

74
op3_demo/src/soccer/ball_tracker.cpp
View File

@ -37,15 +37,12 @@ namespace robotis_op
BallTracker::BallTracker()
: nh_(ros::this_node::getName()),
//FOV_WIDTH(35.2 * M_PI / 180),
FOV_WIDTH(26.4 * M_PI / 180),
FOV_HEIGHT(21.6 * M_PI / 180),
NOT_FOUND_THRESHOLD(50),
use_head_scan_(true),
count_not_found_(0),
on_tracking_(false),
current_head_pan_(-10),
current_head_tilt_(-10),
current_ball_pan_(0),
current_ball_tilt_(0),
current_ball_bottom_(0),
@ -57,10 +54,6 @@ BallTracker::BallTracker()
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);
// todo : remove
current_joint_states_sub_ = nh_.subscribe("/robotis/goal_joint_states", 10, &BallTracker::currentJointStatesCallback,
this);
}
BallTracker::~BallTracker()
@ -101,13 +94,13 @@ void BallTracker::ballTrackerCommandCallback(const std_msgs::String::ConstPtr &m
void BallTracker::startTracking()
{
on_tracking_ = true;
ROS_INFO("Start Ball tracking");
ROS_INFO_COND(DEBUG_PRINT, "Start Ball tracking");
}
void BallTracker::stopTracking()
{
on_tracking_ = false;
ROS_INFO("Stop Ball tracking");
ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking");
}
void BallTracker::setUsingHeadScan(bool use_scan)
@ -115,47 +108,12 @@ void BallTracker::setUsingHeadScan(bool use_scan)
use_head_scan_ = use_scan;
}
// todo : remove
void BallTracker::currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
{
double pan, tilt;
int get_count = 0;
// pan : right (-) , left (+)
// tilt : top (+), bottom (-)
for (int ix = 0; ix < msg->name.size(); ix++)
{
if (msg->name[ix] == "head_pan")
{
// convert direction for internal variable(x / 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
if (current_head_pan_ == -10 || fabs(pan - current_head_pan_) < 5 * M_PI / 180)
current_head_pan_ = pan;
if (current_head_tilt_ == -10 || fabs(tilt - current_head_tilt_) < 5 * M_PI / 180)
current_head_tilt_ = tilt;
}
bool BallTracker::processTracking()
{
if (on_tracking_ == false)
{
ball_position_.z = 0;
count_not_found_ = 0;
current_head_pan_ = -10;
current_head_tilt_ = -10;
return false;
}
@ -183,14 +141,9 @@ bool BallTracker::processTracking()
ball_position_.z = 0;
count_not_found_ = 0;
if (DEBUG_PRINT == true)
{
std::cout << "--------------------------------------------------------------" << std::endl;
std::cout << "Ball position : " << ball_position_.x << " | " << ball_position_.y << std::endl;
std::cout << "Target angle : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI) << std::endl;
std::cout << "Head angle : " << (current_head_pan_ * 180 / M_PI) << " | " << (current_head_tilt_ * 180 / M_PI)
<< std::endl;
}
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_;
@ -203,15 +156,13 @@ bool BallTracker::processTracking()
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;
if (DEBUG_PRINT == true)
{
std::cout << "--------------------------------------------------------------" << std::endl;
std::cout << "error : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI) << std::endl;
std::cout << "error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | "
<< delta_time << std::endl;
std::cout << "error_target : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI)
<< " | P : " << p_gain << " | D : " << d_gain << std::endl;
}
ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------");
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_target : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI) << " | P : " << p_gain << " | D : " << d_gain);
// move head joint
publishHeadJoint(x_error_target, y_error_target);
@ -254,7 +205,6 @@ void BallTracker::scanBall()
scan_msg.data = "scan";
head_scan_pub_.publish(scan_msg);
// ROS_INFO("Scan the ball");
}
}

30
op3_demo/src/soccer/soccer_demo.cpp
View File

@ -36,11 +36,10 @@ namespace robotis_op
{
SoccerDemo::SoccerDemo()
: FALLEN_FORWARD_LIMIT(60),
FALLEN_BEHIND_LIMIT(-60),
: FALL_FORWARD_LIMIT(60),
FALL_BACK_LIMIT(-60),
SPIN_RATE(30),
debug_code_(false),
//enable_(false),
DEBUG_PRINT(false),
wait_count_(0),
on_following_ball_(false),
restart_soccer_(false),
@ -74,12 +73,6 @@ void SoccerDemo::setDemoEnable()
enable_ = true;
startSoccerMode();
// handle enable procedure
// ball_tracker_.startTracking();
// ball_follower_.startFollowing();
// wait_count_ = 1 * SPIN_RATE;
}
void SoccerDemo::setDemoDisable()
@ -113,13 +106,13 @@ void SoccerDemo::process()
ball_follower_.startFollowing();
start_following_ = false;
wait_count_ = 1 * SPIN_RATE; // wait 1 sec
wait_count_ = 1 * SPIN_RATE;
}
// check to stop
if (stop_following_ == true)
{
//ball_tracker_.stopTracking();
ball_tracker_.stopTracking();
ball_follower_.stopFollowing();
stop_following_ = false;
@ -386,7 +379,7 @@ void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
Eigen::MatrixXd rpy_orientation = robotis_framework::convertQuaternionToRPY(orientation);
rpy_orientation *= (180 / M_PI);
ROS_INFO_COND(debug_code_, "Roll : %3.2f, Pitch : %2.2f", rpy_orientation.coeff(0, 0), rpy_orientation.coeff(1, 0));
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);
@ -396,9 +389,9 @@ void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
else
present_pitch_ = present_pitch_ * (1 - alpha) + pitch * alpha;
if (present_pitch_ > FALLEN_FORWARD_LIMIT)
if (present_pitch_ > FALL_FORWARD_LIMIT)
stand_state_ = Fallen_Forward;
else if (present_pitch_ < FALLEN_BEHIND_LIMIT)
else if (present_pitch_ < FALL_BACK_LIMIT)
stand_state_ = Fallen_Behind;
else
stand_state_ = Stand;
@ -427,7 +420,8 @@ void SoccerDemo::handleKick(int ball_position)
usleep(1000 * 1000);
// change to motion module
setModuleToDemo("action_module");
//setModuleToDemo("action_module");
setBodyModuleToDemo("action_module");
usleep(1500 * 1000);
@ -451,7 +445,6 @@ void SoccerDemo::handleKick(int ball_position)
break;
}
// todo : remove this in order to play soccer repeatedly
on_following_ball_ = false;
usleep(2000 * 1000);
@ -460,10 +453,9 @@ void SoccerDemo::handleKick(int ball_position)
return;
// ceremony
//std::cout << "Go Ceremony!!!" << std::endl;
//playMotion(Ceremony);
//restart_soccer_ = true;
restart_soccer_ = true;
}
bool SoccerDemo::handleFallen(int fallen_status)

20
op3_demo/src/soccer_demo_node.cpp
View File

@ -41,10 +41,12 @@
enum Motion_Index
{
GetUpFront = 81,
GetUpBack = 82,
RightKick = 83,
LeftKick = 84,
InitPose = 1,
WalkingReady = 9,
GetUpFront = 122,
GetUpBack = 123,
RightKick = 121,
LeftKick = 120,
Ceremony = 85,
};
@ -64,8 +66,8 @@ enum Robot_Status
ReadyToGetup = 4,
};
const double FALLEN_FORWARD_LIMIT = 60;
const double FALLEN_BEHIND_LIMIT = -60;
const double FALL_FORWARD_LIMIT = 60;
const double FALL_BACK_LIMIT = -60;
const int SPIN_RATE = 30;
void callbackThread();
@ -386,9 +388,9 @@ void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
else
present_pitch = present_pitch * 0.5 + pitch * 0.5;
if (present_pitch > FALLEN_FORWARD_LIMIT)
if (present_pitch > FALL_FORWARD_LIMIT)
stand_state = Fallen_Forward;
else if (present_pitch < FALLEN_BEHIND_LIMIT)
else if (present_pitch < FALL_BACK_LIMIT)
stand_state = Fallen_Behind;
else
stand_state = Stand;
@ -417,7 +419,6 @@ void handleKick(int ball_position)
usleep(500 * 1000);
// change to motion module
// setBodyModuleToDemo("action_module");
setModuleToDemo("action_module");
usleep(1000 * 1000);
@ -489,7 +490,6 @@ bool handleFallen(int fallen_status)
restart_soccer = true;
// reset state
//stand_state = Stand;
on_following_ball = false;
return true;

91
op3_demo/src/test/button_test.cpp
View File

@ -39,8 +39,6 @@ ButtonTest::ButtonTest()
: SPIN_RATE(30),
led_count_(0),
rgb_led_count_(0)
// is_tracking_(false),
// tracking_status_(FaceTracker::Waiting)
{
enable_ = false;
@ -54,68 +52,23 @@ ButtonTest::ButtonTest()
ButtonTest::~ButtonTest()
{
// TODO Auto-generated destructor stub
}
void ButtonTest::setDemoEnable()
{
// change to motion module
// setModuleToDemo("action_module");
// usleep(100 * 1000);
// playMotion(InitPose);
// usleep(1500 * 1000);
// setModuleToDemo("head_control_module");
// usleep(10 * 1000);
enable_ = true;
// face_tracker_.startTracking();
ROS_INFO("Start Button Test");
}
void ButtonTest::setDemoDisable()
{
// face_tracker_.stopTracking();
// is_tracking_ = false;
// tracking_status_ = FaceTracker::Waiting;
enable_ = false;
}
void ButtonTest::process()
{
//bool is_tracked = face_tracker_.processTracking();
// int tracking_status = face_tracker_.processTracking();
//if(is_tracking_ != is_tracked)
// if(tracking_status_ != tracking_status)
// {
// switch(tracking_status)
// {
// case FaceTracker::Found:
// setRGBLED(0x1F, 0x1F, 0x1F);
// break;
//
// case FaceTracker::NotFound:
// setRGBLED(0, 0, 0);
// break;
//
// default:
// break;
// }
// }
// if(tracking_status != FaceTracker::Waiting)
// tracking_status_ = tracking_status;
//is_tracking_ = is_tracked;
// std::cout << "Tracking : " << tracking_status << std::endl;
}
void ButtonTest::processThread()
@ -139,13 +92,10 @@ void ButtonTest::callbackThread()
ros::NodeHandle nh(ros::this_node::getName());
// subscriber & publisher
// module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
// motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
play_sound_pub_ = nh.advertise<std_msgs::String>("/play_sound_file", 0);
buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ButtonTest::buttonHandlerCallback, this);
// faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &ButtonTest::facePositionCallback, this);
while (nh.ok())
{
@ -194,47 +144,6 @@ void ButtonTest::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
}
}
//void ButtonTest::setModuleToDemo(const std::string &module_name)
//{
// std_msgs::String control_msg;
// control_msg.data = module_name;
//
// module_control_pub_.publish(control_msg);
// std::cout << "enable module : " << module_name << std::endl;
//}
//
//void ButtonTest::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 ButtonTest::playMotion(int motion_index)
//{
// std_msgs::Int32 motion_msg;
// motion_msg.data = motion_index;
//
// motion_index_pub_.publish(motion_msg);
//}
void ButtonTest::setRGBLED(int blue, int green, int red)
{
int led_full_unit = 0x1F;

9
op3_demo/src/test/mic_test.cpp
View File

@ -58,7 +58,6 @@ MicTest::MicTest()
MicTest::~MicTest()
{
// TODO Auto-generated destructor stub
}
void MicTest::setDemoEnable()
@ -68,7 +67,6 @@ void MicTest::setDemoEnable()
enable_ = true;
ROS_INFO("Start Mic test Demo");
}
void MicTest::setDemoDisable()
@ -185,15 +183,12 @@ void MicTest::announceTest()
// play mic test sound
playSound(default_mp3_path_ + "Announce mic test.mp3");
//startTimer(3.0);
usleep(3.4 * 1000 * 1000);
}
void MicTest::recordSound(int recording_time)
{
ROS_INFO("Start to record");
// arecord -D plughw:1,0 -f S16_LE -c1 -r22050 -t raw -d 5 | lame -r -s 22.05 -m m -b 64 - mic-input.mp3
// arecord -D plughw:1,0 -f S16_LE -c1 -r22050 -t wav -d 5 test.wav
playSound(default_mp3_path_ + "Start recording.mp3");
@ -209,8 +204,6 @@ void MicTest::recordSound(int recording_time)
case -1:
fprintf(stderr, "Fork Failed!! \n");
ROS_WARN("Fork Failed!! \n");
//done_msg.data = "play_sound_fail";
//g_done_msg_pub.publish(done_msg);
break;
case 0:
@ -283,12 +276,10 @@ void MicTest::startTimer(double wait_time)
{
start_time_ = ros::Time::now();
wait_time_ = wait_time;
//is_wait_ = true;
}
void MicTest::finishTimer()
{
//is_wait_ = false;
wait_time_ = -1;
}

24
op3_demo/src/test_node.cpp
View File

@ -201,30 +201,6 @@ int main(int argc, char **argv)
apply_desired = false;
current_status = desired_status;
}
else
{
if (current_status != desired_status)
{
// // sound out desired status
// switch (desired_status)
// {
// case SoccerDemo:
// playSound(default_path + "Autonomous soccer mode.mp3");
// break;
//
// case VisionDemo:
// playSound(default_path + "Vision processing mode.mp3");
// break;
//
// case ActionDemo:
// playSound(default_path + "Interactive motion mode.mp3");
// break;
//
// default:
// break;
// }
}
}
//execute pending callbacks
ros::spinOnce();